From 0d2591e1f1f92ac0542941cb6d40eb1a93384d4e Mon Sep 17 00:00:00 2001
From: Graham Sanderson <graham.sanderson@raspberrypi.com>
Date: Tue, 10 May 2022 13:03:44 -0500
Subject: [PATCH] Pad all but last sector in flash binary to write whole of
 sector (#800)

---
 tools/elf2uf2/main.cpp | 28 +++++++++++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

diff --git a/tools/elf2uf2/main.cpp b/tools/elf2uf2/main.cpp
index a8de0fb..77e67c7 100644
--- a/tools/elf2uf2/main.cpp
+++ b/tools/elf2uf2/main.cpp
@@ -6,6 +6,7 @@
 
 #include <cstdio>
 #include <map>
+#include <set>
 #include <vector>
 #include <cstring>
 #include <cstdarg>
@@ -21,6 +22,8 @@ typedef unsigned int uint;
 #define ERROR_READ_FAILED -4
 #define ERROR_WRITE_FAILED -5
 
+#define FLASH_SECTOR_ERASE_SIZE 4096u
+
 static char error_msg[512];
 static bool verbose;
 
@@ -295,6 +298,28 @@ int elf2uf2(FILE *in, FILE *out) {
                         MAIN_RAM_START, sp);
         }
 #endif
+    } else {
+        // Fill in empty dummy uf2 pages to align the binary to flash sectors (except for the last sector which we don't
+        // need to pad, and choose not to to avoid making all SDK UF2s bigger)
+        // That workaround is required because the bootrom uses the block number for erase sector calculations:
+        // https://github.com/raspberrypi/pico-bootrom/blob/c09c7f08550e8a36fc38dc74f8873b9576de99eb/bootrom/virtual_disk.c#L205
+
+        std::set<uint32_t> touched_sectors;
+        for (auto& page_entry : pages) {
+            uint32_t sector = page_entry.first / FLASH_SECTOR_ERASE_SIZE;
+            touched_sectors.insert(sector);
+        }
+
+        uint32_t last_page = pages.rbegin()->first;
+        for (uint32_t sector : touched_sectors) {
+            for (uint32_t page = sector * FLASH_SECTOR_ERASE_SIZE; page < (sector + 1) * FLASH_SECTOR_ERASE_SIZE; page += PAGE_SIZE) {
+                if (page < last_page) {
+                    // Create a dummy page, if it does not exist yet. note that all present pages are first
+                    // zeroed before they are filled with any contents, so a dummy page will be all zeros.
+                    auto &dummy = pages[page];
+                }
+            }
+        }
     }
     uf2_block block;
     block.magic_start0 = UF2_MAGIC_START0;
@@ -308,7 +333,8 @@ int elf2uf2(FILE *in, FILE *out) {
         block.target_addr = page_entry.first;
         block.block_no = page_num++;
         if (verbose) {
-            printf("Page %d / %d %08x\n", block.block_no, block.num_blocks, block.target_addr);
+            printf("Page %d / %d %08x%s\n", block.block_no, block.num_blocks, block.target_addr,
+                   page_entry.second.empty() ? " (padding)": "");
         }
         memset(block.data, 0, sizeof(block.data));
         rc = realize_page(in, page_entry.second, block.data, sizeof(block.data));