From c808c344b4f799a69212cbed527bb1ab0b7c08e8 Mon Sep 17 00:00:00 2001 From: BlackMark Date: Fri, 10 Apr 2020 16:09:41 +0200 Subject: [PATCH] Implement basic stk500v2 protocol support --- stk500v2/main.cpp | 263 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 262 insertions(+), 1 deletion(-) diff --git a/stk500v2/main.cpp b/stk500v2/main.cpp index 65eebe2..dd56170 100644 --- a/stk500v2/main.cpp +++ b/stk500v2/main.cpp @@ -2,14 +2,275 @@ #include "uart/uart.hpp" +#include + #include "command.hpp" using uart_interface = uart::Hardware0, uart::Driven::BLOCKING>; +uart::Uart serial; + +struct Message { + uint8_t start; + uint8_t number; + uint16_t size; + uint8_t token; + uint8_t body[275]; + uint8_t checksum; +}; + +static inline bool receiveByte(uint8_t &data, uint16_t timeout = 1000) +{ + for (uint16_t i = 0; i < timeout; ++i) { + if (serial.rxByte(data)) + return true; + } + + return false; +} + +static inline uint8_t calcChecksum(const Message &msg) +{ + uint8_t checksum = msg.start; + checksum ^= msg.number; + checksum ^= msg.size; + checksum ^= msg.token; + for (uint8_t i = 0; i < msg.size; ++i) + checksum ^= msg.body[i]; + + return checksum; +} + +static inline bool receiveMessage(Message &msg) +{ + if (!receiveByte(msg.start) || msg.start != MESSAGE_START) + return false; + if (!receiveByte(msg.number)) + return false; + if (!receiveByte(*(reinterpret_cast(&msg.size) + 1))) + return false; + if (!receiveByte(*reinterpret_cast(&msg.size)) || msg.size > sizeof(msg.body)) + return false; + if (!receiveByte(msg.token) || msg.token != TOKEN) + return false; + for (uint16_t i = 0; i < msg.size; ++i) { + if (!receiveByte(msg.body[i])) + return false; + } + if (!receiveByte(msg.checksum) || msg.checksum != calcChecksum(msg)) + return false; + + return true; +} + +static inline bool isSignOn(const Message &msg) +{ + if (msg.size == 1 && msg.body[0] == CMD_SIGN_ON) + return true; + return false; +} + +static inline bool isGetParameter(const Message &msg) +{ + if (msg.size == 2 && msg.body[0] == CMD_GET_PARAMETER) + return true; + return false; +} + +static inline bool isSetParameter(const Message &msg) +{ + if (msg.size == 3 && msg.body[0] == CMD_SET_PARAMETER) + return true; + return false; +} + +static inline bool isEnterProgmodeIsp(const Message &msg) +{ + if (msg.size == 12 && msg.body[0] == CMD_ENTER_PROGMODE_ISP) + return true; + return false; +} + +static inline bool isReadSignatureIsp(const Message &msg) +{ + if (msg.size == 6 && msg.body[0] == CMD_READ_SIGNATURE_ISP) + return true; + return false; +} + +static inline bool isReadFuseIsp(const Message &msg) +{ + if (msg.size == 6 && msg.body[0] == CMD_READ_FUSE_ISP) + return true; + return false; +} + +static inline bool isLeaveProgmodeIsp(const Message &msg) +{ + if (msg.size == 3 && msg.body[0] == CMD_LEAVE_PROGMODE_ISP) + return true; + return false; +} + +static inline void formatSignOnAnswer(Message &msg) +{ + msg.size = 3 + 8; + msg.body[1] = STATUS_CMD_OK; + msg.body[2] = 8; + msg.body[3] = 'S'; + msg.body[4] = 'T'; + msg.body[5] = 'K'; + msg.body[6] = '5'; + msg.body[7] = '0'; + msg.body[8] = '0'; + msg.body[9] = '_'; + msg.body[10] = '2'; + msg.checksum = calcChecksum(msg); +} + +static inline void formatGetParameterAnswer(Message &msg) +{ + msg.size = 3; + + if (msg.body[1] == PARAM_HW_VER) { + msg.body[2] = 1; + } else if (msg.body[1] == PARAM_SW_MAJOR) { + msg.body[2] = 0x02; + } else if (msg.body[1] == PARAM_SW_MINOR) { + msg.body[2] = 0x0a; + } else if (msg.body[1] == PARAM_SCK_DURATION) { + msg.body[2] = 2; + } else if (msg.body[1] == PARAM_VADJUST) { + msg.body[2] = 25; + } else if (msg.body[1] == PARAM_VTARGET) { + msg.body[2] = 49; + } else if (msg.body[1] == PARAM_OSC_PSCALE) { + msg.body[2] = 2; + } else if (msg.body[1] == PARAM_OSC_CMATCH) { + msg.body[2] = 127; + } else { + msg.size = 2; + } + + if (msg.size == 2) { + msg.body[1] = STATUS_CMD_FAILED; + } else { + msg.body[1] = STATUS_CMD_OK; + } + + msg.checksum = calcChecksum(msg); +} + +static inline void formatSetParameterAnswer(Message &msg) +{ + msg.size = 2; + msg.body[1] = STATUS_CMD_OK; + + msg.checksum = calcChecksum(msg); +} + +static inline void formatEnterProgmodeIspAnswer(Message &msg) +{ + msg.size = 2; + msg.body[1] = STATUS_CMD_OK; + + msg.checksum = calcChecksum(msg); +} + +static inline void formatReadSignatureIspAnswer(Message &msg) +{ + msg.size = 4; + msg.body[2] = boot_signature_byte_get(msg.body[4] * 2); + msg.body[1] = STATUS_CMD_OK; + msg.body[3] = STATUS_CMD_OK; + + msg.checksum = calcChecksum(msg); +} + +static inline void formatReadFuseIspAnswer(Message &msg) +{ + constexpr auto READ_LOW_FUSE_BITS = 0x0050; + constexpr auto READ_HIGH_FUSE_BITS = 0x0858; + constexpr auto READ_EXTENDED_FUSE_BITS = 0x0850; + + msg.size = 4; + + if (*reinterpret_cast(msg.body + 2) == READ_EXTENDED_FUSE_BITS) { + msg.body[2] = boot_lock_fuse_bits_get(GET_EXTENDED_FUSE_BITS); + } + if (*reinterpret_cast(msg.body + 2) == READ_HIGH_FUSE_BITS) { + msg.body[2] = boot_lock_fuse_bits_get(GET_HIGH_FUSE_BITS); + } + if (*reinterpret_cast(msg.body + 2) == READ_LOW_FUSE_BITS) { + msg.body[2] = boot_lock_fuse_bits_get(GET_LOW_FUSE_BITS); + } + + msg.body[1] = STATUS_CMD_OK; + msg.body[3] = STATUS_CMD_OK; + + msg.checksum = calcChecksum(msg); +} + +static inline void formatLeaveProgmodeIspAnswer(Message &msg) +{ + msg.size = 2; + msg.body[1] = STATUS_CMD_OK; + + msg.checksum = calcChecksum(msg); +} + +static inline void formatErrorAnswer(Message &msg) +{ + msg.start = MESSAGE_START; + msg.size = 1; + msg.token = TOKEN; + msg.body[0] = STATUS_CMD_UNKNOWN; + msg.checksum = calcChecksum(msg); +} + +static inline void handleMessage(Message &msg) +{ + if (isSignOn(msg)) + formatSignOnAnswer(msg); + else if (isGetParameter(msg)) + formatGetParameterAnswer(msg); + else if (isSetParameter(msg)) + formatSetParameterAnswer(msg); + else if (isEnterProgmodeIsp(msg)) + formatEnterProgmodeIspAnswer(msg); + else if (isReadSignatureIsp(msg)) + formatReadSignatureIspAnswer(msg); + else if (isReadFuseIsp(msg)) + formatReadFuseIspAnswer(msg); + else if (isLeaveProgmodeIsp(msg)) + formatLeaveProgmodeIspAnswer(msg); + else + formatErrorAnswer(msg); +} + +static inline void transmitMessage(const Message &msg) +{ + serial.txByte(msg.start); + serial.txByte(msg.number); + serial.txByte(msg.size >> 8); + serial.txByte(msg.size & 0xFF); + serial.txByte(msg.token); + for (uint16_t i = 0; i < msg.size; ++i) + serial.txByte(msg.body[i]); + serial.txByte(msg.checksum); +} int main() { - uart::Uart serial; serial.init(); + Message msg; + + while (true) { + if (receiveMessage(msg)) { + handleMessage(msg); + transmitMessage(msg); + } + } + return 0; }