Add portapack cpld write usb serial command for AG256SL100 devices (#2401)

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