Changed armv7m and cortexm3 to use nev arm_adi_v5 instead of cortex_swjdp.
[fw/openocd] / src / jtag / jtag.c
index c06244a0065ce70dfb41667e8c9d2e37c7e18af1..408f88fa5c351642d8f59ef08faabd0583283404 100644 (file)
@@ -4,10 +4,10 @@
  *                                                                         *
  *   Copyright (C) 2007,2008 Ã˜yvind Harboe                                 *
  *   oyvind.harboe@zylin.com                                               *
- *
- *   Copyright (C) 2009 SoftPLC Corporation
- *      http://softplc.com
- *   dick@softplc.com
+ *                                                                         *
+ *   Copyright (C) 2009 SoftPLC Corporation                                *
+ *      http://softplc.com                                                    *
+ *   dick@softplc.com                                                      *
  *                                                                         *
  *   This program is free software; you can redistribute it and/or modify  *
  *   it under the terms of the GNU General Public License as published by  *
@@ -44,7 +44,6 @@
 */
 int jtag_error=ERROR_OK;
 
-
 typedef struct cmd_queue_page_s
 {
        void *address;
@@ -81,8 +80,8 @@ tap_state_t cmd_queue_cur_state = TAP_RESET;
 int jtag_verify_capture_ir = 1;
 
 /* how long the OpenOCD should wait before attempting JTAG communication after reset lines deasserted (in ms) */
-int jtag_nsrst_delay = 0; /* default to no nSRST delay */
-int jtag_ntrst_delay = 0; /* default to no nTRST delay */
+static int jtag_nsrst_delay = 0; /* default to no nSRST delay */
+static int jtag_ntrst_delay = 0; /* default to no nTRST delay */
 
 /* maximum number of JTAG devices expected in the chain
  */
@@ -155,6 +154,10 @@ static int hasKHz = 0;
        extern jtag_interface_t rlink_interface;
 #endif
 
+#if BUILD_ARMJTAGEW == 1
+       extern jtag_interface_t armjtagew_interface;
+#endif
+
 jtag_interface_t *jtag_interfaces[] = {
 #if BUILD_ECOSBOARD == 1
        &zy1000_interface,
@@ -197,6 +200,9 @@ jtag_interface_t *jtag_interfaces[] = {
 #endif
 #if BUILD_RLINK == 1
        &rlink_interface,
+#endif
+#if BUILD_ARMJTAGEW == 1
+       &armjtagew_interface,
 #endif
        NULL,
 };
@@ -204,35 +210,35 @@ jtag_interface_t *jtag_interfaces[] = {
 jtag_interface_t *jtag = NULL;
 
 /* configuration */
-jtag_interface_t *jtag_interface = NULL;
+static jtag_interface_t *jtag_interface = NULL;
 int jtag_speed = 0;
 
 /* forward declarations */
-void jtag_add_pathmove(int num_states, tap_state_t *path);
-void jtag_add_runtest(int num_cycles, tap_state_t endstate);
-void jtag_add_end_state(tap_state_t endstate);
-void jtag_add_sleep(u32 us);
-int jtag_execute_queue(void);
-
+//void jtag_add_pathmove(int num_states, tap_state_t *path);
+//void jtag_add_runtest(int num_cycles, tap_state_t endstate);
+//void jtag_add_end_state(tap_state_t endstate);
+//void jtag_add_sleep(u32 us);
+//int jtag_execute_queue(void);
+static tap_state_t tap_state_by_name(const char *name);
 
 /* jtag commands */
-int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 
-int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 
-int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *argv);
+static int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *argv);
 
-int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
+static int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
 
 jtag_tap_t *jtag_AllTaps(void)
 {
@@ -269,14 +275,13 @@ int jtag_NumEnabledTaps(void)
        return n;
 }
 
-
 jtag_tap_t *jtag_TapByString( const char *s )
 {
        jtag_tap_t *t;
        char *cp;
 
        t = jtag_AllTaps();
-       // try name first
+       /* try name first */
        while(t){
                if( 0 == strcmp( t->dotted_name, s ) ){
                        break;
@@ -284,7 +289,7 @@ jtag_tap_t *jtag_TapByString( const char *s )
                        t = t->next_tap;
                }
        }
-       // backup plan is by number
+       /* backup plan is by number */
        if( t == NULL ){
                /* ok - is "s" a number? */
                int n;
@@ -505,7 +510,7 @@ static void jtag_prelude(tap_state_t state)
 {
        jtag_prelude1();
 
-       if (state != -1)
+       if (state != TAP_INVALID)
                jtag_add_end_state(state);
 
        cmd_queue_cur_state = cmd_queue_end_state;
@@ -531,7 +536,6 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
        int nth_tap;
        int scan_size = 0;
 
-
        last_cmd = jtag_get_last_command_p();
 
        /* allocate memory for a new list member */
@@ -553,7 +557,7 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
        for(;;){
                int found = 0;
 
-               // do this here so it is not forgotten
+               /* do this here so it is not forgotten */
                tap = jtag_NextEnabledTap(tap);
                if( tap == NULL ){
                        break;
@@ -957,8 +961,8 @@ void jtag_add_pathmove(int num_states, tap_state_t *path)
                        LOG_ERROR("BUG: TAP_RESET is not a valid state for pathmove sequences");
                        exit(-1);
                }
-               if ( tap_state_transition(cur_state, TRUE)  != path[i]
-                 && tap_state_transition(cur_state, FALSE) != path[i])
+               if ( tap_state_transition(cur_state, true)  != path[i]
+                 && tap_state_transition(cur_state, false) != path[i])
                {
                        LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_name(cur_state), tap_state_name(path[i]));
                        exit(-1);
@@ -1503,7 +1507,7 @@ int jtag_examine_chain(void)
                return ERROR_JTAG_INIT_FAILED;
        }
 
-       // point at the 1st tap
+       /* point at the 1st tap */
        tap = jtag_NextEnabledTap(NULL);
        if( tap == NULL ){
                LOG_ERROR("JTAG: No taps enabled?");
@@ -1707,9 +1711,7 @@ static Jim_Nvp nvp_config_opts[] = {
        { .name = NULL,          .value = -1 }
 };
 
-static int
-jtag_tap_configure_cmd( Jim_GetOptInfo *goi,
-               jtag_tap_t * tap)
+static int jtag_tap_configure_cmd( Jim_GetOptInfo *goi, jtag_tap_t * tap)
 {
        Jim_Nvp *n;
        Jim_Obj *o;
@@ -1756,7 +1758,7 @@ jtag_tap_configure_cmd( Jim_GetOptInfo *goi,
                                        jteap = tap->event_action;
                                        /* replace existing? */
                                        while (jteap) {
-                                               if (jteap->event == n->value) {
+                                               if (jteap->event == (enum jtag_tap_event)n->value) {
                                                        break;
                                                }
                                                jteap = jteap->next;
@@ -2303,7 +2305,7 @@ static int default_srst_asserted(int *srst_asserted)
        return ERROR_OK;
 }
 
-int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        int i;
        int retval;
@@ -2366,7 +2368,7 @@ int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char
        return ERROR_JTAG_INVALID_INTERFACE;
 }
 
-int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        int e;
        char buf[1024];
@@ -2389,9 +2391,9 @@ int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, cha
                                   args[0],
                                   args[1],
                                   args[2] );
-       command_print( cmd_ctx, "Example: STM32 has 2 taps, the cortexM3(len4) + boundryscan(len5)");
-       command_print( cmd_ctx, "jtag newtap stm32 cortexm3  ....., thus creating the tap: \"stm32.cortexm3\"");
-       command_print( cmd_ctx, "jtag newtap stm32 boundry  ....., and the tap: \"stm32.boundery\"");
+       command_print( cmd_ctx, "Example: STM32 has 2 taps, the cortexM3(len4) + boundaryscan(len5)");
+       command_print( cmd_ctx, "jtag newtap stm32 cortexm3 ....., thus creating the tap: \"stm32.cortexm3\"");
+       command_print( cmd_ctx, "jtag newtap stm32 boundary ....., and the tap: \"stm32.boundary\"");
        command_print( cmd_ctx, "And then refer to the taps by the dotted name.");
 
        newargs[0] = Jim_NewStringObj( interp, "jtag", -1   );
@@ -2427,7 +2429,7 @@ int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, cha
        return e;
 }
 
-int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        jtag_tap_t *tap;
 
@@ -2464,7 +2466,7 @@ int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char
        return ERROR_OK;
 }
 
-int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        if (argc < 1)
                return ERROR_COMMAND_SYNTAX_ERROR;
@@ -2540,7 +2542,7 @@ int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, ch
        return ERROR_OK;
 }
 
-int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        if (argc < 1)
        {
@@ -2555,7 +2557,7 @@ int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd
        return ERROR_OK;
 }
 
-int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        if (argc < 1)
        {
@@ -2570,7 +2572,7 @@ int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd
        return ERROR_OK;
 }
 
-int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        int retval=ERROR_OK;
 
@@ -2598,7 +2600,7 @@ int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char
        return retval;
 }
 
-int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        int retval=ERROR_OK;
        LOG_DEBUG("handle jtag khz");
@@ -2648,7 +2650,7 @@ int handle_jtag_khz_command(struct command_context_s *cmd_ctx, char *cmd, char *
 
 }
 
-int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        tap_state_t state;
 
@@ -2658,21 +2660,20 @@ int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char *
        }
        else
        {
-               for (state = 0; state < 16; state++)
-               {
-                       if (strcmp(args[0], tap_state_name(state)) == 0)
-                       {
-                               jtag_add_end_state(state);
-                               jtag_execute_queue();
-                       }
+               state = tap_state_by_name( args[0] );
+               if( state < 0 ){
+                       command_print( cmd_ctx, "Invalid state name: %s\n", args[0] );
+                       return ERROR_COMMAND_SYNTAX_ERROR;
                }
+               jtag_add_end_state(state);
+               jtag_execute_queue();
        }
        command_print(cmd_ctx, "current endstate: %s", tap_state_name(cmd_queue_end_state));
 
        return ERROR_OK;
 }
 
-int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        int trst = -1;
        int srst = -1;
@@ -2709,31 +2710,59 @@ int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char
        return ERROR_OK;
 }
 
