Add portapack cpld write usb serial command for AG256SL100 devices

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 <device> <target> <file>\r\n"
+        "  device can be: hackrf, portapack\r\n"
+        "  target can be: sram (hackrf only), eeprom\r\n"
+        "  currently only \"cpld_write portapack eeprom <file>\" 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<std::array<uint32_t, 1801>>();
+                uint32_t magic = 0;
+                bool magic_found = false;
+                uint32_t expected_address = 0;
+
+                auto readData = std::vector<uint8_t>();
+
+                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 <lat> <lon> [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},

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<uint32_t, 1801>& 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 */

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<uint32_t, 1801>& block, uint32_t magic_value);
 
    private:
     using idcode_t = uint32_t;

firmware/common/jtag.hpp

@@ -23,6 +23,7 @@
 #define __JTAG_H__
 
 #include "jtag_target.hpp"
+#include "ch.h"
 
 #include <cstdint>
 #include <cstddef>
@@ -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) {

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();