From 2cd183c2d3d319af0586b72357ab6a3e411ffe24 Mon Sep 17 00:00:00 2001
From: David Garske <david@wolfssl.com>
Date: Fri, 23 Aug 2024 15:44:30 -0700
Subject: [PATCH] Fixes for QSPI driver.

---
 hal/zynq.c                     | 70 ++++++++++++++++++----------------
 hal/zynq.h                     |  6 ++-
 src/boot_aarch64_translation.S |  2 -
 src/update_ram.c               |  4 +-
 4 files changed, 43 insertions(+), 39 deletions(-)

diff --git a/hal/zynq.c b/hal/zynq.c
index 702237608..78f1a5c51 100644
--- a/hal/zynq.c
+++ b/hal/zynq.c
@@ -786,7 +786,8 @@ void qspi_init(uint32_t cpu_clock, uint32_t flash_freq)
     /* Clear and disable interrupts */
     reg_cfg = GQSPI_ISR;
     GQSPI_ISR |= GQSPI_ISR_WR_TO_CLR_MASK; /* Clear poll timeout counter interrupt */
-    QSPIDMA_DST_I_STS = QSPIDMA_DST_I_STS; /* clear all active interrupts */
+    reg_cfg = QSPIDMA_DST_I_STS;
+    QSPIDMA_DST_I_STS = reg_cfg; /* clear all active interrupts */
     QSPIDMA_DST_STS |= QSPIDMA_DST_STS_WTC; /* mark outstanding DMA's done */
     GQSPI_IDR = GQSPI_IXR_ALL_MASK; /* disable interrupts */
     QSPIDMA_DST_I_STS = QSPIDMA_DST_I_STS_ALL_MASK; /* disable interrupts */
