[PATCH v1 12/14] arm_mpam: Program Downstream priority value

[Amit Singh Tomar]

Now that Downstream priorities values can be passed from resource control
schemata file, let's program it into memory mapped Priority Partition
Configuration Register.

Signed-off-by: Amit Singh Tomar <amitsinght@xxxxxxxxxxx>
---
Changes since RFC:
		* Priority value is inverted based on DSPRI_0_IS_LOW value.
---
 drivers/platform/mpam/mpam_devices.c | 28 ++++++++++++++++++++++------
 1 file changed, 22 insertions(+), 6 deletions(-)

diff --git a/drivers/platform/mpam/mpam_devices.c b/drivers/platform/mpam/mpam_devices.c
index 9953f2050d35..388d57281fd8 100644
--- a/drivers/platform/mpam/mpam_devices.c
+++ b/drivers/platform/mpam/mpam_devices.c
@@ -1101,6 +1101,7 @@ static void mpam_reprogram_ris_partid(struct mpam_msc_ris *ris, u16 partid,
 	struct mpam_props *rprops = &ris->props;
 	u16 dspri = GENMASK((rprops->dspri_wd - 1), 0);
 	u16 intpri = GENMASK((rprops->intpri_wd - 1), 0);
+	u8 dspri_part_0_low, dspri_rprops, dspri_cfg;
 
 	lockdep_assert_held(&msc->lock);
 
@@ -1143,18 +1144,33 @@ static void mpam_reprogram_ris_partid(struct mpam_msc_ris *ris, u16 partid,
 	if (mpam_has_feature(mpam_feat_ccap_part, rprops))
 		mpam_write_partsel_reg(msc, CMAX, cmax);
 
-	if (mpam_has_feature(mpam_feat_intpri_part, rprops) ||
-	    mpam_has_feature(mpam_feat_dspri_part, rprops)) {
+	if (mpam_has_feature(mpam_feat_intpri_part, rprops)) {
 		/* aces high? */
 		if (!mpam_has_feature(mpam_feat_intpri_part_0_low, rprops))
 			intpri = 0;
-		if (!mpam_has_feature(mpam_feat_dspri_part_0_low, rprops))
-			dspri = 0;
 
 		if (mpam_has_feature(mpam_feat_intpri_part, rprops))
 			pri_val |= FIELD_PREP(MPAMCFG_PRI_INTPRI, intpri);
-		if (mpam_has_feature(mpam_feat_dspri_part, rprops))
-			pri_val |= FIELD_PREP(MPAMCFG_PRI_DSPRI, dspri);
+
+		mpam_write_partsel_reg(msc, PRI, pri_val);
+	}
+
+	dspri_rprops = mpam_has_feature(mpam_feat_dspri_part,
+					rprops);
+	dspri_part_0_low = mpam_has_feature(mpam_feat_dspri_part_0_low,
+						rprops);
+	if (dspri_rprops) {
+		dspri_cfg = mpam_has_feature(mpam_feat_dspri_part,
+						cfg);
+		if (dspri_cfg) {
+			pri_val |= (!dspri_part_0_low)
+			? FIELD_PREP(MPAMCFG_PRI_DSPRI, cfg->dspri ^ dspri)
+			: FIELD_PREP(MPAMCFG_PRI_DSPRI, cfg->dspri & dspri);
+		} else {
+			pri_val |= (!dspri_part_0_low)
+			? FIELD_PREP(MPAMCFG_PRI_DSPRI, 0)
+			: FIELD_PREP(MPAMCFG_PRI_DSPRI, dspri);
+		}
 
 		mpam_write_partsel_reg(msc, PRI, pri_val);
 	}
-- 
2.25.1