-int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        if (argc < 1)
        {
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
 
-       jtag_add_runtest(strtol(args[0], NULL, 0), -1);
+       jtag_add_runtest(strtol(args[0], NULL, 0), TAP_INVALID);
        jtag_execute_queue();
 
        return ERROR_OK;
 
 }
 
-int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        int i;
        scan_field_t *fields;
        jtag_tap_t *tap;
+       tap_state_t endstate;
 
        if ((argc < 2) || (argc % 2))
        {
                return ERROR_COMMAND_SYNTAX_ERROR;
        }
 
+       /* optional "-endstate" */
+       /*          "statename" */
+       /* at the end of the arguments. */
+       /* assume none. */
+       endstate = TAP_INVALID;
+       if( argc >= 4 ){
+               /* have at least one pair of numbers. */
+               /* is last pair the magic text? */
+               if( 0 == strcmp( "-endstate", args[ argc - 2 ] ) ){
+                       const char *cpA;
+                       const char *cpS;
+                       cpA = args[ argc-1 ];
+                       for( endstate = 0 ; endstate < TAP_NUM_STATES ; endstate++ ){
+                               cpS = tap_state_name( endstate );
+                               if( 0 == strcmp( cpA, cpS ) ){
+                                       break;
+                               }
+                       }
+                       if( endstate >= TAP_NUM_STATES ){
+                               return ERROR_COMMAND_SYNTAX_ERROR;
+                       } else {
+                               /* found - remove the last 2 args */
+                               argc -= 2;
+                       }
+               }
+       }
+
        fields = malloc(sizeof(scan_field_t) * argc / 2);
 
        for (i = 0; i < argc / 2; i++)
