crypto: hisilicon/qm - restore original qos values

[ Upstream commit e7066160f5b4187ad9869b712fa7a35d3d5be6b9 ]

When the new qos valus setting fails, restore to
the original qos values.

Fixes: 72b010dc33b9 ("crypto: hisilicon/qm - supports writing QoS int the host")
Signed-off-by: nieweiqiang <nieweiqiang@huawei.com>
Signed-off-by: Chenghai Huang <huangchenghai2@huawei.com>
Signed-off-by: Herbert Xu <herbert@gondor.apana.org.au>
Signed-off-by: Sasha Levin <sashal@kernel.org>

diff --git a/drivers/crypto/hisilicon/qm.c b/drivers/crypto/hisilicon/qm.c
index 3b391a1466353d466bb129d11f5924a5ccc8646f..0968304c0cb51030bdbef6afae8dd1490baed297 100644 (file)
--- a/drivers/crypto/hisilicon/qm.c
+++ b/drivers/crypto/hisilicon/qm.c
@@ -3678,6 +3678,7 @@ static void qm_clear_vft_config(struct hisi_qm *qm)
 static int qm_func_shaper_enable(struct hisi_qm *qm, u32 fun_index, u32 qos)
 {
        struct device *dev = &qm->pdev->dev;
+       struct qm_shaper_factor t_factor;
        u32 ir = qos * QM_QOS_RATE;
        int ret, total_vfs, i;
 
@@ -3685,6 +3686,7 @@ static int qm_func_shaper_enable(struct hisi_qm *qm, u32 fun_index, u32 qos)
        if (fun_index > total_vfs)
                return -EINVAL;
 
+       memcpy(&t_factor, &qm->factor[fun_index], sizeof(t_factor));
        qm->factor[fun_index].func_qos = qos;
 
        ret = qm_get_shaper_para(ir, &qm->factor[fun_index]);
@@ -3698,11 +3700,21 @@ static int qm_func_shaper_enable(struct hisi_qm *qm, u32 fun_index, u32 qos)
                ret = qm_set_vft_common(qm, SHAPER_VFT, fun_index, i, 1);
                if (ret) {
                        dev_err(dev, "type: %d, failed to set shaper vft!\n", i);
-                       return -EINVAL;
+                       goto back_func_qos;
                }
        }
 
        return 0;
+
+back_func_qos:
+       memcpy(&qm->factor[fun_index], &t_factor, sizeof(t_factor));
+       for (i--; i >= ALG_TYPE_0; i--) {
+               ret = qm_set_vft_common(qm, SHAPER_VFT, fun_index, i, 1);
+               if (ret)
+                       dev_err(dev, "failed to restore shaper vft during rollback!\n");
+       }
+
+       return -EINVAL;
 }
 
 static u32 qm_get_shaper_vft_qos(struct hisi_qm *qm, u32 fun_index)