drivers: marvell: Fix the LLC SRAM driver

- Fix the line address macro
- LLC invalidate and enable before ways lock for allocation
- Add support for limited SRAM size allocation
- Add SRAM RW test function

Change-Id: I1867ece3047566ddd7931bd7472e1f47fb42c8d4
Signed-off-by: Konstantin Porotchkin <kostap@marvell.com>

2 files changed, 49 insertions, 8 deletions

diff --git a/drivers/marvell/cache_llc.c b/drivers/marvell/cache_llc.c
index 836aae7b8..4b06b4752 100644
--- a/drivers/marvell/cache_llc.c
+++ b/drivers/marvell/cache_llc.c
@@ -111,28 +111,36 @@ void llc_runtime_enable(int ap_index)
 }
 
 #if LLC_SRAM
-void llc_sram_enable(int ap_index)
+int llc_sram_enable(int ap_index, int size)
 {
-	uint32_t tc, way;
+	uint32_t tc, way, ways_to_allocate;
 	uint32_t way_addr;
 
+	if ((size <= 0) || (size > LLC_SIZE) || (size % LLC_WAY_SIZE))
+		return -1;
+
+	llc_enable(ap_index, 1);
+	llc_inv_all(ap_index);
+
+	ways_to_allocate = size / LLC_WAY_SIZE;
+
 	/* Lockdown all available ways for all traffic classes */
 	for (tc = 0; tc < LLC_TC_NUM; tc++)
-		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), LLC_WAY_MASK);
+		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), LLC_ALL_WAYS_MASK);
 
 	/* Clear the high bits of SRAM address */
 	mmio_write_32(LLC_BANKED_MNT_AHR(ap_index), 0);
 
 	way_addr = PLAT_MARVELL_TRUSTED_RAM_BASE;
-	for (way = 0; way < LLC_WAYS; way++) {
+	for (way = 0; way < ways_to_allocate; way++) {
 		/* Trigger allocation block command */
 		mmio_write_32(LLC_BLK_ALOC(ap_index),
 			      LLC_BLK_ALOC_BASE_ADDR(way_addr) |
-			      LLC_BLK_ALOC_WAY_DATA_CLR |
+			      LLC_BLK_ALOC_WAY_DATA_SET |
 			      LLC_BLK_ALOC_WAY_ID(way));
 		way_addr += LLC_WAY_SIZE;
 	}
-	llc_enable(ap_index, 1);
+	return 0;
 }
 
 void llc_sram_disable(int ap_index)
@@ -146,4 +154,36 @@ void llc_sram_disable(int ap_index)
 	/* Invalidate all ways */
 	llc_inv_all(ap_index);
 }
+
+int llc_sram_test(int ap_index, int size, char *msg)
+{
+	uintptr_t addr, end_addr;
+	uint32_t data = 0;
+
+	if ((size <= 0) || (size > LLC_SIZE))
+		return -1;
+
+	INFO("=== LLC SRAM WRITE test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		mmio_write_32(addr, addr);
+	}
+	INFO("=== LLC SRAM WRITE test %s PASSED\n", msg);
+	INFO("=== LLC SRAM READ test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		data = mmio_read_32(addr);
+		if (data != addr) {
+			INFO("=== LLC SRAM READ test %s FAILED @ 0x%08lx)\n",
+			     msg, addr);
+			return -1;
+		}
+	}
+	INFO("=== LLC SRAM READ test %s PASSED (last read = 0x%08x)\n",
+	     msg, data);
+	return 0;
+}
+
 #endif /* LLC_SRAM */
diff --git a/include/drivers/marvell/cache_llc.h b/include/drivers/marvell/cache_llc.h
index b326474ee..d6dd65382 100644
--- a/include/drivers/marvell/cache_llc.h
+++ b/include/drivers/marvell/cache_llc.h
@@ -41,7 +41,7 @@
 #define LLC_BLK_ALOC_WAY_DATA_DSBL	(0x0 << 6)
 #define LLC_BLK_ALOC_WAY_DATA_CLR	(0x1 << 6)
 #define LLC_BLK_ALOC_WAY_DATA_SET	(0x3 << 6)
-#define LLC_BLK_ALOC_BASE_ADDR(addr)	((addr) & (LLC_WAY_SIZE - 1))
+#define LLC_BLK_ALOC_BASE_ADDR(addr)	((addr) & ~(LLC_WAY_SIZE - 1))
 
 #ifndef __ASSEMBLER__
 void llc_cache_sync(int ap_index);
@@ -53,8 +53,9 @@ void llc_enable(int ap_index, int excl_mode);
 int llc_is_exclusive(int ap_index);
 void llc_runtime_enable(int ap_index);
 #if LLC_SRAM
-void llc_sram_enable(int ap_index);
+int llc_sram_enable(int ap_index, int size);
 void llc_sram_disable(int ap_index);
+int llc_sram_test(int ap_index, int size, char *msg);
 #endif /* LLC_SRAM */
 #endif /* __ASSEMBLY__ */