@@ -2755,7 +2784,11 @@ int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **a
                fields[i].in_handler_priv = NULL;
        }
 
-       jtag_add_ir_scan(argc / 2, fields, -1);
+       jtag_add_ir_scan(argc / 2, fields, TAP_INVALID);
+       /* did we have an endstate? */
+       if (endstate != TAP_INVALID)
+               jtag_add_end_state(endstate);
+
        jtag_execute_queue();
 
        for (i = 0; i < argc / 2; i++)
@@ -2766,7 +2799,7 @@ int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **a
        return ERROR_OK;
 }
 
-int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
+static int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
 {
        int retval;
        scan_field_t *fields;
@@ -2774,11 +2807,16 @@ int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
        int field_count = 0;
        int i, e;
        jtag_tap_t *tap;
+       tap_state_t endstate;
 
        /* args[1] = device
         * args[2] = num_bits
         * args[3] = hex string
         * ... repeat num bits and hex string ...
+        *
+        * .. optionally:
+       *     args[N-2] = "-endstate"
+        *     args[N-1] = statename
         */
        if ((argc < 4) || ((argc % 2)!=0))
        {
@@ -2786,14 +2824,54 @@ int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
                return JIM_ERR;
        }
 
+       /* assume no endstate */
+       endstate = TAP_INVALID;
+       /* validate arguments as numbers */
+       e = JIM_OK;
        for (i = 2; i < argc; i+=2)
        {
                long bits;
+               const char *cp;
 
                e = Jim_GetLong(interp, args[i], &bits);
-               if (e != JIM_OK)
+               /* If valid - try next arg */
+               if( e == JIM_OK ){
+                       continue;
+               }
+
+               /* Not valid.. are we at the end? */
+               if ( ((i+2) != argc) ){
+                       /* nope, then error */
                        return e;
-       }
+               }
+
+               /* it could be: "-endstate FOO" */
+
+               /* get arg as a string. */
+               cp = Jim_GetString( args[i], NULL );
+               /* is it the magic? */
+               if( 0 == strcmp( "-endstate", cp ) ){
+                       /* is the statename valid? */
+                       cp = Jim_GetString( args[i+1], NULL );
+
+                       /* see if it is a valid state name */
+                       endstate = tap_state_by_name(cp);
+                       if( endstate < 0 ){
+                               /* update the error message */
+                               Jim_SetResult_sprintf(interp,"endstate: %s invalid", cp );
+                       } else {
+                               /* valid - so clear the error */
+                               e = JIM_OK;
+                               /* and remove the last 2 args */
+                               argc -= 2;
+                       }
+               }
+
+               /* Still an error? */
+               if( e != JIM_OK ){
+                       return e; /* too bad */
+               }
+       } /* validate args */
 
        tap = jtag_TapByJimObj( interp, args[1] );
        if( tap == NULL ){
@@ -2823,7 +2901,11 @@ int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
                fields[field_count++].in_handler_priv = NULL;
        }
 
