SimonQian <simonqian@SimonQian.com>:

[openocd.git] / src / svf / svf.c

diff --git a/src/svf/svf.c b/src/svf/svf.c
index 3cad4c7e96e0970725345755652b75064273553a..caaacabafdb30eb6c1b8b87c2727fc10af20649c 100644 (file)
--- a/src/svf/svf.c
+++ b/src/svf/svf.c
@@ -218,6 +218,54 @@ void svf_free_xxd_para(svf_xxr_para_t *para)
        }
 }
 
+unsigned svf_get_mask_u32(int bitlen)
+{
+       u32 bitmask;
+
+       if (bitlen < 0)
+       {
+               bitmask = 0;
+       }
+       else if (bitlen >= 32)
+       {
+               bitmask = 0xFFFFFFFF;
+       }
+       else
+       {
+               bitmask = (1 << bitlen) - 1;
+       }
+
+       return bitmask;
+}
+
+static const char* tap_state_svf_name(tap_state_t state)
+{
+       const char* ret;
+
+       switch( state )
+       {
+       case TAP_RESET:         ret = "RESET";          break;
+       case TAP_IDLE:          ret = "IDLE";           break;
+       case TAP_DRSELECT:      ret = "DRSELECT";       break;
+       case TAP_DRCAPTURE: ret = "DRCAPTURE";  break;
+       case TAP_DRSHIFT:       ret = "DRSHIFT";        break;
+       case TAP_DREXIT1:       ret = "DREXIT1";        break;
+       case TAP_DRPAUSE:       ret = "DRPAUSE";        break;
+       case TAP_DREXIT2:       ret = "DREXIT2";        break;
+       case TAP_DRUPDATE:      ret = "DRUPDATE";       break;
+       case TAP_IRSELECT:      ret = "IRSELECT";       break;
+       case TAP_IRCAPTURE: ret = "IRCAPTURE";  break;
+       case TAP_IRSHIFT:       ret = "IRSHIFT";        break;
+       case TAP_IREXIT1:       ret = "IREXIT1";        break;
+       case TAP_IRPAUSE:       ret = "IRPAUSE";        break;
+       case TAP_IREXIT2:       ret = "IREXIT2";        break;
+       case TAP_IRUPDATE:      ret = "IRUPDATE";       break;
+       default:                        ret = "???";            break;
+       }
+
+       return ret;
+}
+
 static int handle_svf_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
 #define SVF_NUM_OF_OPTIONS                     1
@@ -305,7 +353,7 @@ static int handle_svf_command(struct command_context_s *cmd_ctx, char *cmd, char
        memcpy(&svf_para, &svf_para_init, sizeof(svf_para));
        for (i = 0; i < (int)dimof(svf_tap_state_name); i++)
        {
-               svf_tap_state_name[i] = (char *)tap_state_name(i);
+               svf_tap_state_name[i] = (char *)tap_state_svf_name(i);
        }
        // TAP_RESET
        jtag_add_tlr();
@@ -628,32 +676,29 @@ static int svf_copy_hexstring_to_binary(char *str, u8 **bin, int orig_bit_len, i
 
 static int svf_check_tdo(void)
 {
-       int i, j, byte_len, index;
+       int i, len, index;
 
        for (i = 0; i < svf_check_tdo_para_index; i++)
        {
-               if (svf_check_tdo_para[i].enabled)
+               index = svf_check_tdo_para[i].buffer_offset;
+               len = svf_check_tdo_para[i].bit_len;
+               if ((svf_check_tdo_para[i].enabled) 
+                       && buf_cmp_mask(&svf_tdi_buffer[index], &svf_tdo_buffer[index], &svf_mask_buffer[index], len))
                {
-                       byte_len = (svf_check_tdo_para[i].bit_len + 7) >> 3;
-                       index = svf_check_tdo_para[i].buffer_offset;
-                       for (j = 0; j < byte_len; j++)
-                       {
-                               if ((svf_tdi_buffer[index + j] & svf_mask_buffer[index + j]) != svf_tdo_buffer[index + j])
-                               {
-                                       unsigned bitmask = (1 << svf_check_tdo_para[i].bit_len) - 1;
-                                       unsigned received, expected, tapmask;
-                                       memcpy(&received, svf_tdi_buffer + index, sizeof(unsigned));
-                                       memcpy(&expected, svf_tdo_buffer + index, sizeof(unsigned));
-                                       memcpy(&tapmask, svf_mask_buffer + index, sizeof(unsigned));
-                                       LOG_ERROR("tdo check error at line %d, "
-                                               "read = 0x%X, want = 0x%X, mask = 0x%X",
-                                                               svf_check_tdo_para[i].line_num,
-                                                               received & bitmask,
-                                                               expected & bitmask,
-                                                               tapmask & bitmask);
-                                       return ERROR_FAIL;
-                               }
-                       }
+                       unsigned bitmask;
+                       unsigned received, expected, tapmask;
+                       bitmask = svf_get_mask_u32(svf_check_tdo_para[i].bit_len);
+
+                       memcpy(&received, svf_tdi_buffer + index, sizeof(unsigned));
+                       memcpy(&expected, svf_tdo_buffer + index, sizeof(unsigned));
+                       memcpy(&tapmask, svf_mask_buffer + index, sizeof(unsigned));
+                       LOG_ERROR("tdo check error at line %d", 
+                                         svf_check_tdo_para[i].line_num);
+                       LOG_ERROR("read = 0x%X, want = 0x%X, mask = 0x%X", 
+                                         received & bitmask, 
+                                         expected & bitmask, 
+                                         tapmask & bitmask);
+                       return ERROR_FAIL;
                }
        }
        svf_check_tdo_para_index = 0;
@@ -859,7 +904,7 @@ static int svf_run_command(struct command_context_s *cmd_ctx, char *cmd_str)
                                LOG_ERROR("fail to parse hex value");
                                return ERROR_FAIL;
                        }
-                       LOG_DEBUG("\t%s = 0x%X", argus[i], (**(int**)pbuffer_tmp) & ((1 << (xxr_para_tmp->len)) - 1));
+                       LOG_DEBUG("\t%s = 0x%X", argus[i], (**(int**)pbuffer_tmp) & svf_get_mask_u32(xxr_para_tmp->len));
                }
                // If a command changes the length of the last scan of the same type and the MASK parameter is absent,
                // the mask pattern used is all cares
@@ -1372,7 +1417,7 @@ static int svf_run_command(struct command_context_s *cmd_ctx, char *cmd_str)
                                int read_value;
                                memcpy(&read_value, svf_tdi_buffer, sizeof(int));
                                // in debug mode, data is from index 0
-                               int read_mask = (1 << (svf_check_tdo_para[0].bit_len)) - 1;
+                               int read_mask = svf_get_mask_u32(svf_check_tdo_para[0].bit_len);
                                LOG_DEBUG("\tTDO read = 0x%X", read_value & read_mask);
                        }
                }