diff --git a/stk500v2/main.cpp b/stk500v2/main.cpp index 67af69b..8daabee 100644 --- a/stk500v2/main.cpp +++ b/stk500v2/main.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include "command.hpp" @@ -126,6 +127,13 @@ static inline bool isReadFlashIsp(const Message &msg) return false; } +static inline bool isReadEepromIsp(const Message &msg) +{ + if (msg.size == 4 && msg.body[0] == CMD_READ_EEPROM_ISP) + return true; + return false; +} + static inline bool isLeaveProgmodeIsp(const Message &msg) { if (msg.size == 3 && msg.body[0] == CMD_LEAVE_PROGMODE_ISP) @@ -264,6 +272,43 @@ static inline void formatReadFlashIspAnswer(Message &msg, uint32_t &addr) msg.checksum = calcChecksum(msg); } +namespace { + +bool isEepromReady() +{ + return (EECR & (1 << EEPE)) ? false : true; +} + +void waitEepromReady() +{ + while (!isEepromReady()) + ; +} + +uint8_t readEepromByte(const uint8_t *addr) +{ + EEAR = reinterpret_cast(addr); + EECR |= (1 << EERE); + return EEDR; +} + +} // namespace + +static inline void formatReadEepromIspAnswer(Message &msg, uint32_t &addr) +{ + const uint16_t numBytes = static_cast(msg.body[1]) << 8 | msg.body[2]; + msg.size = 3 + numBytes; + msg.body[1] = STATUS_CMD_OK; + waitEepromReady(); + for (uint16_t i = 0; i < numBytes; ++i) { + msg.body[i + 2] = readEepromByte(reinterpret_cast(addr + i)); + } + addr += numBytes; + msg.body[numBytes + 2] = STATUS_CMD_OK; + + msg.checksum = calcChecksum(msg); +} + static inline void formatLeaveProgmodeIspAnswer(Message &msg) { msg.size = 2; @@ -307,6 +352,8 @@ static inline void handleMessage(Message &msg) formatLoadAddressAnswer(msg); } else if (isReadFlashIsp(msg)) formatReadFlashIspAnswer(msg, s_address); + else if (isReadEepromIsp(msg)) + formatReadEepromIspAnswer(msg, s_address); else if (isLeaveProgmodeIsp(msg)) formatLeaveProgmodeIspAnswer(msg); else