[SC64][SW] Implement correct behavior for FlashRAM writes

Polprzewodnikowy · Jan 10, 2024 · 13fc38f · 13fc38f
1 parent 2639a93
commit 13fc38f
Showing 1 changed file with 10 additions and 4 deletions.
diff --git a/sw/controller/src/flashram.c b/sw/controller/src/flashram.c
@@ -43,7 +43,8 @@ void flashram_init (void) {
 void flashram_process (void) {
     uint32_t scr = fpga_reg_get(REG_FLASHRAM_SCR);
     enum operation op = flashram_operation_type(scr);
-    uint8_t buffer[FLASHRAM_PAGE_SIZE];
+    uint8_t read_buffer[FLASHRAM_PAGE_SIZE];
+    uint8_t write_buffer[FLASHRAM_PAGE_SIZE];
     uint32_t address = FLASHRAM_ADDRESS;
     uint32_t erase_size = (op == OP_ERASE_SECTOR) ? FLASHRAM_SECTOR_SIZE : FLASHRAM_SIZE;
     uint32_t page = (op != OP_ERASE_ALL) ? ((scr & FLASHRAM_SCR_PAGE_MASK) >> FLASHRAM_SCR_PAGE_BIT) : 0;
@@ -53,16 +54,21 @@ void flashram_process (void) {
         case OP_ERASE_ALL:
         case OP_ERASE_SECTOR:
             for (int i = 0; i < FLASHRAM_PAGE_SIZE; i++) {
-                buffer[i] = 0xFF;
+                write_buffer[i] = 0xFF;
             }
             for (int i = 0; i < erase_size; i += FLASHRAM_PAGE_SIZE) {
-                fpga_mem_write(address + i, FLASHRAM_PAGE_SIZE, buffer);
+                fpga_mem_write(address + i, FLASHRAM_PAGE_SIZE, write_buffer);
             }
             fpga_reg_set(REG_FLASHRAM_SCR, FLASHRAM_SCR_DONE);
             break;
 
         case OP_WRITE_PAGE:
-            fpga_mem_copy(FLASHRAM_BUFFER_ADDRESS, address, FLASHRAM_PAGE_SIZE);
+            fpga_mem_read(FLASHRAM_BUFFER_ADDRESS, FLASHRAM_PAGE_SIZE, read_buffer);
+            fpga_mem_read(address, FLASHRAM_PAGE_SIZE, write_buffer);
+            for (int i = 0; i < FLASHRAM_PAGE_SIZE; i++) {
+                write_buffer[i] &= read_buffer[i];
+            }
+            fpga_mem_write(address, FLASHRAM_PAGE_SIZE, write_buffer);
             fpga_reg_set(REG_FLASHRAM_SCR, FLASHRAM_SCR_DONE);
             break;