zy1000: fix arm11 optimisation

copy & paste error + added FIFO throttling to work around
lockup bug in FPGA.

The arm11 optimisation was introduced post v0.4.0, so this
is not a regression compared to previous release.

Signed-off-by: Øyvind Harboe <oyvind.harboe@zylin.com>

diff --git a/src/jtag/zy1000/zy1000.c b/src/jtag/zy1000/zy1000.c
index b50e96a8a00c2e9178be5111a77ff14f54a6baec..013d8658a7ec6bd55fe02f293d2900005b48f639 100644 (file)
--- a/src/jtag/zy1000/zy1000.c
+++ b/src/jtag/zy1000/zy1000.c
@@ -878,101 +878,100 @@ void embeddedice_write_dcc(struct jtag_tap *tap, int reg_addr, uint8_t *buffer,
 
 int arm11_run_instr_data_to_core_noack_inner(struct jtag_tap * tap, uint32_t opcode, uint32_t * data, size_t count)
 {
-#if 0
-       int arm11_run_instr_data_to_core_noack_inner_default(struct jtag_tap * tap, uint32_t opcode, uint32_t * data, size_t count);
-       return arm11_run_instr_data_to_core_noack_inner_default(tap, opcode, data, count);
-#else
-       static const int bits[] = {32, 2};
-       uint32_t values[] = {0, 0};
-
-       /* FIX!!!!!! the target_write_memory() API started this nasty problem
-        * with unaligned uint32_t * pointers... */
-       const uint8_t *t = (const uint8_t *)data;
-
-
        /* bypass bits before and after */
        int pre_bits;
        int post_bits;
        jtag_pre_post_bits(tap, &pre_bits, &post_bits);
-
-       bool found = false;
-       struct jtag_tap *cur_tap, *nextTap;
-       for (cur_tap = jtag_tap_next_enabled(NULL); cur_tap!= NULL; cur_tap = nextTap)
-       {
-               nextTap = jtag_tap_next_enabled(cur_tap);
-               if (cur_tap == tap)
-               {
-                       found = true;
-               } else
-               {
-                       if (found)
-                       {
-                               post_bits++;
-                       } else
-                       {
-                               pre_bits++;
-                       }
-               }
-       }
-
        post_bits+=2;
 
-
-       while (--count > 0)
+       if ((pre_bits > 32) || (post_bits > 32))
        {
-               shiftValueInner(TAP_DRSHIFT, TAP_DRSHIFT, pre_bits, 0);
-
-               uint32_t value;
-               value = *t++;
-               value |= (*t++<<8);
-               value |= (*t++<<16);
-               value |= (*t++<<24);
+               int arm11_run_instr_data_to_core_noack_inner_default(struct jtag_tap * tap, uint32_t opcode, uint32_t * data, size_t count);
+               return arm11_run_instr_data_to_core_noack_inner_default(tap, opcode, data, count);
+       } else
+       {
+               static const int bits[] = {32, 2};
+               uint32_t values[] = {0, 0};
 
-               shiftValueInner(TAP_DRSHIFT, TAP_DRSHIFT, 32, value);
-               /* minimum 2 bits */
-               shiftValueInner(TAP_DRSHIFT, TAP_DRPAUSE, post_bits, 0);
+               /* FIX!!!!!! the target_write_memory() API started this nasty problem
+                * with unaligned uint32_t * pointers... */
+               const uint8_t *t = (const uint8_t *)data;
 
+               while (--count > 0)
+               {
 #if 1
-               /* copy & paste from arm11_dbgtap.c */
-               //TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
+                       /* Danger! This code doesn't update cmd_queue_cur_state, so
+                        * invoking jtag_add_pathmove() before jtag_add_dr_out() after
+                        * this loop would fail!
+                        */
+                       shiftValueInner(TAP_DRSHIFT, TAP_DRSHIFT, pre_bits, 0);
 
-               waitIdle();
-               ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 1);
-               ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 1);
-               ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
-               ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
-               ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
-               ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 1);
-               ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
-               ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
-               /* we don't have to wait for the queue to empty here. waitIdle();        */
-               ZY1000_POKE(ZY1000_JTAG_BASE + 0x20, TAP_DRSHIFT);
+                       uint32_t value;
+                       value = *t++;
+                       value |= (*t++<<8);
+                       value |= (*t++<<16);
+                       value |= (*t++<<24);
+
+                       shiftValueInner(TAP_DRSHIFT, TAP_DRSHIFT, 32, value);
+                       /* minimum 2 bits */
+                       shiftValueInner(TAP_DRSHIFT, TAP_DRPAUSE, post_bits, 0);
+
+                       /* copy & paste from arm11_dbgtap.c */
+                       //TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
+                       /* KLUDGE! we have to flush the fifo or the Nios CPU locks up.
+                        * This is probably a bug in the Avalon bus(cross clocking bridge?)
+                        * or in the jtag registers module.
+                        */
+                       waitIdle();
+                       ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 1);
+                       ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 1);
+                       ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
+                       ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
+                       ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
+                       ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 1);
+                       ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
+                       ZY1000_POKE(ZY1000_JTAG_BASE + 0x28, 0);
+                       /* we don't have to wait for the queue to empty here */
+                       ZY1000_POKE(ZY1000_JTAG_BASE + 0x20, TAP_DRSHIFT);
+                       waitIdle();
 #else
-               static const tap_state_t arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay[] =
-               {
-                       TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
-               };
-
-               jtag_add_pathmove(ARRAY_SIZE(arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay),
-                       arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay);
+                       static const tap_state_t arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay[] =
+                       {
+                               TAP_DREXIT2, TAP_DRUPDATE, TAP_IDLE, TAP_IDLE, TAP_IDLE, TAP_DRSELECT, TAP_DRCAPTURE, TAP_DRSHIFT
+                       };
+
+                       values[0] = *t++;
+                       values[0] |= (*t++<<8);
+                       values[0] |= (*t++<<16);
+                       values[0] |= (*t++<<24);
+
+                       jtag_add_dr_out(tap,
+                               2,
+                               bits,
+                               values,
+                               TAP_IDLE);
+
+                       jtag_add_pathmove(ARRAY_SIZE(arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay),
+                               arm11_MOVE_DRPAUSE_IDLE_DRPAUSE_with_delay);
 #endif
-       }
+               }
 
-       values[0] = *t++;
-       values[0] |= (*t++<<8);
-       values[0] |= (*t++<<16);
-       values[0] |= (*t++<<24);
+               values[0] = *t++;
+               values[0] |= (*t++<<8);
+               values[0] |= (*t++<<16);
+               values[0] |= (*t++<<24);
 
-       /* This will happen on the last iteration updating the current tap state
-        * so we don't have to track it during the common code path */
-       jtag_add_dr_out(tap,
-               2,
-               bits,
-               values,
-               TAP_IDLE);
+               /* This will happen on the last iteration updating cmd_queue_cur_state
+                * so we don't have to track it during the common code path
+                */
+               jtag_add_dr_out(tap,
+                       2,
+                       bits,
+                       values,
+                       TAP_IDLE);
 
-       return jtag_execute_queue();
-#endif
+               return jtag_execute_queue();
+       }
 }