From 8dca488432f85debad73710c88caa52f98c4be4f Mon Sep 17 00:00:00 2001
From: Gabriel Somlo <gsomlo@gmail.com>
Date: Wed, 21 Jun 2023 15:20:42 -0400
Subject: [PATCH] software/bios/cmd_litesata: add multisector read/write test

---
 litex/soc/software/bios/cmds/cmd_litesata.c | 140 ++++++++++++++++++++
 1 file changed, 140 insertions(+)

diff --git a/litex/soc/software/bios/cmds/cmd_litesata.c b/litex/soc/software/bios/cmds/cmd_litesata.c
index 83b0c3728..10511c203 100644
--- a/litex/soc/software/bios/cmds/cmd_litesata.c
+++ b/litex/soc/software/bios/cmds/cmd_litesata.c
@@ -176,3 +176,143 @@ static void sata_mem2sec_handler(int nb_params, char **params)
 
 define_command(sata_mem2sec, sata_mem2sec_handler, "Write SATA from memory", LITESATA_CMDS);
 #endif
+
+/* LiteSATA read/write test */
+#if defined(CSR_SATA_SECTOR2MEM_BASE) && defined(CSR_SATA_MEM2SECTOR_BASE)
+#include <string.h>
+static int sata_rd(uint32_t sector, uint32_t count, void *mem)
+{
+	uint32_t done_cnt;
+	uint8_t retry_cnt;
+
+	for (retry_cnt = 8; retry_cnt > 0; retry_cnt--) {
+		sata_sector2mem_base_write((uint64_t)(uintptr_t)mem);
+		sata_sector2mem_sector_write(sector);
+		sata_sector2mem_nsectors_write(count);
+		sata_sector2mem_start_write(1);
+		for (done_cnt = 0x0000ffff; done_cnt > 0; done_cnt --) {
+			if ((sata_sector2mem_done_read() & 0x1) != 0) {
+				if ((sata_sector2mem_error_read() & 0x1) == 0)
+					return 0;
+				else {
+					printf("sata_rd: op failed, retry\n");
+					break;
+				}
+			}
+		}
+		printf("sata_rd: op timeout (done_cnt)\n");
+		busy_wait_us(10);
+	}
+	printf("sata_rd: out of retries\n");
+	return -1;
+}
+
+static int sata_rd_1(uint32_t sector, uint32_t count, void *mem)
+{
+	while(count--) {
+		sata_read(sector++, 1, mem);
+		mem += 512;
+	}
+	return 0;
+}
+
+static int sata_wr(uint32_t sector, uint32_t count, void *mem)
+{
+	uint32_t done_cnt;
+	uint8_t retry_cnt;
+
+	for (retry_cnt = 8; retry_cnt > 0; retry_cnt--) {
+		sata_mem2sector_base_write((uint64_t)(uintptr_t)mem);
+		sata_mem2sector_sector_write(sector);
+		sata_mem2sector_nsectors_write(count);
+		sata_mem2sector_start_write(1);
+		for (done_cnt = 0x000fffff; done_cnt > 0; done_cnt --) {
+			if ((sata_mem2sector_done_read() & 0x1) != 0) {
+				if ((sata_mem2sector_error_read() & 0x1) == 0)
+					return 0;
+				else {
+					printf("sata_wr: op failed, retry\n");
+					break;
+				}
+			}
+		}
+		printf("sata_wr: op timeout (done_cnt)\n");
+		busy_wait_us(10);
+	}
+	printf("sata_wr: out of retries\n");
+	return -1;
+}
+
+static int sata_mem_cmp(char *mem1, char *mem2, uint32_t count)
+{
+	uint32_t i, j;
+
+	for (i = 0; i < count; i++)
+		for (j = 0; j < 512; j++)
+			if (mem1[512*i + j] != mem2[512*i + j]) {
+				printf("sata_mem_cmp: mismatch in sector %d "
+					"byte %d: %d != %d\n",
+					i, j, mem1[512*i + j], mem2[512*i + j]);
+				return -1;
+			}
+	return 0;
+}
+
+static int sata_do_rwtest(uint32_t sec, uint32_t cnt, char *mem, char *str)
+{
+	char *c = str;
+	int i;
+
+	if (c != NULL) {
+		for (i = 0; i < 512 * cnt; i++) {
+			mem[i] = *c++;
+			if (*c == 0)
+				c = str;
+		}
+	}
+	if (sata_wr(sec, cnt, mem) < 0)
+		return -1;
+	if (sata_rd(sec, cnt, mem + 512*cnt) < 0)
+		return -1;
+	if (sata_mem_cmp(mem, mem + 512*cnt, cnt) == 0)
+		return 0;
+	printf("compare failed, retrying with single-sector reads:\n");
+	return sata_rd_1(sec, cnt, mem + 512*cnt);
+}
+
+static void sata_rwtest_handler(int nb_params, char **params)
+{
+	unsigned int sec, cnt;
+	char *mem;
+	char *c;
+
+	if (nb_params < 4) {
+		printf("sata_rwtest <sector> <address> <count> <str>");
+		return;
+	}
+
+	sec = strtoul(params[0], &c, 0);
+	if (*c != 0) {
+		printf("incorrect sector");
+		return;
+	}
+
+	mem = (char *)strtoul(params[1], &c, 0);
+	if (*c != 0) {
+		printf("incorrect address");
+		return;
+	}
+
+	cnt = strtoul(params[2], &c, 0);
+	if (*c != 0) {
+		printf("incorrect count");
+		return;
+	}
+
+	if (sata_do_rwtest(sec, cnt, mem, params[3]))
+		printf("Failure.");
+	else
+		printf("Success.");
+}
+define_command(sata_rwtest, sata_rwtest_handler, "SATA read/write test", LITESATA_CMDS);
+#endif