MLK-17591-1: CSU Fix Slave configuration
there is a bug in the CSL assignment.
the CSL(n) and CSL(n+1) configuration were reversed.
Signed-off-by: Silvano di Ninno <silvano.dininno@nxp.com>
Reviewed-by: Peng Fan <peng.fan@nxp.com>
(cherry picked from commit f8b639ddea3be778b006d1a5397d21e2ca6ae7c8)
diff --git a/plat/imx/imx8mq/imx_csu.c b/plat/imx/imx8mq/imx_csu.c
index 194644c..3362908 100644
--- a/plat/imx/imx8mq/imx_csu.c
+++ b/plat/imx/imx8mq/imx_csu.c
@@ -151,18 +151,22 @@
NOTICE("CSU CSLn(%d) already locked with mode:0x%x\n", index, read_mode);
return;
}
- if (read_mode == mode && lock == 0) {
+ if (read_mode == mode) {
NOTICE("CSU CSLn(%d) mode 0x%x already written\n", index, read_mode);
return;
}
reg = (uintptr_t)(IMX_CSU_BASE + (index / 2) * 4);
tmp = mmio_read_32(reg);
- if (index % 2 == 0) {
+
+ if (lock)
+ mode |= 1 << 8;
+
+ if (index % 2) {
tmp &= 0x0000ffff;
- tmp |= mode << 16 | lock << 24;
+ tmp |= mode << 16;
} else {
tmp &= 0xffff0000;
- tmp |= mode | lock << 8;
+ tmp |= mode;
}
mmio_write_32(reg, tmp);
}
@@ -175,13 +179,13 @@
reg = (uintptr_t)(IMX_CSU_BASE + (index / 2) * 4);
tmp = mmio_read_32(reg);
- if (index % 2 == 0) {
- *mode = (uint16_t)(tmp >> 16 & 0xff);
- *lock = (uint8_t)(tmp >> 24 & 0x01);
- } else {
- *mode = (uint16_t)(tmp & 0xff);
- *lock = (uint8_t)(tmp >> 8 & 0x01);
- }
+ if (index % 2)
+ tmp = tmp >> 16;
+
+ tmp &= 0x1ff;
+
+ *mode = tmp & 0xff;
+ *lock = tmp >> 8;
}
void csu_set_slaves_modes(struct csu_slave_conf *csu_config, uint32_t count)
