]> git.sur5r.net Git - openocd/blobdiff - src/jtag/jtag.c
Fix fallout from r1818
[openocd] / src / jtag / jtag.c
index c58cde6fe05020525234a7923b73bfe15495478e..f1c6676ea5e99a1ddf53b38663ee4da1028116c1 100644 (file)
@@ -89,7 +89,7 @@ static struct jtag_callback_entry *jtag_callback_queue_tail = NULL;
 
 
 jtag_command_t *jtag_command_queue = NULL;
-jtag_command_t **last_comand_pointer = &jtag_command_queue;
+jtag_command_t **last_command_pointer = &jtag_command_queue;
 static jtag_tap_t *jtag_all_taps = NULL;
 
 enum reset_types jtag_reset_config = RESET_NONE;
@@ -436,9 +436,24 @@ jtag_command_t** jtag_get_last_command_p(void)
 
        return &cmd->next;*/
 
-       return last_comand_pointer;
+       return last_command_pointer;
 }
 
+
+void jtag_queue_command(jtag_command_t * cmd)
+{
+       jtag_command_t **last_cmd;
+
+       last_cmd = jtag_get_last_command_p();
+
+       *last_cmd = cmd;
+
+       (*last_cmd)->next = NULL;
+
+       last_command_pointer = &((*last_cmd)->next);
+}
+
+
 void* cmd_queue_alloc(size_t size)
 {
        cmd_queue_page_t **p_page = &cmd_queue_pages;
@@ -575,28 +590,26 @@ void jtag_add_ir_scan(int num_fields, scan_field_t *fields, tap_state_t state)
 
 int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 {
-       jtag_command_t **last_cmd;
        jtag_tap_t *tap;
        int j;
        int x;
        int nth_tap;
        int scan_size = 0;
 
-       last_cmd = jtag_get_last_command_p();
-
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_SCAN;
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+       
+       jtag_queue_command(cmd);
+       
+       cmd->type = JTAG_SCAN;
 
        /* allocate memory for ir scan command */
-       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
-       (*last_cmd)->cmd.scan->ir_scan = 1;
+       cmd->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+       cmd->cmd.scan->ir_scan = true;
        x = jtag_NumEnabledTaps();
-       (*last_cmd)->cmd.scan->num_fields = x;  /* one field per device */
-       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(x  * sizeof(scan_field_t));
-       (*last_cmd)->cmd.scan->end_state = state;
+       cmd->cmd.scan->num_fields = x;  /* one field per device */
+       cmd->cmd.scan->fields = cmd_queue_alloc(x  * sizeof(scan_field_t));
+       cmd->cmd.scan->end_state = state;
 
        nth_tap = -1;
        tap = NULL;
@@ -613,9 +626,9 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
                assert(nth_tap < x );
 
                scan_size = tap->ir_length;
-               (*last_cmd)->cmd.scan->fields[nth_tap].tap = tap;
-               (*last_cmd)->cmd.scan->fields[nth_tap].num_bits = scan_size;
-               (*last_cmd)->cmd.scan->fields[nth_tap].in_value = NULL; /* do not collect input for tap's in bypass */
+               cmd->cmd.scan->fields[nth_tap].tap = tap;
+               cmd->cmd.scan->fields[nth_tap].num_bits = scan_size;
+               cmd->cmd.scan->fields[nth_tap].in_value = NULL; /* do not collect input for tap's in bypass */
 
                /* search the list */
                for (j = 0; j < num_fields; j++)
@@ -623,8 +636,8 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
                        if (tap == fields[j].tap)
                        {
                                found = 1;
-                               (*last_cmd)->cmd.scan->fields[nth_tap].in_value = fields[j].in_value;
-                               (*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+                               cmd->cmd.scan->fields[nth_tap].in_value = fields[j].in_value;
+                               cmd->cmd.scan->fields[nth_tap].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
 
                                tap->bypass = 0;
                                break;
@@ -634,12 +647,12 @@ int MINIDRIVER(interface_jtag_add_ir_scan)(int num_fields, scan_field_t *fields,
                if (!found)
                {
                        /* if a tap isn't listed, set it to BYPASS */
-                       (*last_cmd)->cmd.scan->fields[nth_tap].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+                       cmd->cmd.scan->fields[nth_tap].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
                        tap->bypass = 1;
                }
 
                /* update device information */
-               buf_cpy((*last_cmd)->cmd.scan->fields[nth_tap].out_value, tap->cur_instr, scan_size);
+               buf_cpy(cmd->cmd.scan->fields[nth_tap].out_value, tap->cur_instr, scan_size);
        }
        assert(nth_tap == (x-1));
 
@@ -659,32 +672,31 @@ void jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, tap_state_t st
 
 int MINIDRIVER(interface_jtag_add_plain_ir_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 {
-       int i;
-       jtag_command_t **last_cmd;
+       /* allocate memory for a new list member */
 
-       last_cmd = jtag_get_last_command_p();
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+       
+       jtag_queue_command(cmd);
 
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_SCAN;
+       cmd->type = JTAG_SCAN;
 
        /* allocate memory for ir scan command */
-       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
-       (*last_cmd)->cmd.scan->ir_scan = 1;
-       (*last_cmd)->cmd.scan->num_fields = num_fields;
-       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
-       (*last_cmd)->cmd.scan->end_state = state;
+       cmd->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+       cmd->cmd.scan->ir_scan = true;
+       cmd->cmd.scan->num_fields = num_fields;
+       cmd->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
+       cmd->cmd.scan->end_state = state;
 
-       for( i = 0 ; i < num_fields ; i++ ){
+       for (int i = 0; i < num_fields; i++)
+       {
                int num_bits = fields[i].num_bits;
                int num_bytes = CEIL(fields[i].num_bits, 8);
-               (*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
-               (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
-               (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
-               (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
+               cmd->cmd.scan->fields[i].tap = fields[i].tap;
+               cmd->cmd.scan->fields[i].num_bits = num_bits;
+               cmd->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
+               cmd->cmd.scan->fields[i].in_value = fields[i].in_value;
        }
+
        return ERROR_OK;
 }
 
@@ -773,7 +785,6 @@ int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields,
        int field_count = 0;
        int scan_size;
 
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
        jtag_tap_t *tap;
 
        /* count devices in bypass */
@@ -790,17 +801,19 @@ int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields,
        }
 
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->next = NULL;
-       (*last_cmd)->type = JTAG_SCAN;
+       
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+       
+       jtag_queue_command(cmd);
+       
+       cmd->type = JTAG_SCAN;
 
        /* allocate memory for dr scan command */
-       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
-       (*last_cmd)->cmd.scan->ir_scan = 0;
-       (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
-       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
-       (*last_cmd)->cmd.scan->end_state = state;
+       cmd->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+       cmd->cmd.scan->ir_scan = false;
+       cmd->cmd.scan->num_fields = num_fields + bypass_devices;
+       cmd->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
+       cmd->cmd.scan->end_state = state;
 
        tap = NULL;
        nth_tap = -1;
@@ -811,7 +824,7 @@ int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields,
                        break;
                }
                int found = 0;
-               (*last_cmd)->cmd.scan->fields[field_count].tap = tap;
+               cmd->cmd.scan->fields[field_count].tap = tap;
 
                for (j = 0; j < num_fields; j++)
                {
@@ -819,9 +832,9 @@ int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields,
                        {
                                found = 1;
                                scan_size = fields[j].num_bits;
-                               (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
-                               (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-                               (*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;
+                               cmd->cmd.scan->fields[field_count].num_bits = scan_size;
+                               cmd->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+                               cmd->cmd.scan->fields[field_count].in_value = fields[j].in_value;
                                field_count++;
                        }
                }
@@ -836,9 +849,9 @@ int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields,
                        }
 #endif
                        /* program the scan field to 1 bit length, and ignore it's value */
-                       (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
-                       (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
-                       (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
+                       cmd->cmd.scan->fields[field_count].num_bits = 1;
+                       cmd->cmd.scan->fields[field_count].out_value = NULL;
+                       cmd->cmd.scan->fields[field_count].in_value = NULL;
                        field_count++;
                }
                else
@@ -853,6 +866,9 @@ int MINIDRIVER(interface_jtag_add_dr_scan)(int num_fields, scan_field_t *fields,
 #endif
                }
        }
+
+       /* field_count represents the true number of fields setup*/
+       cmd->cmd.scan->num_fields = field_count;
        return ERROR_OK;
 }
 
@@ -867,7 +883,6 @@ void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
        int scan_size;
        int bypass_devices = 0;
 
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
        jtag_tap_t *tap;
 
        /* count devices in bypass */
@@ -884,17 +899,18 @@ void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
        }
 
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->next = NULL;
-       (*last_cmd)->type = JTAG_SCAN;
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+
+       jtag_queue_command(cmd);
+
+       cmd->type = JTAG_SCAN;
 
        /* allocate memory for dr scan command */
-       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
-       (*last_cmd)->cmd.scan->ir_scan = 0;
-       (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
-       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
-       (*last_cmd)->cmd.scan->end_state = end_state;
+       cmd->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+       cmd->cmd.scan->ir_scan = false;
+       cmd->cmd.scan->num_fields = num_fields + bypass_devices;
+       cmd->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
+       cmd->cmd.scan->end_state = end_state;
 
        tap = NULL;
        nth_tap = -1;
@@ -904,7 +920,7 @@ void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
                        break;
                }
                nth_tap++;
-               (*last_cmd)->cmd.scan->fields[field_count].tap = tap;
+               cmd->cmd.scan->fields[field_count].tap = tap;
 
                if (tap == target_tap)
                {
@@ -922,9 +938,9 @@ void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
                                u8 out_value[4];
                                scan_size = num_bits[j];
                                buf_set_u32(out_value, 0, scan_size, value[j]);
-                               (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
-                               (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-                               (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
+                               cmd->cmd.scan->fields[field_count].num_bits = scan_size;
+                               cmd->cmd.scan->fields[field_count].out_value = buf_cpy(out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
+                               cmd->cmd.scan->fields[field_count].in_value = NULL;
                                field_count++;
                        }
                } else
@@ -938,9 +954,9 @@ void MINIDRIVER(interface_jtag_add_dr_out)(jtag_tap_t *target_tap,
                        }
 #endif
                        /* program the scan field to 1 bit length, and ignore it's value */
-                       (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
-                       (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
-                       (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
+                       cmd->cmd.scan->fields[field_count].num_bits = 1;
+                       cmd->cmd.scan->fields[field_count].out_value = NULL;
+                       cmd->cmd.scan->fields[field_count].in_value = NULL;
                        field_count++;
                }
        }
@@ -959,30 +975,28 @@ void jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, tap_state_t st
 
 int MINIDRIVER(interface_jtag_add_plain_dr_scan)(int num_fields, scan_field_t *fields, tap_state_t state)
 {
-       int i;
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->next = NULL;
-       (*last_cmd)->type = JTAG_SCAN;
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+
+       jtag_queue_command(cmd);
+
+       cmd->type = JTAG_SCAN;
 
        /* allocate memory for scan command */
-       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
-       (*last_cmd)->cmd.scan->ir_scan = 0;
-       (*last_cmd)->cmd.scan->num_fields = num_fields;
-       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
-       (*last_cmd)->cmd.scan->end_state = state;
+       cmd->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
+       cmd->cmd.scan->ir_scan = false;
+       cmd->cmd.scan->num_fields = num_fields;
+       cmd->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
+       cmd->cmd.scan->end_state = state;
 
-       for (i = 0; i < num_fields; i++)
+       for (int i = 0; i < num_fields; i++)
        {
                int num_bits = fields[i].num_bits;
                int num_bytes = CEIL(fields[i].num_bits, 8);
-               (*last_cmd)->cmd.scan->fields[i].tap = fields[i].tap;
-               (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
-               (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
-               (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
+               cmd->cmd.scan->fields[i].tap = fields[i].tap;
+               cmd->cmd.scan->fields[i].num_bits = num_bits;
+               cmd->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
+               cmd->cmd.scan->fields[i].in_value = fields[i].in_value;
        }
 
        return ERROR_OK;
@@ -1001,16 +1015,16 @@ void jtag_add_tlr(void)
 int MINIDRIVER(interface_jtag_add_tlr)(void)
 {
        tap_state_t state = TAP_RESET;
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
 
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->next = NULL;
-       (*last_cmd)->type = JTAG_STATEMOVE;
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+
+       jtag_queue_command(cmd);
 
-       (*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
-       (*last_cmd)->cmd.statemove->end_state = state;
+       cmd->type = JTAG_STATEMOVE;
+
+       cmd->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
+       cmd->cmd.statemove->end_state = state;
 
        return ERROR_OK;
 }
@@ -1055,38 +1069,35 @@ void jtag_add_pathmove(int num_states, tap_state_t *path)
 
 int MINIDRIVER(interface_jtag_add_pathmove)(int num_states, tap_state_t *path)
 {
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-       int i;
-
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->next = NULL;
-       (*last_cmd)->type = JTAG_PATHMOVE;
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+
+       jtag_queue_command(cmd);
+
+       cmd->type = JTAG_PATHMOVE;
 
-       (*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));
-       (*last_cmd)->cmd.pathmove->num_states = num_states;
-       (*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(tap_state_t) * num_states);
+       cmd->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));
+       cmd->cmd.pathmove->num_states = num_states;
+       cmd->cmd.pathmove->path = cmd_queue_alloc(sizeof(tap_state_t) * num_states);
 
-       for (i = 0; i < num_states; i++)
-               (*last_cmd)->cmd.pathmove->path[i] = path[i];
+       for (int i = 0; i < num_states; i++)
+               cmd->cmd.pathmove->path[i] = path[i];
 
        return ERROR_OK;
 }
 
 int MINIDRIVER(interface_jtag_add_runtest)(int num_cycles, tap_state_t state)
 {
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_RUNTEST;
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+
+       jtag_queue_command(cmd);
 
-       (*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));
-       (*last_cmd)->cmd.runtest->num_cycles = num_cycles;
-       (*last_cmd)->cmd.runtest->end_state = state;
+       cmd->type = JTAG_RUNTEST;
+
+       cmd->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));
+       cmd->cmd.runtest->num_cycles = num_cycles;
+       cmd->cmd.runtest->end_state = state;
 
        return ERROR_OK;
 }
@@ -1106,16 +1117,16 @@ void jtag_add_runtest(int num_cycles, tap_state_t state)
 
 int MINIDRIVER(interface_jtag_add_clocks)( int num_cycles )
 {
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_STABLECLOCKS;
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
+
+       jtag_queue_command(cmd);
+
+       cmd->type = JTAG_STABLECLOCKS;
+
+       cmd->cmd.stableclocks = cmd_queue_alloc(sizeof(stableclocks_command_t));
+       cmd->cmd.stableclocks->num_cycles = num_cycles;
 
-       (*last_cmd)->cmd.stableclocks = cmd_queue_alloc(sizeof(stableclocks_command_t));
-       (*last_cmd)->cmd.stableclocks->num_cycles = num_cycles;
        return ERROR_OK;
 }
 
@@ -1246,17 +1257,16 @@ void jtag_add_reset(int req_tlr_or_trst, int req_srst)
 
 int MINIDRIVER(interface_jtag_add_reset)(int req_trst, int req_srst)
 {
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_RESET;
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 
-       (*last_cmd)->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));
-       (*last_cmd)->cmd.reset->trst = req_trst;
-       (*last_cmd)->cmd.reset->srst = req_srst;
+       jtag_queue_command(cmd);
+
+       cmd->type = JTAG_RESET;
+
+       cmd->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));
+       cmd->cmd.reset->trst = req_trst;
+       cmd->cmd.reset->srst = req_srst;
 
        return ERROR_OK;
 }
@@ -1272,16 +1282,15 @@ void jtag_add_end_state(tap_state_t state)
 
 int MINIDRIVER(interface_jtag_add_sleep)(u32 us)
 {
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-
        /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_SLEEP;
+       jtag_command_t * cmd = cmd_queue_alloc(sizeof(jtag_command_t));
 
-       (*last_cmd)->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));
-       (*last_cmd)->cmd.sleep->us = us;
+       jtag_queue_command(cmd);
+
+       cmd->type = JTAG_SLEEP;
+
+       cmd->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));
+       cmd->cmd.sleep->us = us;
 
        return ERROR_OK;
 }
@@ -1315,7 +1324,7 @@ int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
        int i;
 
        bit_count = jtag_scan_size(cmd);
-       *buffer = malloc(CEIL(bit_count, 8));
+       *buffer = calloc(1,CEIL(bit_count, 8));
 
        bit_count = 0;
 
@@ -1548,7 +1557,7 @@ int interface_jtag_execute_queue(void)
        jtag_callback_queue_tail = NULL;
 
        jtag_command_queue = NULL;
-       last_comand_pointer = &jtag_command_queue;
+       last_command_pointer = &jtag_command_queue;
 
        return retval;
 }
@@ -3298,23 +3307,23 @@ static struct
 
        /* to state: */
        /*      RESET                   IDLE                            DRSHIFT                 DRPAUSE                 IRSHIFT                 IRPAUSE */                      /* from state: */
-       {       B8(1111111,7),  B8(0,1),                        B8(00101,5),    B8(01010,5),            B8(001101,6),   B8(010110,6) },         /* RESET */
-       {       B8(1111111,7),  B8(0,1),                        B8(001,3),              B8(0101,4),             B8(0011,4),     B8(01011,5) },          /* IDLE */
+       {       B8(1111111,7),  B8(0000000,7),  B8(00101,5),            B8(01010,5),    B8(001101,6),   B8(010110,6) },         /* RESET */
+       {       B8(1111111,7),  B8(0000000,7),  B8(001,3),                      B8(0101,4),             B8(0011,4),     B8(01011,5) },          /* IDLE */
        {       B8(1111111,7),  B8(011,3),              B8(00111,5),            B8(01,2),               B8(001111,6),   B8(0101111,7) },                /* DRSHIFT */
-       {       B8(1111111,7),  B8(011,3),              B8(01,2),               B8(0,1),                        B8(001111,6),   B8(0101111,7) },                /* DRPAUSE */
+       {       B8(1111111,7),  B8(011,3),              B8(01,2),               B8(0,1),                B8(001111,6),   B8(0101111,7) },                /* DRPAUSE */
        {       B8(1111111,7),  B8(011,3),              B8(00111,5),            B8(010111,6),   B8(001111,6),   B8(01,2) },                     /* IRSHIFT */
        {       B8(1111111,7),  B8(011,3),              B8(00111,5),            B8(010111,6),   B8(01,2),               B8(0,1) }                       /* IRPAUSE */
 
 #else  /* this is the old table, converted from hex and with the bit_count set to 7 for each combo, like before */
 
        /* to state: */
-       /*      RESET                   IDLE                            DRSHIFT                 DRPAUSE                 IRSHIFT                 IRPAUSE */                      /* from state: */
-       {       B8(1111111,7),  B8(0000000,7),  B8(0010111,7),  B8(0001010,7),  B8(0011011,7),  B8(0010110,7) },                /* RESET */
-       {       B8(1111111,7),  B8(0000000,7),  B8(0100101,7),  B8(0000101,7),  B8(0101011,7),  B8(0001011,7) },                /* IDLE */
-       {       B8(1111111,7),  B8(0110001,7),  B8(0000000,7),  B8(0000001,7),  B8(0001111,7),  B8(0101111,7) },                /* DRSHIFT */
-       {       B8(1111111,7),  B8(0110000,7),  B8(0100000,7),  B8(0010111,7),  B8(0011110,7),  B8(0101111,7) },                /* DRPAUSE */
-       {       B8(1111111,7),  B8(0110001,7),  B8(0000111,7),  B8(0010111,7),  B8(0000000,7),  B8(0000001,7) },                /* IRSHIFT */
-       {       B8(1111111,7),  B8(0110000,7),  B8(0011100,7),  B8(0010111,7),  B8(0100000,7),  B8(0101111,7) }                 /* IRPAUSE */
+       /*      RESET                   IDLE                    DRSHIFT                 DRPAUSE                 IRSHIFT                 IRPAUSE         */              /* from state: */
+       {       B8(1111111,7),  B8(0000000,7),  B8(0010111,7),  B8(0001010,7),  B8(0011011,7),  B8(0010110,7) },        /* RESET */
+       {       B8(1111111,7),  B8(0000000,7),  B8(0100101,7),  B8(0000101,7),  B8(0101011,7),  B8(0001011,7) },        /* IDLE */
+       {       B8(1111111,7),  B8(0110001,7),  B8(0000000,7),  B8(0000001,7),  B8(0001111,7),  B8(0101111,7) },        /* DRSHIFT */
+       {       B8(1111111,7),  B8(0110000,7),  B8(0100000,7),  B8(0010111,7),  B8(0011110,7),  B8(0101111,7) },        /* DRPAUSE */
+       {       B8(1111111,7),  B8(0110001,7),  B8(0000111,7),  B8(0010111,7),  B8(0000000,7),  B8(0000001,7) },        /* IRSHIFT */
+       {       B8(1111111,7),  B8(0110000,7),  B8(0011100,7),  B8(0010111,7),  B8(0011110,7),  B8(0101111,7) },        /* IRPAUSE */
 
 #endif