From d05ddd188d10aa43398c6f33d9e6515c00b7cd2c Mon Sep 17 00:00:00 2001
From: Stefan Rueger <stefan.rueger@urclocks.com>
Date: Sat, 16 Jul 2022 11:06:18 +0100
Subject: [PATCH] Fix usbtiny read/verify for parts with more than 64 kB flash

Usbtiny has a protocol or firmware problem that prevents it from reading
flash above 64 kB in page mode (used by -U flash:r:... and -U flash:v:...).
This commit fixes that problem by falling back on byte access for flash paged
reads above 64k. It also issues the correct load extended address command for
parts with more than 128 kB flash thus extending support to ATmega2560 et al.
---
 src/usbtiny.c | 38 +++++++++++++++++++++++++++++++-------
 1 file changed, 31 insertions(+), 7 deletions(-)

diff --git a/src/usbtiny.c b/src/usbtiny.c
index 4bbaca7c..75c88be6 100644
--- a/src/usbtiny.c
+++ b/src/usbtiny.c
@@ -657,15 +657,39 @@ static int usbtiny_paged_load (PROGRAMMER * pgm, AVRPART * p, AVRMEM* m,
                                unsigned int addr, unsigned int n_bytes)
 {
   unsigned int maxaddr = addr + n_bytes;
-  int chunk;
-  int function;
-
+  int chunk, function;
+  OPCODE *lext, *readop;
+  unsigned char cmd[8];
 
   // First determine what we're doing
-  if (strcmp( m->desc, "flash" ) == 0) {
-    function = USBTINY_FLASH_READ;
-  } else {
-    function = USBTINY_EEPROM_READ;
+  function = strcmp(m->desc, "eeprom")==0?
+    USBTINY_EEPROM_READ: USBTINY_FLASH_READ;
+
+  // paged_load() only called for pages, so OK to set ext addr once at start
+  if((lext = m->op[AVR_OP_LOAD_EXT_ADDR])) {
+    memset(cmd, 0, sizeof(cmd));
+    avr_set_bits(lext, cmd);
+    avr_set_addr(lext, cmd, addr/2);
+    if(pgm->cmd(pgm, cmd, cmd+4) < 0)
+      return -1;
+  }
+
+  // Byte acces as work around to correctly read flash above 64 kiB
+  if(function == USBTINY_FLASH_READ && addr >= 0x10000) {
+    for(unsigned int i=0; i<n_bytes; i++, addr++) {
+      if(!(readop = m->op[addr&1? AVR_OP_READ_HI: AVR_OP_READ_LO]))
+        return -1;
+
+      memset(cmd, 0, sizeof(cmd));
+      avr_set_bits(readop, cmd);
+      avr_set_addr(readop, cmd, addr/2);
+      if(pgm->cmd(pgm, cmd, cmd+4) < 0)
+        return -1;
+      m->buf[addr] = 0;
+      avr_get_output(readop, cmd+4, m->buf + addr);
+    }
+
+    return n_bytes;
   }
 
   for (; addr < maxaddr; addr += chunk) {