added cpld info shell commands (#1703)

* added cpld info shell commands

* fixed reset

firmware/application/usb_serial_shell.cpp

@@ -27,6 +27,15 @@
 #include "png_writer.hpp"
 #include "irq_controls.hpp"
 
+#include "portapack.hpp"
+#include "portapack_hal.hpp"
+#include "hackrf_gpio.hpp"
+#include "jtag_target_gpio.hpp"
+#include "cpld_max5.hpp"
+#include "portapack_cpld_data.hpp"
+#include "crc.hpp"
+#include "hackrf_cpld_data.hpp"
+
 #include "usb_serial_io.h"
 #include "ff.h"
 #include "chprintf.h"
@@ -36,6 +45,7 @@
 #include <codecvt>
 #include <cstring>
 #include <locale>
+#include <libopencm3/lpc43xx/wwdt.h>
 
 #define SHELL_WA_SIZE THD_WA_SIZE(1024 * 3)
 #define palOutputPad(port, pad) (LPC_GPIO->DIR[(port)] |= 1 << (pad))
@@ -476,6 +486,314 @@ static void cmd_sd_write(BaseSequentialStream* chp, int argc, char* argv[]) {
     chprintf(chp, "ok\r\n");
 }
 
+static void cpld_info(BaseSequentialStream* chp, int argc, char* argv[]) {
+    const char* usage =
+        "usage: cpld_info <device>\r\n"
+        "  supported modes:\r\n"
+        "    cpld_info hackrf\r\n"
+        "    cpld_info portapack\r\n";
+    if (argc != 1) {
+        chprintf(chp, usage);
+        return;
+    }
+
+    if (strncmp(argv[0], "hackrf", 5) == 0) {
+        jtag::GPIOTarget jtag_target_hackrf_cpld{
+            hackrf::one::gpio_cpld_tck,
+            hackrf::one::gpio_cpld_tms,
+            hackrf::one::gpio_cpld_tdi,
+            hackrf::one::gpio_cpld_tdo,
+        };
+
+        hackrf::one::cpld::CPLD hackrf_cpld{jtag_target_hackrf_cpld};
+        {
+            CRC<32> crc{0x04c11db7, 0xffffffff, 0xffffffff};
+
+            hackrf_cpld.prepare_read_eeprom();
+
+            for (const auto& block : hackrf::one::cpld::verify_blocks) {
+                auto from_device = hackrf_cpld.read_block_eeprom(block.id);
+
+                for (std::array<bool, 274UL>::reverse_iterator i = from_device.rbegin(); i != from_device.rend(); ++i) {
+                    auto bit = *i;
+                    crc.process_bit(bit);
+                }
+            }
+
+            hackrf_cpld.finalize_read_eeprom();
+            chprintf(chp, "CPLD eeprom firmware checksum: 0x%08X\r\n", crc.checksum());
+        }
+
+        {
+            CRC<32> crc{0x04c11db7, 0xffffffff, 0xffffffff};
+
+            hackrf_cpld.prepare_read_sram();
+
+            for (const auto& block : hackrf::one::cpld::verify_blocks) {
+                auto from_device = hackrf_cpld.read_block_sram(block);
+
+                for (std::array<bool, 274UL>::reverse_iterator i = from_device.rbegin(); i != from_device.rend(); ++i) {
+                    auto bit = *i;
+                    crc.process_bit(bit);
+                }
+            }
+
+            hackrf_cpld.finalize_read_sram(hackrf::one::cpld::verify_blocks[0].id);
+            chprintf(chp, "CPLD sram firmware checksum: 0x%08X\r\n", crc.checksum());
+        }
+
+    } else if (strncmp(argv[0], "portapack", 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 == 0x20A50DD) {
+            chprintf(chp, "CPLD Model: Altera MAX V 5M40Z\r\n");
+            cpld.reset();
+            cpld.run_test_idle();
+            cpld.sample();
+            cpld.bypass();
+            cpld.enable();
+
+            CRC<32> crc{0x04c11db7, 0xffffffff, 0xffffffff};
+            cpld.prepare_read(0x0000);
+
+            for (size_t i = 0; i < 3328; i++) {
+                uint16_t data = cpld.read();
+                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.prepare_read(0x0001);
+
+            for (size_t i = 0; i < 512; i++) {
+                uint16_t data = cpld.read();
+                crc.process_byte((data >> 0) & 0xff);
+                crc.process_byte((data >> 8) & 0xff);
+                crc.process_byte((data >> 16) & 0xff);
+                crc.process_byte((data >> 24) & 0xff);
+            }
+
+            chprintf(chp, "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;
+
+        } else if (idcode == 0x00025610) {
+            chprintf(chp, "CPLD Model: AGM AG256SL100\r\n");
+
+            if (cpld.AGM_enter_maintenance_mode() == false) {
+                return;
+            }
+
+            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, "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;
+
+        } else {
+            chprintf(chp, "CPLD Model: unknown\r\n");
+        }
+
+    } else {
+        chprintf(chp, usage);
+    }
+}
+
+static void cmd_cpld_read(BaseSequentialStream* chp, int argc, char* argv[]) {
+    const char* usage =
+        "usage: cpld_read <device> <target>\r\n"
+        "  device can be: hackrf, portapack\r\n"
+        "  target can be: sram (hackrf only), eeprom\r\n";
+
+    if (argc != 2) {
+        chprintf(chp, usage);
+        return;
+    }
+
+    if (strncmp(argv[0], "hackrf", 5) == 0) {
+        if (strncmp(argv[1], "eeprom", 5) == 0) {
+            jtag::GPIOTarget jtag_target_hackrf_cpld{
+                hackrf::one::gpio_cpld_tck,
+                hackrf::one::gpio_cpld_tms,
+                hackrf::one::gpio_cpld_tdi,
+                hackrf::one::gpio_cpld_tdo,
+            };
+
+            hackrf::one::cpld::CPLD hackrf_cpld{jtag_target_hackrf_cpld};
+
+            hackrf_cpld.prepare_read_eeprom();
+
+            for (const auto& block : hackrf::one::cpld::verify_blocks) {
+                auto from_device = hackrf_cpld.read_block_eeprom(block.id);
+
+                chprintf(chp, "bank %04X: ", block.id);
+                uint32_t n = 6;
+                uint8_t byte = 0;
+                for (std::array<bool, 274UL>::reverse_iterator i = from_device.rbegin(); i != from_device.rend(); ++i) {
+                    auto bit = *i;
+                    byte |= bit << (7 - (n % 8));
+                    if (n % 8 == 7) {
+                        chprintf(chp, "%02X ", byte);
+                        byte = 0;
+                    }
+                    n++;
+                }
+                chprintf(chp, "\r\n");
+            }
+
+            hackrf_cpld.finalize_read_eeprom();
+        }
+
+        else if (strncmp(argv[1], "sram", 5) == 0) {
+            jtag::GPIOTarget jtag_target_hackrf_cpld{
+                hackrf::one::gpio_cpld_tck,
+                hackrf::one::gpio_cpld_tms,
+                hackrf::one::gpio_cpld_tdi,
+                hackrf::one::gpio_cpld_tdo,
+            };
+
+            hackrf::one::cpld::CPLD hackrf_cpld{jtag_target_hackrf_cpld};
+
+            hackrf_cpld.prepare_read_sram();
+
+            for (const auto& block : hackrf::one::cpld::verify_blocks) {
+                auto from_device = hackrf_cpld.read_block_sram(block);
+
+                chprintf(chp, "bank %04X: ", block.id);
+                uint32_t n = 6;
+                uint8_t byte = 0;
+                for (std::array<bool, 274UL>::reverse_iterator i = from_device.rbegin(); i != from_device.rend(); ++i) {
+                    auto bit = *i;
+                    byte |= bit << (7 - (n % 8));
+                    if (n % 8 == 7) {
+                        chprintf(chp, "%02X ", byte);
+                        byte = 0;
+                    }
+                    n++;
+                }
+                chprintf(chp, "\r\n");
+            }
+
+            hackrf_cpld.finalize_read_sram(hackrf::one::cpld::verify_blocks[0].id);
+        }
+    } else if (strncmp(argv[0], "portapack", 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 == 0x20A50DD) {
+            chprintf(chp, "CPLD Model: Altera MAX V 5M40Z\r\n");
+            cpld.reset();
+            cpld.run_test_idle();
+            cpld.sample();
+            cpld.bypass();
+            cpld.enable();
+
+            cpld.prepare_read(0x0000);
+
+            for (size_t i = 0; i < 3328; i++) {
+                uint16_t data = cpld.read();
+                chprintf(chp, "%d: 0x%04X\r\n", i, data);
+            }
+
+            cpld.prepare_read(0x0001);
+
+            for (size_t i = 0; i < 512; i++) {
+                uint16_t data = cpld.read();
+                chprintf(chp, "%d: 0x%04X\r\n", i, data);
+            }
+
+            m4_request_shutdown();
+            chThdSleepMilliseconds(1000);
+
+            WWDT_MOD = WWDT_MOD_WDEN | WWDT_MOD_WDRESET;
+            WWDT_TC = 100000 & 0xFFFFFF;
+            WWDT_FEED_SEQUENCE;
+
+        } else if (idcode == 0x00025610) {
+            chprintf(chp, "CPLD Model: AGM AG256SL100\r\n");
+
+            if (cpld.AGM_enter_maintenance_mode() == false) {
+                return;
+            }
+
+            cpld.AGM_enter_read_mode();
+
+            for (size_t i = 0; i < 2048; i++) {
+                uint32_t data = cpld.AGM_read(i);
+                if (i % 4 == 0)
+                    chprintf(chp, "%5d: ", i * 4);
+
+                chprintf(chp, "0x%08X", data);
+
+                if (i % 4 == 3)
+                    chprintf(chp, "\r\n");
+                else
+                    chprintf(chp, " ");
+            }
+
+            cpld.AGM_exit_maintenance_mode();
+
+            m4_request_shutdown();
+            chThdSleepMilliseconds(1000);
+
+            WWDT_MOD = WWDT_MOD_WDEN | WWDT_MOD_WDRESET;
+            WWDT_TC = 100000 & 0xFFFFFF;
+            WWDT_FEED_SEQUENCE;
+
+        } else {
+            chprintf(chp, "CPLD Model: unknown\r\n");
+        }
+
+    } else {
+        chprintf(chp, usage);
+    }
+}
+
 static const ShellCommand commands[] = {
     {"reboot", cmd_reboot},
     {"dfu", cmd_dfu},
@@ -496,6 +814,8 @@ static const ShellCommand commands[] = {
     {"read", cmd_sd_read},
     {"write", cmd_sd_write},
     {"filesize", cmd_sd_filesize},
+    {"cpld_info", cpld_info},
+    {"cpld_read", cmd_cpld_read},
     {NULL, NULL}};
 
 static const ShellConfig shell_cfg1 = {

firmware/common/cpld_max5.cpp

@@ -147,9 +147,12 @@ void CPLD::sector_select(const uint16_t id) {
 }
 
 bool CPLD::idcode_ok() {
+    return (get_idcode() == idcode);
+}
+
+uint32_t CPLD::get_idcode() {
     shift_ir(instruction_t::IDCODE);
-    const auto idcode_read = jtag.shift_dr(idcode_length, 0);
+    return jtag.shift_dr(idcode_length, 0);
-    return (idcode_read == idcode);
 }
 
 std::array<uint16_t, 5> CPLD::read_silicon_id() {
@@ -208,6 +211,16 @@ void CPLD::program_block(
     }
 }
 
+void CPLD::prepare_read(uint16_t block) {
+    sector_select(block);
+    shift_ir(instruction_t::ISC_READ);
+    jtag.runtest_tck(93);  // 5us
+}
+
+uint32_t CPLD::read() {
+    return jtag.shift_dr(16, 0xffff) & 0xfbff;
+}
+
 bool CPLD::verify_block(
     const uint16_t id,
     const uint16_t* const data,
@@ -265,5 +278,59 @@ bool CPLD::is_blank() {
     return block_0_blank && block_1_blank;
 }
 
+bool CPLD::AGM_enter_maintenance_mode() {
+    shift_ir(instruction_t::AGM_STAGE_1);
+    jtag.runtest_tck(100);
+    shift_ir(instruction_t::AGM_STAGE_2);
+    jtag.runtest_tck(100);
+    shift_ir(instruction_t::AGM_STAGE_1);
+    jtag.runtest_tck(100);
+
+    shift_ir(instruction_t::AGM_SET_REGISTER);
+    jtag.runtest_tck(100);
+    jtag.shift_dr(8, 0x0);
+    jtag.runtest_tck(100);
+
+    shift_ir(instruction_t::AGM_PROGRAM);
+    jtag.runtest_tck(100);
+    jtag.shift_dr(32, 0x203f0044uL, 0x80000000);
+
+    shift_ir(instruction_t::IDCODE);
+    jtag.runtest_tck(100);
+    auto idcode = jtag.shift_dr(idcode_length, 0);
+
+    return idcode == 0x00025610;
+}
+
+void CPLD::AGM_exit_maintenance_mode() {
+    shift_ir(instruction_t::AGM_RESET);
+    jtag.runtest_tck(100);
+}
+
+void CPLD::AGM_enter_read_mode() {
+    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_READ);
+    jtag.runtest_tck(100);
+}
+
+uint32_t CPLD::AGM_encode_address(uint32_t address, uint32_t trailer) {
+    uint32_t p = trailer;
+    for (size_t i = 0; i < 18; i++) {
+        auto address_bit = (address >> i) & 0x01;
+        p |= address_bit << (31 - i);
+    }
+
+    return p;
+}
+
+uint32_t CPLD::AGM_read(uint32_t address) {
+    auto encoded_address = AGM_encode_address(address * 4, 0xC0);
+    return jtag.shift_dr(32, encoded_address, 0x0);
+}
+
 } /* namespace max5 */
 } /* namespace cpld */

firmware/common/cpld_max5.hpp

@@ -60,6 +60,7 @@ class CPLD {
     }
 
     bool idcode_ok();
+    uint32_t get_idcode();
 
     void enable();
 
@@ -90,6 +91,15 @@ class CPLD {
 
     std::pair<bool, uint8_t> boundary_scan();
 
+    void prepare_read(uint16_t block);
+    uint32_t read();
+
+    bool AGM_enter_maintenance_mode();
+    void AGM_exit_maintenance_mode();
+    void AGM_enter_read_mode();
+    uint32_t AGM_encode_address(uint32_t address, uint32_t trailer);
+    uint32_t AGM_read(uint32_t address);
+
    private:
     using idcode_t = uint32_t;
     static constexpr size_t idcode_length = 32;
@@ -115,6 +125,13 @@ class CPLD {
         ISC_ADDRESS_SHIFT = 0b1000000011,  // 0x203
         ISC_READ = 0b1000000101,           // 0x205
         ISC_NOOP = 0b1000010000,           // 0x210
+        AGM_RESET = 0x3f7,
+        AGM_STAGE_1 = 0x3f8,
+        AGM_STAGE_2 = 0x3f9,
+        AGM_PROGRAM = 0x3fa,
+        AGM_SET_REGISTER = 0x3fc,
+        AGM_READ = 0x3fd,
+        AGM_ERASE = 0x3fe,
     };
 
     void shift_ir(const instruction_t instruction) {

firmware/common/cpld_xilinx.cpp

@@ -19,6 +19,7 @@
  * Boston, MA 02110-1301, USA.
  */
 
+#include "ch.h"
 #include "cpld_xilinx.hpp"
 
 namespace cpld {
@@ -47,6 +48,25 @@ void XC2C64A::write_sram(const verify_blocks_t& blocks) {
 }
 
 bool XC2C64A::verify_sram(const verify_blocks_t& blocks) {
+    prepare_read_sram();
+
+    const jtag::tap::bits_t empty_row{block_length};
+
+    auto error = false;
+    for (const auto& block : blocks) {
+        tap.shift({&block.id, block_id_length}, true);
+        tap.state(state_t::run_test_idle);
+
+        tap.state(state_t::shift_dr);
+        error |= tap.shift(empty_row, {block.data.data(), block_length}, {block.mask.data(), block_length}, false, nullptr);
+    }
+
+    finalize_read_sram(blocks[0].id);
+
+    return !error;
+}
+
+void XC2C64A::prepare_read_sram() {
     tap.set_repeat(0);
     tap.set_end_ir(state_t::run_test_idle);
     tap.set_end_dr(state_t::run_test_idle);
@@ -61,17 +81,25 @@ bool XC2C64A::verify_sram(const verify_blocks_t& blocks) {
 
     tap.state(state_t::shift_dr);
     tap.shift(empty_row, false);
+}
 
-    auto error = false;
+std::array<bool, 274> XC2C64A::read_block_sram(verify_block_t block) {
-    for (const auto& block : blocks) {
     tap.shift({&block.id, block_id_length}, true);
     tap.state(state_t::run_test_idle);
 
     tap.state(state_t::shift_dr);
-        error |= tap.shift(empty_row, {block.data.data(), block_length}, {block.mask.data(), block_length}, false);
+    const jtag::tap::bits_t empty_row{block_length};
+    std::vector<bool> from_device;
+
+    tap.shift(empty_row, {block.data.data(), block_length}, {block.mask.data(), block_length}, false, &from_device);
+
+    std::array<bool, block_length> ret;
+    std::copy_n(std::make_move_iterator(from_device.begin()), block_length, ret.begin());
+    return ret;
 }
-    // Redundant operation to finish the row.
+
-    tap.shift({&blocks[0].id, block_id_length}, true);
+void XC2C64A::finalize_read_sram(block_id_t id) {
+    tap.shift({&id, block_id_length}, true);
     tap.state(state_t::run_test_idle);
     tap.set_end_dr(state_t::run_test_idle);
 
@@ -79,8 +107,6 @@ bool XC2C64A::verify_sram(const verify_blocks_t& blocks) {
     bypass();
 
     tap.state(state_t::test_logic_reset);
-
-    return !error;
 }
 
 bool XC2C64A::verify_eeprom(const verify_blocks_t& blocks) {
@@ -133,6 +159,44 @@ void XC2C64A::init_from_eeprom() {
     tap.state(state_t::test_logic_reset);
 }
 
+void XC2C64A::prepare_read_eeprom() {
+    tap.set_repeat(0);
+    tap.set_end_ir(state_t::run_test_idle);
+    tap.set_end_dr(state_t::run_test_idle);
+
+    reset();
+    bypass();
+    enable();
+
+    shift_ir(instruction_t::ISC_READ);
+}
+
+std::array<bool, 274> XC2C64A::read_block_eeprom(block_id_t id) {
+    const jtag::tap::bits_t empty_row{block_length};
+
+    tap.set_end_dr(state_t::pause_dr);
+    tap.shift_dr({&id, block_id_length});
+    tap.set_end_ir(state_t::run_test_idle);
+    tap.wait(state_t::pause_dr, state_t::pause_dr, 20);
+    tap.set_end_ir(state_t::run_test_idle);
+    tap.wait(state_t::run_test_idle, state_t::run_test_idle, 100);
+
+    std::vector<bool> from_device = tap.shift_dr_read(empty_row);
+
+    tap.wait(state_t::run_test_idle, state_t::run_test_idle, 100);
+
+    std::array<bool, block_length> ret;
+    std::copy_n(std::make_move_iterator(from_device.begin()), block_length, ret.begin());
+    return ret;
+}
+
+void XC2C64A::finalize_read_eeprom() {
+    disable();
+    bypass();
+
+    tap.state(state_t::test_logic_reset);
+}
+
 bool XC2C64A::shift_ir(const instruction_t instruction) {
     const ir_t ir_buffer = toUType(instruction);
     const jtag::tap::bits_t bits{&ir_buffer, ir_length};

firmware/common/cpld_xilinx.hpp

@@ -68,6 +68,13 @@ class XC2C64A {
     bool verify_eeprom(const verify_blocks_t& blocks);
     void init_from_eeprom();
 
+    void prepare_read_eeprom();
+    void prepare_read_sram();
+    std::array<bool, block_length> read_block_eeprom(block_id_t id);
+    std::array<bool, block_length> read_block_sram(verify_block_t block);
+    void finalize_read_eeprom();
+    void finalize_read_sram(block_id_t id);
+
    private:
     static constexpr size_t idcode_length = 32;
     using idcode_t = uint32_t;

firmware/common/jtag.cpp

@@ -37,4 +37,15 @@ uint32_t JTAG::shift(const size_t count, uint32_t value) {
     return value;
 }
 
+uint32_t JTAG::shift_header(const size_t count, uint32_t value) {
+    for (size_t i = 0; i < count; i++) {
+        const auto tdo = target.clock(
+            0,
+            value & 1);
+        value >>= 1;
+        value |= tdo << (count - 1);
+    }
+    return value;
+}
+
 } /* namespace jtag */

firmware/common/jtag.hpp

@@ -89,10 +89,29 @@ class JTAG {
         return result;
     }
 
+    uint32_t shift_dr(const size_t count, const uint32_t address, const uint32_t value) {
+        /* Run-Test/Idle -> Select-DR-Scan */
+        target.clock(1, 0);
+        /* Scan -> Capture -> Shift */
+        target.clock(0, 0);
+        target.clock(0, 0);
+
+        shift_header(count, address);
+        const auto result = shift(count, value);
+
+        /* Exit1 -> Update */
+        target.clock(1, 0);
+        /* Update -> Run-Test/Idle */
+        target.clock(0, 0);
+
+        return result;
+    }
+
    private:
     Target& target;
 
     uint32_t shift(const size_t count, uint32_t value);
+    uint32_t shift_header(const size_t count, uint32_t value);
 };
 
 } /* namespace jtag */

firmware/common/jtag_tap.cpp

@@ -191,6 +191,10 @@ bool TAPMachine::shift_dr(const bits_t& tdi_value, const bits_t& tdo_expected, c
     return shift_data(tdi_value, tdo_expected, tdo_mask, state_t::shift_dr, _end_dr, _run_test);
 }
 
+std::vector<bool> TAPMachine::shift_dr_read(const bits_t& tdi_value) {
+    return shift_data_read(tdi_value, state_t::shift_dr, _end_dr, _run_test);
+}
+
 void TAPMachine::state(const state_t state) {
     if (state == state_t::test_logic_reset) {
         for (int i = 0; i < 5; i++) {
@@ -227,7 +231,7 @@ void TAPMachine::shift_start(const state_t state) {
     advance_to_state(state);
 }
 
-bool TAPMachine::shift(const bits_t& tdi, const bits_t& tdo_expected, const bits_t& tdo_mask, const bool end_tms) {
+bool TAPMachine::shift(const bits_t& tdi, const bits_t& tdo_expected, const bits_t& tdo_mask, const bool end_tms, std::vector<bool>* from_device) {
     if (tdo_expected.length() != tdo_mask.length()) {
         return false;
     }
@@ -239,6 +243,10 @@ bool TAPMachine::shift(const bits_t& tdi, const bits_t& tdo_expected, const bits
     for (uint32_t i = 0; i < tdi.length(); i++) {
         const auto tms = end_tms & (i == (tdi.length() - 1));
         const auto tdo = clock(tms, tdi[i]);
+
+        if (from_device != nullptr)
+            from_device->push_back(tdo);
+
         if (tdo_expected && tdo_mask) {
             tdo_error |= (tdo & tdo_mask[i]) != (tdo_expected[i] & tdo_mask[i]);
         }
@@ -258,7 +266,15 @@ void TAPMachine::shift_end(const state_t end_state, const uint32_t end_delay) {
 
 bool TAPMachine::shift_data(const bits_t& tdi, const bits_t& tdo_expected, const bits_t& tdo_mask, const state_t state, const state_t end_state, const uint32_t end_delay) {
     shift_start(state);
-    const auto result = shift(tdi, tdo_expected, tdo_mask, true);
+    const auto result = shift(tdi, tdo_expected, tdo_mask, true, nullptr);
+    shift_end(end_state, end_delay);
+    return result;
+}
+
+std::vector<bool> TAPMachine::shift_data_read(const bits_t& tdi, const state_t state, const state_t end_state, const uint32_t end_delay) {
+    shift_start(state);
+    std::vector<bool> result;
+    shift(tdi, {}, {}, true, &result);
     shift_end(end_state, end_delay);
     return result;
 }

firmware/common/jtag_tap.hpp

@@ -26,6 +26,7 @@
 
 #include <cstdint>
 #include <cstddef>
+#include <vector>
 
 namespace jtag {
 namespace tap {
@@ -113,13 +114,14 @@ class TAPMachine {
     void set_end_dr(const state_t state);
 
     bool shift(const bits_t& tdi, const bool end_tms) {
-        return shift(tdi, {}, {}, end_tms);
+        return shift(tdi, {}, {}, end_tms, nullptr);
     }
 
-    bool shift(const bits_t& tdi, const bits_t& tdo_expected, const bits_t& tdo_mask, const bool end_tms);
+    bool shift(const bits_t& tdi, const bits_t& tdo_expected, const bits_t& tdo_mask, const bool end_tms, std::vector<bool>* from_device);
 
     bool shift_ir(const bits_t& tdi_value, const bits_t& tdo_expected = {}, const bits_t& tdo_mask = {});
     bool shift_dr(const bits_t& tdi_value, const bits_t& tdo_expected = {}, const bits_t& tdo_mask = {});
+    std::vector<bool> shift_dr_read(const bits_t& tdi_value);
 
     void state(const state_t state);
 
@@ -142,6 +144,7 @@ class TAPMachine {
     void shift_end(const state_t end_state, const uint32_t end_delay);
 
     bool shift_data(const bits_t& tdi, const bits_t& tdo_expected, const bits_t& tdo_mask, const state_t state, const state_t end_state, const uint32_t end_delay);
+    std::vector<bool> shift_data_read(const bits_t& tdi, const state_t state, const state_t end_state, const uint32_t end_delay);
 };
 
 } /* namespace tap */