- fixed arm926 cp15 command bug (thanks to Vincent Palatin for this patch)

[openocd.git] / src / jtag / gw16012.c

diff --git a/src/jtag/gw16012.c b/src/jtag/gw16012.c
index 6210a52c48821527901685a16ced649d48c766e5..1f1f6086833c3f62f35c2cef088ddf454ff681cb 100644 (file)
--- a/src/jtag/gw16012.c
+++ b/src/jtag/gw16012.c
@@ -25,6 +25,10 @@
 
 #include "jtag.h"
 
+#if 1
+#define _DEBUG_GW16012_IO_
+#endif
+
 /* system includes */
 
 /* system includes */
@@ -123,6 +127,10 @@ void gw16012_data(u8 value)
 {
        value = (value & 0x7f) | gw16012_msb;
        gw16012_msb ^= 0x80; /* toggle MSB */
+
+#ifdef _DEBUG_GW16012_IO_
+       DEBUG("%2.2x", value);
+#endif
        
        #if PARPORT_USE_PPDEV == 1
                ioctl(device_handle, PPWDATA, &value);
@@ -141,6 +149,10 @@ void gw16012_control(u8 value)
        {
                gw16012_control_value = value;
 
+#ifdef _DEBUG_GW16012_IO_
+               DEBUG("%2.2x", gw16012_control_value);
+#endif
+
                #if PARPORT_USE_PPDEV == 1
                        ioctl(device_handle, PPWCONTROL, &gw16012_control_value);
                #else
@@ -160,6 +172,10 @@ void gw16012_input(u8 *value)
        #else
                *value = inb(gw16012_port + 1);
        #endif
+
+#ifdef _DEBUG_GW16012_IO_
+       DEBUG("%2.2x", *value);
+#endif
 }
 
 /* (1) assert or (0) deassert reset lines */
@@ -211,6 +227,37 @@ void gw16012_state_move(void)
        cur_state = end_state;
 }
 
+void gw16012_path_move(pathmove_command_t *cmd)
+{
+       int num_states = cmd->num_states;
+       int state_count;
+
+       state_count = 0;
+       while (num_states)
+       {
+               gw16012_control(0x0); /* single-bit mode */
+               if (tap_transitions[cur_state].low == cmd->path[state_count])
+               {
+                       gw16012_data(0x0); /* TCK cycle with TMS low */
+               }
+               else if (tap_transitions[cur_state].high == cmd->path[state_count])
+               {
+                       gw16012_data(0x2); /* TCK cycle with TMS high */
+               }
+               else
+               {
+                       ERROR("BUG: %s -> %s isn't a valid TAP transition", tap_state_strings[cur_state], tap_state_strings[cmd->path[state_count]]);
+                       exit(-1);
+               }
+               
+               cur_state = cmd->path[state_count];
+               state_count++;
+               num_states--;
+       }
+       
+       end_state = cur_state;
+}
+
 void gw16012_runtest(int num_cycles)
 {
        enum tap_state saved_end_state = end_state;
@@ -241,13 +288,17 @@ void gw16012_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
        enum tap_state saved_end_state = end_state;
        u8 scan_out, scan_in;
 
-       if (ir_scan)
-               gw16012_end_state(TAP_SI);
-       else
-               gw16012_end_state(TAP_SD);
+       /* only if we're not already in the correct Shift state */
+       if (!((!ir_scan && (cur_state == TAP_SD)) || (ir_scan && (cur_state == TAP_SI))))
+       {
+               if (ir_scan)
+                       gw16012_end_state(TAP_SI);
+               else
+                       gw16012_end_state(TAP_SD);
 
-       gw16012_state_move();
-       gw16012_end_state(saved_end_state);
+               gw16012_state_move();
+               gw16012_end_state(saved_end_state);
+       }
 
        while (type == SCAN_OUT && ((bits_left - 1) > 7))
        {
@@ -262,6 +313,9 @@ void gw16012_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
        while (bits_left-- > 0)
        {
                u8 tms = 0;
+               
+               scan_out = buf_get_u32(buffer, bit_count, 1);
+               
                if (bits_left == 0) /* last bit */
                {
                        if ((ir_scan && (end_state == TAP_SI))
@@ -274,14 +328,15 @@ void gw16012_scan(int ir_scan, enum scan_type type, u8 *buffer, int scan_size)
                                tms = 2;
                        }
                }
-               
-               scan_out = buf_get_u32(buffer, bit_count, 1);
+
                gw16012_data(scan_out | tms);
+
                if (type != SCAN_OUT)
                {
                        gw16012_input(&scan_in);
                        buf_set_u32(buffer, bit_count, 1, ((scan_in & 0x08) >> 3));
-               }
+               }               
+
                bit_count++;
        }
 
@@ -343,6 +398,12 @@ int gw16012_execute_queue(void)
                                        gw16012_end_state(cmd->cmd.statemove->end_state);
                                gw16012_state_move();
                                break;
+                       case JTAG_PATHMOVE:
+#ifdef _DEBUG_JTAG_IO_
+                               DEBUG("pathmove: %i states, end in %i", cmd->cmd.pathmove->num_states, cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]);
+#endif
+                               gw16012_path_move(cmd->cmd.pathmove);
+                               break;
                        case JTAG_SCAN:
                                if (cmd->cmd.scan->end_state != -1)
                                        gw16012_end_state(cmd->cmd.scan->end_state);
@@ -360,7 +421,7 @@ int gw16012_execute_queue(void)
                                break;
                        case JTAG_SLEEP:
 #ifdef _DEBUG_JTAG_IO_
-                               DEBUG("sleep", cmd->cmd.sleep->us);
+                               DEBUG("sleep %i", cmd->cmd.sleep->us);
 #endif
                                jtag_sleep(cmd->cmd.sleep->us);
                                break;
@@ -466,7 +527,7 @@ int gw16012_init(void)
                WARNING("No gw16012 port specified, using default '0x378' (LPT1)");
        }
        
-       DEBUG("requesting privileges for parallel port 0x%x...", gw16012_port);
+       DEBUG("requesting privileges for parallel port 0x%lx...", gw16012_port);
 #if PARPORT_USE_GIVEIO == 1
        if (gw16012_get_giveio_access() != 0)
 #else /* PARPORT_USE_GIVEIO */
@@ -477,6 +538,13 @@ int gw16012_init(void)
                return ERROR_JTAG_INIT_FAILED;
        }
        DEBUG("...privileges granted");
+
+       /* make sure parallel port is in right mode (clear tristate and interrupt */
+#ifdef __FreeBSD__
+       outb(gw16012_port + 2, 0x0);
+#else
+       outb(0x0, gw16012_port + 2);
+#endif
 #endif /* PARPORT_USE_PPDEV */
        
        gw16012_input(&status_port);