From a72bf67b07b1929e1576d59eaba894eef8f2ebbe Mon Sep 17 00:00:00 2001 From: BlackMark Date: Sun, 12 Apr 2020 13:10:00 +0200 Subject: [PATCH] Fix flash memory address being a word-address --- stk500v2/main.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/stk500v2/main.cpp b/stk500v2/main.cpp index cab4170..77fbeec 100644 --- a/stk500v2/main.cpp +++ b/stk500v2/main.cpp @@ -321,13 +321,15 @@ static inline void formatLoadAddressAnswer(Message &msg) static inline void formatReadFlashIspAnswer(Message &msg, uint32_t &addr) { + const uint16_t byteAddress = 2 * addr; const uint16_t numBytes = static_cast(msg.body[1]) << 8 | msg.body[2]; msg.size = 3 + numBytes; msg.body[1] = STATUS_CMD_OK; for (uint16_t i = 0; i < numBytes; ++i) { - msg.body[i + 2] = pgm_read_byte(static_cast(addr + i)); + msg.body[i + 2] = pgm_read_byte(static_cast(byteAddress + i)); } - addr += numBytes; + const auto numWords = numBytes / 2; + addr += numWords; msg.body[numBytes + 2] = STATUS_CMD_OK; msg.checksum = calcChecksum(msg); @@ -459,10 +461,12 @@ static inline void formatReadEepromIspAnswer(Message &msg, uint32_t &addr) static inline void formatProgramFlashIspAnswer(Message &msg, uint32_t &addr) { - if (addr < getFlashSize()) - writeFlashPage(addr, msg.body + 10); + const auto byteAddress = 2 * addr; + if (byteAddress < getFlashSize()) + writeFlashPage(byteAddress, msg.body + 10); const uint16_t numBytes = static_cast(msg.body[1]) << 8 | msg.body[2]; - addr += numBytes; + const auto numWords = numBytes / 2; + addr += numWords; msg.size = 2; msg.body[1] = STATUS_CMD_OK;