Merge branch 'main' into nx_support
authorGregory Davill <greg.davill@gmail.com>
Sat, 4 Dec 2021 07:44:52 +0000 (18:14 +1030)
committerGitHub <noreply@github.com>
Sat, 4 Dec 2021 07:44:52 +0000 (18:14 +1030)
1  2 
ecpprog/ecpprog.c
ecpprog/jtag_tap.c

diff --combined ecpprog/ecpprog.c
index 87a7200505e2165c605d9e8dd13c66e60ad47a12,316000da27551e94b6b5d760afbd8d95d1cee096..7d2037000ec0108cec3bb32d3b41ae7bc0a2f8af
  
  static bool verbose = false;
  
 +enum device_type {
 +      TYPE_NONE = 0,
 +      TYPE_ECP5 = 1,
 +      TYPE_NX = 2,
 +};
 +
 +struct device_info {
 +      const char*      name;
 +      uint32_t         id;
 +      enum device_type type;
 +};
 +
 +static struct device_info connected_device = {0};
 +
 +
  // ---------------------------------------------------------
  // FLASH definitions
  // ---------------------------------------------------------
@@@ -182,7 -167,7 +182,7 @@@ static void flash_read_id(
         */
  
        uint8_t data[260] = { FC_JEDECID };
 -      int len = 5; // command + 4 response bytes
 +      int len = 4; // command + 4 response bytes
  
        if (verbose)
                fprintf(stderr, "read flash ID..\n");
        // Write command and read first 4 bytes
        xfer_spi(data, len);
  
 -      if (data[4] == 0xFF)
 -              fprintf(stderr, "Extended Device String Length is 0xFF, "
 -                              "this is likely a read error. Ignoring...\n");
 -      else {
 -              // Read extended JEDEC ID bytes
 -              if (data[4] != 0) {
 -                      len += data[4];
 -                      data[0] = FC_JEDECID;
 -                      xfer_spi(data, len);
 -              }
 -      }
 -      
        fprintf(stderr, "flash ID:");
        for (int i = 1; i < len; i++)
                fprintf(stderr, " 0x%02X", data[i]);
  static void flash_reset()
  {
        uint8_t data[8] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-       xfer_spi(data, 8);
+       // This disables CRM is if it was enabled
+       jtag_go_to_state(STATE_SHIFT_DR);
+       jtag_tap_shift(data, data, 64, true);
+       // This disables QPI if it was enabled
+       jtag_go_to_state(STATE_SHIFT_DR);
+       jtag_tap_shift(data, data, 2, true);
+       // This issues a flash reset command
+       jtag_go_to_state(STATE_SHIFT_DR);
+       jtag_tap_shift(data, data, 8, true);
  }
  
  static uint8_t read_status_1(){
@@@ -438,30 -446,15 +449,30 @@@ static void flash_disable_protection(
  // ECP5 specific JTAG functions
  // ---------------------------------------------------------
  
 -
  static void print_idcode(uint32_t idcode){
 -      for(int i = 0; i < sizeof(ecp_devices)/sizeof(struct ecp_device_id); i++){
 +      connected_device.id = idcode;
 +      
 +      /* ECP5 Parts */
 +      for(int i = 0; i < sizeof(ecp_devices)/sizeof(struct device_id_pair); i++){
                if(idcode == ecp_devices[i].device_id)
                {
 +                      connected_device.name = ecp_devices[i].device_name;
 +                      connected_device.type = TYPE_ECP5;
                        printf("IDCODE: 0x%08x (%s)\n", idcode ,ecp_devices[i].device_name);
                        return;
                }
        }
 +
 +      /* NX Parts */
 +      for(int i = 0; i < sizeof(nx_devices)/sizeof(struct device_id_pair); i++){
 +              if(idcode == nx_devices[i].device_id)
 +              {
 +                      connected_device.name = nx_devices[i].device_name;
 +                      connected_device.type = TYPE_NX;
 +                      printf("IDCODE: 0x%08x (%s)\n", idcode ,nx_devices[i].device_name);
 +                      return;
 +              }
 +      }
        printf("IDCODE: 0x%08x does not match :(\n", idcode);
  }
  
@@@ -485,7 -478,8 +496,7 @@@ static void read_idcode()
        print_idcode(idcode);
  }
  
 -
 -static void print_status_register(uint32_t status){   
 +void print_ecp5_status_register(uint32_t status){     
        printf("ECP5 Status Register: 0x%08x\n", status);
  
        if(verbose){
        }
  }
  
 +void print_nx_status_register(uint64_t status){       
 +      printf("NX Status Register: 0x%016lx\n", status);
 +
 +      if(verbose){
 +              printf("  Transparent Mode:   %s\n",  status & (1 << 0)  ? "Yes" : "No" );
 +              printf("  Config Target:      ");
 +              uint8_t config_target = status & (0b111 << 1) >> 1;
 +              switch (config_target){
 +                      case 0b000: printf("SRAM (0b000)\n"); break;
 +                      case 0b001: printf("EFUSE Normal (0b001)\n"); break;
 +                      case 0b010: printf("EFUSE Pseudo (0b010)\n"); break;
 +                      case 0b011: printf("EFUSE Safe (0b011)\n"); break;
 +                      default: printf("Invalid (%u)\n", config_target); break;
 +              }
 +
 +              printf("  JTAG Active:        %s\n",  status & (1 << 4)  ? "Yes" : "No" );
 +              printf("  PWD Protection:     %s\n",  status & (1 << 5)  ? "Yes" : "No" );
 +              printf("  OTP:                %s\n",  status & (1 << 6)  ? "Yes" : "No" );
 +              printf("  DONE:               %s\n",  status & (1 << 8)  ? "Yes" : "No" );
 +              printf("  ISC Enable:         %s\n",  status & (1 << 9)  ? "Yes" : "No" );
 +              printf("  Write Enable:       %s\n",  status & (1 << 10) ? "Writable" : "Not Writable");
 +              printf("  Read Enable:        %s\n",  status & (1 << 11) ? "Readable" : "Not Readable");
 +              printf("  Busy Flag:          %s\n",  status & (1 << 12) ? "Yes" : "No" );
 +              printf("  Fail Flag:          %s\n",  status & (1 << 13) ? "Yes" : "No" );
 +              printf("  Decrypt Only:       %s\n",  status & (1 << 15) ? "Yes" : "No" );
 +              printf("  PWD Enable:         %s\n",  status & (1 << 16) ? "Yes" : "No" );
 +              printf("  PWD All:            %s\n",  status & (1 << 17) ? "Yes" : "No" );
 +              printf("  CID EN:             %s\n",  status & (1 << 18) ? "Yes" : "No" );
 +              printf("  Encrypt Preamble:   %s\n",  status & (1 << 21) ? "Yes" : "No" );
 +              printf("  Std Preamble:       %s\n",  status & (1 << 22) ? "Yes" : "No" );
 +              printf("  SPIm Fail 1:        %s\n",  status & (1 << 23) ? "Yes" : "No" );
 +              
 +              uint8_t bse_error = (status & (0b1111 << 24)) >> 24;
 +              switch (bse_error){
 +                      case 0b0000: printf("  BSE Error Code:     No Error (0b000)\n"); break;
 +                      case 0b0001: printf("  BSE Error Code:     ID Error (0b001)\n"); break;
 +                      case 0b0010: printf("  BSE Error Code:     CMD Error - illegal command (0b010)\n"); break;
 +                      case 0b0011: printf("  BSE Error Code:     CRC Error (0b011)\n"); break;
 +                      case 0b0100: printf("  BSE Error Code:     PRMB Error - preamble error (0b100)\n"); break;
 +                      case 0b0101: printf("  BSE Error Code:     ABRT Error - configuration aborted by the user (0b101)\n"); break;
 +                      case 0b0110: printf("  BSE Error Code:     OVFL Error - data overflow error (0b110)\n"); break;
 +                      case 0b0111: printf("  BSE Error Code:     SDM Error - bitstream pass the size of SRAM array (0b111)\n"); break;
 +                      case 0b1000: printf("  BSE Error Code:     Authentication Error (0b1000)\n"); break;
 +                      case 0b1001: printf("  BSE Error Code:     Authentication Setup Error (0b1001)\n"); break;
 +                      case 0b1010: printf("  BSE Error Code:     Bitstream Engine Timeout Error (0b1010) \n"); break;
 +              }
 +
 +              printf("  Execution Error:    %s\n",  status & (1 << 28) ? "Yes" : "No" );
 +              printf("  ID Error:           %s\n",  status & (1 << 29) ? "Yes" : "No" );
 +              printf("  Invalid Command:    %s\n",  status & (1 << 30) ? "Yes" : "No" );
 +              printf("  WDT Busy:           %s\n",  status & (1 << 31) ? "Yes" : "No" );
 +              printf("  Dry Run DONE:       %s\n",  status & (1UL << 33) ? "Yes" : "No" );
 +              
 +              uint8_t bse_error1 = (status & (0b1111UL << 34)) >> 34;
 +              switch (bse_error1){
 +                      case 0b0000: printf("  BSE Error 1 Code: (Previous Bitstream)  No Error (0b000)\n"); break;
 +                      case 0b0001: printf("  BSE Error 1 Code: (Previous Bitstream)  ID Error (0b001)\n"); break;
 +                      case 0b0010: printf("  BSE Error 1 Code: (Previous Bitstream)  CMD Error - illegal command (0b010)\n"); break;
 +                      case 0b0011: printf("  BSE Error 1 Code: (Previous Bitstream)  CRC Error (0b011)\n"); break;
 +                      case 0b0100: printf("  BSE Error 1 Code: (Previous Bitstream)  PRMB Error - preamble error (0b100)\n"); break;
 +                      case 0b0101: printf("  BSE Error 1 Code: (Previous Bitstream)  ABRT Error - configuration aborted by the user (0b101)\n"); break;
 +                      case 0b0110: printf("  BSE Error 1 Code: (Previous Bitstream)  OVFL Error - data overflow error (0b110)\n"); break;
 +                      case 0b0111: printf("  BSE Error 1 Code: (Previous Bitstream)  SDM Error - bitstream pass the size of SRAM array (0b111)\n"); break;
 +                      case 0b1000: printf("  BSE Error 1 Code: (Previous Bitstream)  Authentication Error (0b1000)\n"); break;
 +                      case 0b1001: printf("  BSE Error 1 Code: (Previous Bitstream)  Authentication Setup Error (0b1001)\n"); break;
 +                      case 0b1010: printf("  BSE Error 1 Code: (Previous Bitstream)  Bitstream Engine Timeout Error (0b1010) \n"); break;
 +              }
 +
 +              printf("  Bypass Mode:        %s\n",  status & (1UL << 38) ? "Yes" : "No" );
 +              printf("  Flow Through Mode:  %s\n",  status & (1UL << 39) ? "Yes" : "No" );
 +              printf("  SFDP Timeout:       %s\n",  status & (1UL << 42) ? "Yes" : "No" );
 +              printf("  Key Destroy Pass:   %s\n",  status & (1UL << 43) ? "Yes" : "No" );
 +              printf("  INITN:              %s\n",  status & (1UL << 44) ? "Yes" : "No" );
 +              printf("  I3C Parity Error 2: %s\n",  status & (1UL << 45) ? "Yes" : "No" );
 +              printf("  Init Bus ID Error:  %s\n",  status & (1UL << 46) ? "Yes" : "No" );
 +              printf("  I3C Parity Error 1: %s\n",  status & (1UL << 47) ? "Yes" : "No" );
 +              
 +              uint8_t auth_mode = (status & (0b11UL << 48)) >> 48;
 +              switch (auth_mode){
 +                      case 0b00: printf("  Authentication Mode:  No Auth (0b00)\n"); break;
 +                      case 0b01: printf("  Authentication Mode:  ECDSA (0b01)\n"); break;
 +                      case 0b10: printf("  Authentication Mode:  HMAC (0b10)\n"); break;
 +                      case 0b11: printf("  Authentication Mode:  No Auth (0b11)\n"); break;
 +              }
 +
 +              printf("  Authentication Done: %s\n",  status & (1UL << 50) ? "Yes" : "No" );
 +              printf("  Dry Run Authentication Done: %s\n",  status & (1UL << 51) ? "Yes" : "No" );
 +              printf("  JTAG Locked:         %s\n",  status & (1UL << 52) ? "Yes" : "No" );
 +              printf("  SSPI Locked:         %s\n",  status & (1UL << 53) ? "Yes" : "No" );
 +              printf("  I2C/I3C Locked:      %s\n",  status & (1UL << 54) ? "Yes" : "No" );
 +              printf("  PUB Read Lock:       %s\n",  status & (1UL << 55) ? "Yes" : "No" );
 +              printf("  PUB Write Lock:      %s\n",  status & (1UL << 56) ? "Yes" : "No" );
 +              printf("  FEA Read Lock:       %s\n",  status & (1UL << 57) ? "Yes" : "No" );
 +              printf("  FEA Write Lock:      %s\n",  status & (1UL << 58) ? "Yes" : "No" );
 +              printf("  AES Read Lock:       %s\n",  status & (1UL << 59) ? "Yes" : "No" );
 +              printf("  AES Write Lock:      %s\n",  status & (1UL << 60) ? "Yes" : "No" );
 +              printf("  PWD Read Lock:       %s\n",  status & (1UL << 61) ? "Yes" : "No" );
 +              printf("  PWD Write Lock:      %s\n",  status & (1UL << 62) ? "Yes" : "No" );
 +              printf("  Global Lock:         %s\n",  status & (1UL << 63) ? "Yes" : "No" );
 +      }
 +
 +}
  
  static void read_status_register(){
  
 -      uint8_t data[4] = {LSC_READ_STATUS};
 +      uint8_t data[8] = {LSC_READ_STATUS};
  
        jtag_go_to_state(STATE_SHIFT_IR);
        jtag_tap_shift(data, data, 8, true);
  
        data[0] = 0;
        jtag_go_to_state(STATE_SHIFT_DR);
 -      jtag_tap_shift(data, data, 32, true);
        //jtag_go_to_state(STATE_PAUSE_DR);
 -
 -      uint32_t status = 0;
        
 -      /* Format the IDCODE into a 32bit value */
 -      for(int i = 0; i< 4; i++)
 -              status = data[i] << 24 | status >> 8;
 +      if(connected_device.type == TYPE_ECP5){
 +              jtag_tap_shift(data, data, 32, true);
 +              uint32_t status = 0;
 +              
 +              /* Format the status into a 32bit value */
 +              for(int i = 0; i< 4; i++)
 +                      status = data[i] << 24 | status >> 8;
 +
 +              print_ecp5_status_register(status);
 +      }else if(connected_device.type == TYPE_NX){
 +
 +              jtag_tap_shift(data, data, 64, true);
 +
 +              uint64_t status = 0;
 +              
 +              /* Format the status into a 32bit value */
 +              for(int i = 0; i< 8; i++)
 +                      status = (uint64_t)data[i] << 56 | status >> 8;
  
 -      print_status_register(status);
 +              print_nx_status_register(status);
 +      }
  }
  
  
@@@ -715,7 -594,7 +726,7 @@@ void ecp_jtag_cmd8(uint8_t cmd, uint8_
  
  static void help(const char *progname)
  {
 -      fprintf(stderr, "Simple programming tool for FTDI-based Lattice ECP JTAG programmers.\n");
 +      fprintf(stderr, "Simple programming tool for Lattice ECP5/NX using FTDI-based JTAG programmers.\n");
        fprintf(stderr, "Usage: %s [-b|-n|-c] <input file>\n", progname);
        fprintf(stderr, "       %s -r|-R<bytes> <output file>\n", progname);
        fprintf(stderr, "       %s -S <input file>\n", progname);
        fprintf(stderr, "  -o <offset in bytes>  start address for read/write [default: 0]\n");
        fprintf(stderr, "                          (append 'k' to the argument for size in kilobytes,\n");
        fprintf(stderr, "                          or 'M' for size in megabytes)\n");
 -      fprintf(stderr, "  -s                    slow SPI (50 kHz instead of 6 MHz)\n");
 +      fprintf(stderr, "  -k <divider>          divider for SPI clock [default: 1]\n");
 +      fprintf(stderr, "                          clock speed is 6MHz/divider");
 +      fprintf(stderr, "  -s                    slow SPI. (50 kHz instead of 6 MHz)\n");
 +      fprintf(stderr, "                          Equivalent to -k 30\n");
        fprintf(stderr, "  -v                    verbose output\n");
        fprintf(stderr, "  -i [4,32,64]          select erase block size [default: 64k]\n");
+       fprintf(stderr, "  -a                    reinitialize the device after any operation\n");
        fprintf(stderr, "\n");
        fprintf(stderr, "Mode of operation:\n");
        fprintf(stderr, "  [default]             write file contents to flash, then verify\n");
@@@ -789,8 -666,8 +801,9 @@@ int main(int argc, char **argv
        int erase_block_size = 64;
        int erase_size = 0;
        int rw_offset = 0;
 +      int clkdiv = 1;
  
+       bool reinitialize = false;
        bool read_mode = false;
        bool check_mode = false;
        bool erase_mode = false;
        bool dont_erase = false;
        bool prog_sram = false;
        bool test_mode = false;
 -      bool slow_clock = false;
        bool disable_protect = false;
        bool disable_verify = false;
        const char *filename = NULL;
        /* Decode command line parameters */
        int opt;
        char *endptr;
 -      while ((opt = getopt_long(argc, argv, "d:i:I:rR:e:o:cabnStvspX", long_options, NULL)) != -1) {
 +      while ((opt = getopt_long(argc, argv, "d:i:I:rR:e:o:k:scbnStvpX", long_options, NULL)) != -1) {
                switch (opt) {
                case 'd': /* device string */
                        devstr = optarg;
                                return EXIT_FAILURE;
                        }
                        break;
 +              case 'k': /* set clock div */
 +                      clkdiv = strtol(optarg, &endptr, 0);
 +                        if (clkdiv < 1 || clkdiv > 65536) {
 +                              fprintf(stderr, "%s: clock divider must be in range 1-65536 `%s' is not a valid divider\n", my_name, optarg);
 +                              return EXIT_FAILURE;
 +                        }
 +                      break;
 +              case 's': /* use slow SPI clock */
 +                      clkdiv = 30;
 +                      break;
                case 'c': /* do not write just check */
                        check_mode = true;
                        break;
+               case 'a': /* reinitialize ECP5 device reading new configuration */
+                       reinitialize = true;
+                       break;
                case 'b': /* bulk erase before writing */
                        bulk_erase = true;
                        break;
                case 'v': /* provide verbose output */
                        verbose = true;
                        break;
 -              case 's': /* use slow SPI clock */
 -                      slow_clock = true;
 -                      break;
                case 'p': /* disable flash protect before erase/write */
                        disable_protect = true;
                        break;
                   named pipe, or contrarily, the standard input may be an
                   ordinary file. */
  
 -              if (!prog_sram && !check_mode) {
 +              if (!prog_sram) {
                        if (fseek(f, 0L, SEEK_END) != -1) {
                                file_size = ftell(f);
                                if (file_size == -1) {
        // ---------------------------------------------------------
  
        fprintf(stderr, "init..\n");
 -      jtag_init(ifnum, devstr, slow_clock);
 +      jtag_init(ifnum, devstr, clkdiv);
  
        read_idcode();
        read_status_register();
        if (test_mode)
        {
                /* Reset ECP5 to release SPI interface */
 -              ecp_jtag_cmd(ISC_ENABLE);
 -              ecp_jtag_cmd(ISC_ERASE);
 +              ecp_jtag_cmd8(ISC_ENABLE,0);
 +              usleep(10000);
 +              ecp_jtag_cmd8(ISC_ERASE,0);
 +              usleep(10000);
                ecp_jtag_cmd(ISC_DISABLE);
  
                /* Put device into SPI bypass mode */
                                uint8_t buffer[4096];
  
                                /* Show progress */
 -                              fprintf(stderr, "\r\033[0Kreading..    %04u/%04lu", addr, file_size);
 +                              fprintf(stderr, "\r\033[0Kreading..    %04u/%04u", addr + 4096, read_size);
  
                                flash_continue_read(buffer, 4096);
                                fwrite(buffer, read_size - addr > 4096 ? 4096 : read_size - addr, 1, f);
                }
        }
  
+       if (reinitialize) {
+               fprintf(stderr, "rebooting ECP5...\n");
+               ecp_jtag_cmd(LSC_REFRESH);
+       }
        if (f != NULL && f != stdin && f != stdout)
                fclose(f);
  
diff --combined ecpprog/jtag_tap.c
index f791bdc981206aeeb43c3be4763ad7c24bd20df7,538d675bca676383ff337e368defa691c3552e79..a6d508d78c0a9a77e7eb7ae2b56bfcdafbc75870
@@@ -130,12 -130,12 +130,12 @@@ void jtag_deinit()
  /**
   * Performs any start-of-day tasks necessary to talk JTAG to our FPGA.
   */
 -void jtag_init(int ifnum, const char *devstr, bool slow_clock)
 +void jtag_init(int ifnum, const char *devstr, int clkdiv)
  {
 -      mpsse_init(ifnum, devstr, slow_clock);
 +      mpsse_init(ifnum, devstr, clkdiv);
  
        jtag_set_current_state(STATE_TEST_LOGIC_RESET);
-     jtag_go_to_state(STATE_TEST_LOGIC_RESET);
+       jtag_go_to_state(STATE_TEST_LOGIC_RESET);
  }
  
  uint8_t data[32*1024];
@@@ -146,9 -146,9 +146,9 @@@ extern struct ftdi_context mpsse_ftdic
  
  static inline void jtag_pulse_clock_and_read_tdo(bool tms, bool tdi)
  {
-     *ptr++ = MC_DATA_TMS | MC_DATA_IN | MC_DATA_LSB | MC_DATA_BITS | MC_DATA_OCN | MC_DATA_ICN;
 -      *ptr++ = MC_DATA_TMS | MC_DATA_IN | MC_DATA_LSB | MC_DATA_BITS | MC_DATA_OCN;
++  *ptr++ = MC_DATA_TMS | MC_DATA_IN | MC_DATA_LSB | MC_DATA_BITS | MC_DATA_OCN | MC_DATA_ICN;
        *ptr++ =  0;        
-     *ptr++ = (tdi ? 0x80 : 0) | (tms ? 0x01 : 0);
+       *ptr++ = (tdi ? 0x80 : 0) | (tms ? 0x01 : 0);
        rx_cnt++;
  }
  
@@@ -168,9 -168,9 +168,9 @@@ static void _jtag_tap_shift
        for (uint32_t i = 0; i < byte_count; ++i) {
                uint8_t byte_out = input_data[i];
                for (int j = 0; j < 8 && bit_count-- > 0; ++j) {
-             bool tms = false;
+                       bool tms = false;
                        if (bit_count == 0 && must_end) {
-                 tms = true;
+                               tms = true;
                                jtag_state_ack(1);
                        }
                        jtag_pulse_clock_and_read_tdo(tms, byte_out & 1);
@@@ -200,7 -200,10 +200,7 @@@ static void jtag_shift_bytes
        }
        //printf("jtag_shift_bytes(0x%08x,0x%08x,%u,%s);\n",input_data, output_data, data_bits, must_end ? "true" : "false");
        uint32_t byte_count = data_bits / 8;
 -
 -
 -
 -      data[0] = MC_DATA_OUT | MC_DATA_IN | MC_DATA_LSB | MC_DATA_OCN;
 +      data[0] = MC_DATA_OUT | MC_DATA_IN | MC_DATA_LSB | MC_DATA_OCN | MC_DATA_ICN;
        data[1] = (byte_count - 1); 
        data[2] = (byte_count - 1) >> 8;        
        memcpy(data + 3, input_data, byte_count);
  }
  
  #ifndef MIN
- #define MIN(a,b) (a < b) ? a : b
+       #define MIN(a,b) ((a) < (b)) ? (a) : (b)
  #endif
  
  void jtag_tap_shift(
  {
        /* if 'must_end' the send last byte seperately 
         * This way we toggle TMS on the last clock cycle */
-       if(must_end)
-               data_bits -= 8;
-       
-       uint32_t data_bits_sent = 0;
-       if(data_bits){
-               while(data_bits_sent != data_bits){
-                       uint32_t _data_bits = MIN(4096 + 2048, data_bits - data_bits_sent);
-                       jtag_shift_bytes(
-                               input_data + data_bits_sent/8,
-                               output_data + data_bits_sent/8,
-                               _data_bits,
-                               false
-                       );
-                       data_bits_sent += _data_bits;
-               }
+       while (data_bits >= (8 + must_end)) {
+               uint32_t _data_bits = MIN(4096 + 2048, data_bits - must_end) & ~7U;
+               jtag_shift_bytes(
+                       input_data,
+                       output_data,
+                       _data_bits,
+                       false
+               );
+               data_bits   -= _data_bits;
+               input_data  += _data_bits / 8;
+               output_data += _data_bits / 8;
        }
  
-       /* Send our last byte */
-       if(must_end){
+       if (data_bits > 0) {
                _jtag_tap_shift(
-                       input_data + data_bits_sent/8,
-                       output_data + data_bits_sent/8,
-                       8,
+                       input_data,
+                       output_data,
+                       data_bits,
                        must_end
                );
        }
@@@ -270,7 -269,7 +266,7 @@@ void jtag_go_to_state(unsigned state
                }
  
                uint8_t data[3] = { 
 -                      MC_DATA_TMS | MC_DATA_LSB | MC_DATA_BITS,
 +                      MC_DATA_TMS | MC_DATA_LSB | MC_DATA_BITS | MC_DATA_ICN,
                        5 - 1,
                        0b11111
                };