diff --git a/firmware/application/usb_serial_shell.cpp b/firmware/application/usb_serial_shell.cpp index 80678d93..1c8fb9df 100644 --- a/firmware/application/usb_serial_shell.cpp +++ b/firmware/application/usb_serial_shell.cpp @@ -907,6 +907,191 @@ static void cmd_cpld_read(BaseSequentialStream* chp, int argc, char* argv[]) { } } +static uint32_t reverse(uint32_t x) { + x = ((x >> 1) & 0x55555555u) | ((x & 0x55555555u) << 1); + x = ((x >> 2) & 0x33333333u) | ((x & 0x33333333u) << 2); + x = ((x >> 4) & 0x0f0f0f0fu) | ((x & 0x0f0f0f0fu) << 4); + x = ((x >> 8) & 0x00ff00ffu) | ((x & 0x00ff00ffu) << 8); + x = ((x >> 16) & 0xffffu) | ((x & 0xffffu) << 16); + return x; +} + +static void cmd_cpld_write(BaseSequentialStream* chp, int argc, char* argv[]) { + const char* usage = + "usage: cpld_write \r\n" + " device can be: hackrf, portapack\r\n" + " target can be: sram (hackrf only), eeprom\r\n" + " currently only \"cpld_write portapack eeprom \" is supported\r\n"; + + if (argc != 3) { + chprintf(chp, usage); + return; + } + + if (strncmp(argv[0], "hackrf", 5) == 0) { + chprintf(chp, usage); + } else if (strncmp(argv[0], "portapack", 5) == 0) { + if (strncmp(argv[1], "eeprom", 5) == 0) { + jtag::GPIOTarget target{ + portapack::gpio_cpld_tck, + portapack::gpio_cpld_tms, + portapack::gpio_cpld_tdi, + portapack::gpio_cpld_tdo}; + jtag::JTAG jtag{target}; + portapack::cpld::CPLD cpld{jtag}; + + cpld.reset(); + cpld.run_test_idle(); + uint32_t idcode = cpld.get_idcode(); + + chprintf(chp, "CPLD IDCODE: 0x%08X\r\n", idcode); + + if (idcode == 0x00025610) { + chprintf(chp, "CPLD Model: AGM AG256SL100\r\n"); + + if (cpld.AGM_enter_maintenance_mode() == false) { + return; + } + + File* file = new File(); + auto path = path_from_string8(argv[2]); + auto error = file->open(path); + + if (error.is_valid()) { + chprintf(chp, "Error opening file: %s %d %s\r\n", argv[2], error.value().code(), error.value().what().c_str()); + delete file; + cpld.AGM_exit_maintenance_mode(); + return; + } + + auto data = std::make_unique>(); + uint32_t magic = 0; + bool magic_found = false; + uint32_t expected_address = 0; + + auto readData = std::vector(); + + chprintf(chp, "Reading file...\r\n"); + file->seek(0); + + while (!file->eof().value()) { + uint32_t remainingData = readData.size(); + uint32_t bytesToRead = 512 - remainingData; + + readData.resize(512); + + auto result = file->read(readData.data() + remainingData, bytesToRead); + + if (result.is_error()) { + chprintf(chp, "Error reading file: %d %s\r\n", result.error().code(), result.error().what().c_str()); + cpld.AGM_exit_maintenance_mode(); + file->close(); + delete file; + return; + } + + if (result.value() != 512) + readData.resize(remainingData + result.value()); + + do { + auto it = std::find(readData.begin(), readData.end(), '\n'); + if (it != readData.end()) { + std::string line(readData.begin(), it); + readData.erase(readData.begin(), it + 1); + + line.erase(std::remove(line.begin(), line.end(), '\r'), line.end()); + line.erase(std::remove(line.begin(), line.end(), '\n'), line.end()); + + auto prefix = line.find("sdr 64 -tdi ", 0); + auto suffix = line.find("0040", line.size() - 4); + + if (prefix == 0 && suffix == line.size() - 4) { + std::string dataString = line.substr(line.size() - 16, 16); + + uint32_t address = reverse(std::stoul(dataString.substr(8, 8), nullptr, 16) - 64) / 4; + uint32_t value = std::stoul(dataString.substr(0, 8), nullptr, 16); + + if (expected_address == 299 && address == 0) { + magic = value; + magic_found = true; + chprintf(chp, "Magic found: %08X\r\n", magic); + continue; + } + + if (expected_address != address) { + chprintf(chp, "Error: expected address %d, got %d\r\n", expected_address, address); + cpld.AGM_exit_maintenance_mode(); + file->close(); + delete file; + return; + } + + (*data)[expected_address] = value; + expected_address++; + + if (expected_address == 1801) { + if (!magic_found) { + chprintf(chp, "Error: magic not found\r\n"); + cpld.AGM_exit_maintenance_mode(); + file->close(); + delete file; + return; + } + + chprintf(chp, "Writing data to CPLD...\r\n"); + file->close(); + delete file; + + cpld.AGM_write(*data, magic); + + cpld.AGM_enter_read_mode(); + + CRC<32> crc{0x04c11db7, 0xffffffff, 0xffffffff}; + for (size_t i = 0; i < 2048; i++) { + uint32_t data = cpld.AGM_read(i); + crc.process_byte((data >> 0) & 0xff); + crc.process_byte((data >> 8) & 0xff); + crc.process_byte((data >> 16) & 0xff); + crc.process_byte((data >> 24) & 0xff); + } + + cpld.AGM_exit_maintenance_mode(); + + chprintf(chp, "New CPLD firmware checksum: 0x%08X\r\n", crc.checksum()); + + m4_request_shutdown(); + chThdSleepMilliseconds(1000); + + WWDT_MOD = WWDT_MOD_WDEN | WWDT_MOD_WDRESET; + WWDT_TC = 100000 & 0xFFFFFF; + WWDT_FEED_SEQUENCE; + + return; + break; + } + } + } else { + break; + } + } while (true); + } + + file->close(); + delete file; + + cpld.AGM_exit_maintenance_mode(); + + } else { + chprintf(chp, "CPLD Model: unknown\r\n"); + } + } else { + chprintf(chp, usage); + } + } else { + chprintf(chp, usage); + } +} + static void cmd_gotgps(BaseSequentialStream* chp, int argc, char* argv[]) { const char* usage = "usage: gotgps [altitude] [speed] [satinuse]\r\n"; if (argc < 2 || argc > 5) { @@ -1203,6 +1388,7 @@ static const ShellCommand commands[] = { {"rtcset", cmd_rtcset}, {"cpld_info", cpld_info}, {"cpld_read", cmd_cpld_read}, + {"cpld_write", cmd_cpld_write}, {"accessibility_readall", cmd_accessibility_readall}, {"accessibility_readcurr", cmd_accessibility_readcurr}, {"applist", cmd_applist}, diff --git a/firmware/common/cpld_max5.cpp b/firmware/common/cpld_max5.cpp index 894c0a78..b1fb6889 100644 --- a/firmware/common/cpld_max5.cpp +++ b/firmware/common/cpld_max5.cpp @@ -332,5 +332,40 @@ uint32_t CPLD::AGM_read(uint32_t address) { return jtag.shift_dr(32, encoded_address, 0x0); } +void CPLD::AGM_write(const std::array& block, uint32_t magic_value) { + shift_ir(instruction_t::AGM_SET_REGISTER); + jtag.runtest_tck(100); + jtag.shift_dr(8, 0xf0); + jtag.runtest_tck(100); + + shift_ir(instruction_t::AGM_ERASE); + jtag.runtest_tck(100); + jtag.runtest_ms(500); + + shift_ir(instruction_t::AGM_SET_REGISTER); + jtag.runtest_tck(100); + jtag.shift_dr(8, 0xf0); + jtag.runtest_tck(100); + + shift_ir(instruction_t::AGM_PROGRAM); + jtag.runtest_tck(100); + + auto data = block.data(); + for (size_t i = 0; i < 0x12B; i++) { + auto address = AGM_encode_address(i * 4, 0x40); + jtag.shift_dr(32, address, data[i]); + jtag.runtest_ms(2); + } + + jtag.shift_dr(32, 0x00000040, magic_value); + jtag.runtest_ms(2); + + for (size_t i = 0x12B; i < block.size(); i++) { + auto address = AGM_encode_address(i * 4, 0x40); + jtag.shift_dr(32, address, data[i]); + jtag.runtest_ms(2); + } +} + } /* namespace max5 */ } /* namespace cpld */ diff --git a/firmware/common/cpld_max5.hpp b/firmware/common/cpld_max5.hpp index e8dfdf8c..dcbbac2a 100644 --- a/firmware/common/cpld_max5.hpp +++ b/firmware/common/cpld_max5.hpp @@ -99,6 +99,7 @@ class CPLD { void AGM_enter_read_mode(); uint32_t AGM_encode_address(uint32_t address, uint32_t trailer); uint32_t AGM_read(uint32_t address); + void AGM_write(const std::array& block, uint32_t magic_value); private: using idcode_t = uint32_t; diff --git a/firmware/common/jtag.hpp b/firmware/common/jtag.hpp index 561446ae..3ef42627 100644 --- a/firmware/common/jtag.hpp +++ b/firmware/common/jtag.hpp @@ -23,6 +23,7 @@ #define __JTAG_H__ #include "jtag_target.hpp" +#include "ch.h" #include #include @@ -51,7 +52,16 @@ class JTAG { } void runtest_tck(const size_t count) { - target.delay(count); + for (size_t i = 0; i < count; i++) { + target.clock(0, 0); + } + } + + void runtest_ms(const size_t count) { + auto starttime = chTimeNow(); + + while ((chTimeNow() - starttime) < (count + 1)) + target.clock(0, 0); } uint32_t shift_ir(const size_t count, const uint32_t value) { diff --git a/firmware/common/message_queue.hpp b/firmware/common/message_queue.hpp index 57534dc9..edef2ef7 100644 --- a/firmware/common/message_queue.hpp +++ b/firmware/common/message_queue.hpp @@ -101,7 +101,11 @@ class MessageQueue { } bool push(const void* const buf, const size_t len) { - chMtxLock(&mutex_write); + bool lock_success = chMtxTryLock(&mutex_write); + if (!lock_success) { + return false; + } + const auto result = fifo.in_r(buf, len); chMtxUnlock();