-       jtag_add_dr_scan(num_fields, fields, -1);
+       jtag_add_dr_scan(num_fields, fields, TAP_INVALID);
+       /* did we get an end state? */
+       if (endstate != TAP_INVALID)
+               jtag_add_end_state(endstate);
+
        retval = jtag_execute_queue();
        if (retval != ERROR_OK)
        {
@@ -2854,7 +2936,7 @@ int Jim_Command_drscan(Jim_Interp *interp, int argc, Jim_Obj *const *args)
        return JIM_OK;
 }
 
-int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
+static int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 {
        if (argc == 1)
        {
@@ -2964,6 +3046,7 @@ tap_state_t tap_get_end_state()
        return end_state_follower;
 }
 
+
 int tap_move_ndx( tap_state_t astate )
 {
        /* given a stable state, return the index into the tms_seqs[] array within tap_get_tms_path() */
@@ -3006,7 +3089,7 @@ int tap_get_tms_path( tap_state_t from, tap_state_t to )
         *
         * DRSHIFT->DRSHIFT and IRSHIFT->IRSHIFT have to be caught in interface specific code
         */
-       const static u8 tms_seqs[6][6] =
+       static const u8 tms_seqs[6][6] =
        {
                /* value clocked to TMS to move from one of six stable states to another */
 
@@ -3035,9 +3118,10 @@ int tap_get_tms_path( tap_state_t from, tap_state_t to )
        return tms_seqs[tap_move_ndx(from)][tap_move_ndx(to)];
 }
 
-int tap_is_state_stable(tap_state_t astate)
+
+bool tap_is_state_stable(tap_state_t astate)
 {
-       int is_stable;
+       bool is_stable;
 
        /*      A switch() is used because it is symbol dependent
                (not value dependent like an array), and can also check bounds.
@@ -3050,16 +3134,16 @@ int tap_is_state_stable(tap_state_t astate)
        case TAP_DRPAUSE:
        case TAP_IRSHIFT:
        case TAP_IRPAUSE:
-               is_stable = 1;
+               is_stable = true;
                break;
        default:
-               is_stable = 0;
+               is_stable = false;
        }
 
        return is_stable;
 }
 
-tap_state_t tap_state_transition(tap_state_t cur_state, int tms)
+tap_state_t tap_state_transition(tap_state_t cur_state, bool tms)
 {
        tap_state_t new_state;
 
@@ -3164,7 +3248,7 @@ const char* tap_state_name(tap_state_t state)
        switch( state )
        {
        case TAP_RESET:         ret = "RESET";                  break;
-       case TAP_IDLE:          ret = "IDLE";                   break;
+       case TAP_IDLE:          ret = "RUN/IDLE";                       break;
        case TAP_DRSELECT:      ret = "DRSELECT";               break;
        case TAP_DRCAPTURE: ret = "DRCAPTURE";          break;
        case TAP_DRSHIFT:       ret = "DRSHIFT";                        break;
@@ -3185,4 +3269,103 @@ const char* tap_state_name(tap_state_t state)
        return ret;
 }
 
+static tap_state_t tap_state_by_name( const char *name )
+{
+       tap_state_t x;
+
+       for( x = 0 ; x < TAP_NUM_STATES ; x++ ){
+               /* be nice to the human */
+               if( 0 == strcasecmp( name, tap_state_name(x) ) ){
+                       return x;
+               }
+       }
+       /* not found */
+       return TAP_INVALID;
+}
+
+#ifdef _DEBUG_JTAG_IO_
+
+#define JTAG_DEBUG_STATE_APPEND(buf, len, bit) \
+               do { buf[len] = bit ? '1' : '0'; } while(0)
+#define JTAG_DEBUG_STATE_PRINT(a, b, astr, bstr) \
+               DEBUG_JTAG_IO("TAP/SM: %9s -> %5s\tTMS: %s\tTDI: %s", \
+                       tap_state_name(a), tap_state_name(b), astr, bstr)
+
+tap_state_t jtag_debug_state_machine(const void *tms_buf, const void *tdi_buf,
+               unsigned tap_bits, tap_state_t next_state)
+{
+       const u8 *tms_buffer;
+       const u8 *tdi_buffer;
+       unsigned tap_bytes;
+       unsigned cur_byte;
+       unsigned cur_bit;
+
+       unsigned tap_out_bits;
+       char tms_str[33];
+       char tdi_str[33];
+
+       tap_state_t last_state;
+
+       // set startstate (and possibly last, if tap_bits == 0) 
+       last_state = next_state;
+       DEBUG_JTAG_IO("TAP/SM: START state: %s", tap_state_name(next_state));
+
+       tms_buffer = (const u8 *)tms_buf;
+       tdi_buffer = (const u8 *)tdi_buf;
+
+       tap_bytes = TAP_SCAN_BYTES(tap_bits);
+       DEBUG_JTAG_IO("TAP/SM: TMS bits: %u (bytes: %u)", tap_bits, tap_bytes);
+
+       tap_out_bits = 0;
+       for(cur_byte = 0; cur_byte < tap_bytes; cur_byte++)
+       {
+               for(cur_bit = 0; cur_bit < 8; cur_bit++)
+               {
+                       // make sure we do not run off the end of the buffers
+                       unsigned tap_bit = cur_byte * 8 + cur_bit;
+                       if (tap_bit == tap_bits)
+                               break;
+
+                       // check and save TMS bit
+                       tap_bit = !!(tms_buffer[cur_byte] & (1 << cur_bit));
+                       JTAG_DEBUG_STATE_APPEND(tms_str, tap_out_bits, tap_bit);
+
+                       // use TMS bit to find the next TAP state
+                       next_state = tap_state_transition(last_state, tap_bit);
+
+                       // check and store TDI bit
+                       tap_bit = !!(tdi_buffer[cur_byte] & (1 << cur_bit));
+                       JTAG_DEBUG_STATE_APPEND(tdi_str, tap_out_bits, tap_bit);
+
+                       // increment TAP bits
+                       tap_out_bits++;
+
+                       // Only show TDO bits on state transitions, or
+                       // after some number of bits in the same state.
+                       if ((next_state == last_state) && (tap_out_bits < 32))
+                               continue;
+
+                       // terminate strings and display state transition
+                       tms_str[tap_out_bits] = tdi_str[tap_out_bits] = 0;
+                       JTAG_DEBUG_STATE_PRINT(last_state, next_state, tms_str, tdi_str);
+
+                       // reset state
+                       last_state = next_state;
+                       tap_out_bits = 0;
+               }
+       }
+
+       if (tap_out_bits)
+       {
+               // terminate strings and display state transition
+               tms_str[tap_out_bits] = tdi_str[tap_out_bits] = 0;
+               JTAG_DEBUG_STATE_PRINT(last_state, next_state, tms_str, tdi_str);
+       }
+
+       DEBUG_JTAG_IO("TAP/SM: FINAL state: %s", tap_state_name(next_state));
+
+       return next_state;
+}
+#endif // _DEBUG_JTAG_IO_
+
 /*-----</Cable Helper API>--------------------------------------*/