@@ -948,46 +949,49 @@ int RAMFUNCTION ext_flash_write(uintptr_t address, const uint8_t *data, int len)
 {
     int ret = 0;
     uint8_t cmd[8]; /* size multiple of uint32_t */
-    uint32_t xferSz, page, pages, idx = 0;
+    uint32_t xferSz, page, pages, idx;
     uintptr_t addr;
 
     /* write by page */
     pages = ((len + (FLASH_PAGE_SIZE-1)) / FLASH_PAGE_SIZE);
     for (page = 0; page < pages; page++) {
         ret = qspi_write_enable(&mDev);
-        if (ret == GQSPI_CODE_SUCCESS) {
-            xferSz = len;
-            if (xferSz > FLASH_PAGE_SIZE)
-                xferSz = FLASH_PAGE_SIZE;
-
-            addr = address + (page * FLASH_PAGE_SIZE);
-            if (mDev.stripe) {
-                /* For dual parallel the address divide by 2 */
-                addr /= 2;
-            }
+        if (ret != GQSPI_CODE_SUCCESS) {
+            break;
+        }
+        xferSz = len;
+        if (xferSz > FLASH_PAGE_SIZE)
+            xferSz = FLASH_PAGE_SIZE;
+
+        addr = address + (page * FLASH_PAGE_SIZE);
+        if (mDev.stripe) {
+            /* For dual parallel the address divide by 2 */
+            addr /= 2;
+        }
 
-            /* ------ Write Flash (page at a time) ------ */
-            memset(cmd, 0, sizeof(cmd));
-            cmd[idx++] = PAGE_PROG_CMD;
-        #if GQPI_USE_4BYTE_ADDR == 1
-            cmd[idx++] = ((addr >> 24) & 0xFF);
-        #endif
-            cmd[idx++] = ((addr >> 16) & 0xFF);
-            cmd[idx++] = ((addr >> 8)  & 0xFF);
-            cmd[idx++] = ((addr >> 0)  & 0xFF);
-            ret = qspi_transfer(&mDev, cmd, idx,
-                (const uint8_t*)(data + (page * FLASH_PAGE_SIZE)),
-                xferSz, NULL, 0, 0, GQSPI_GEN_FIFO_MODE_SPI);
-            wolfBoot_printf("Flash Page %d Write: Ret %d\n", page, ret);
-            if (ret != GQSPI_CODE_SUCCESS)
-                break;
+        /* ------ Write Flash (page at a time) ------ */
+        memset(cmd, 0, sizeof(cmd));
+        idx = 0;
+        cmd[idx++] = PAGE_PROG_CMD;
+    #if GQPI_USE_4BYTE_ADDR == 1
+        cmd[idx++] = ((addr >> 24) & 0xFF);
+    #endif
+        cmd[idx++] = ((addr >> 16) & 0xFF);
+        cmd[idx++] = ((addr >> 8)  & 0xFF);
+        cmd[idx++] = ((addr >> 0)  & 0xFF);
+        ret = qspi_transfer(&mDev, cmd, idx,
+            (const uint8_t*)(data + (page * FLASH_PAGE_SIZE)),
+            xferSz, NULL, 0, 0, GQSPI_GEN_FIFO_MODE_SPI);
+        wolfBoot_printf("Flash Page %d Write: Ret %d\n", page, ret);
+        if (ret != GQSPI_CODE_SUCCESS)
+            break;
 
-            ret = qspi_wait_ready(&mDev); /* Wait for not busy */
-            if (ret != GQSPI_CODE_SUCCESS) {
-                break;
-            }
-            qspi_write_disable(&mDev);
+        ret = qspi_wait_ready(&mDev); /* Wait for not busy */
+        if (ret != GQSPI_CODE_SUCCESS) {
+            break;
         }
+        qspi_write_disable(&mDev);
+        len -= xferSz;
     }
 
     return ret;
@@ -1101,7 +1105,7 @@ static int test_ext_flash(QspiDev_t* dev)
 {
     int ret;
     uint32_t i;
-    uint8_t pageData[FLASH_PAGE_SIZE];
+    uint8_t pageData[FLASH_PAGE_SIZE*4];
 
 #ifndef TEST_FLASH_READONLY
     /* Erase sector */
diff --git a/hal/zynq.h b/hal/zynq.h
index 6e0a7b29f..1e59949f7 100644
--- a/hal/zynq.h
+++ b/hal/zynq.h
@@ -195,9 +195,11 @@
 #endif
 #ifndef FLASH_PAGE_SIZE
     #ifdef ZCU102
-        #define FLASH_PAGE_SIZE 256 /* MT25QU512ABB */
+        /* MT25QU512ABB */
+        #define FLASH_PAGE_SIZE 256
     #else
-        #define FLASH_PAGE_SIZE 512 /* MT25QU01GBBB */
+        /* MT25QU01GBBB */
+        #define FLASH_PAGE_SIZE 512
     #endif
 #endif
 #define FLASH_NUM_SECTORS      (FLASH_DEVICE_SIZE/WOLFBOOT_SECTOR_SIZE)
diff --git a/src/boot_aarch64_translation.S b/src/boot_aarch64_translation.S
index 34e697226..2252f4d4e 100644
--- a/src/boot_aarch64_translation.S
+++ b/src/boot_aarch64_translation.S
@@ -118,7 +118,6 @@ MMUTableL1:
 .endif
 #else
 .set DDR_1_REG, 0
-#warning "There's no DDR_1 in the HW design. MMU translation table marks 32 GB DDR address space as undefined"
 #endif
 
 .set UNDEF_1_REG, 0x20 - DDR_1_REG
@@ -169,7 +168,6 @@ MMUTableL2:
 .endif
 #else
 .set DDR_0_REG, 0
-#warning "There's no DDR_0 in the HW design. MMU translation table marks 2 GB DDR address space as undefined"
 #endif
 
 .set UNDEF_0_REG, 0x400 - DDR_0_REG
diff --git a/src/update_ram.c b/src/update_ram.c
index edd8591fc..9856d6ede 100644
--- a/src/update_ram.c
+++ b/src/update_ram.c
@@ -39,8 +39,8 @@ extern int wolfBoot_get_dts_size(void *dts_addr);
 extern uint32_t kernel_load_addr;
 extern uint32_t dts_load_addr;
 
-#if (defined(EXT_FLASH) && defined(NO_XIP)) || \
-    (defined(EXT_ENCRYPTED) && defined(MMU)) && \
+#if ((defined(EXT_FLASH) && defined(NO_XIP)) || \
+    (defined(EXT_ENCRYPTED) && defined(MMU))) && \
     !defined(WOLFBOOT_NO_RAMBOOT)
     /* Load firmware to RAM on boot (single flash read) */
     #undef  WOLFBOOT_USE_RAMBOOT