Index: sandbox/JamPlayerUSB/jbistub.c
===================================================================
--- sandbox/JamPlayerUSB/jbistub.c	(revision 109)
+++ sandbox/JamPlayerUSB/jbistub.c	(revision 110)
@@ -163,16 +163,46 @@
 /******************************************************************/
 
-void usb_blaster_buf_flush()
+void usb_blaster_wait(int count, int tms)
 {
     DWORD cnt;
-    int len;
-  
-    if(usb_buf_pos > 0)
-    {
-        len = usb_buf_pos >> 3;
-        usb_buf[0] = len | 0x80;
-
-        usb_blaster_buf_write(usb_buf, len + 1, &cnt);
-        usb_buf_pos = 0;
+
+    BYTE buf[64];
+
+    int i, len, extra;
+
+    printf("usb_blaster_wait (count tms)  -> (%d %d)\n", count, tms);
+    fflush(stdout);
+
+    if (count <= 0) return;
+    
+    memset(buf, 0, 64);
+
+    extra = (count & 7);
+
+    buf[0] = 0x0C | (tms ? 0x02 : 0);
+    usb_blaster_buf_write(buf, 1, &cnt);
+
+    while (count > 0)
+    {
+        if (count > 504)
+        {
+            len = 63;
+            count -= 504;
+        }
+        else
+        {
+            len = count >> 3;
+            count = 0;
+        }
+ 
+        if (len > 0)
+        {
+            buf[0] = len | 0x80;
+            usb_blaster_buf_write(buf, len + 1, &cnt);            
+        }
+    }
+    for (i = 0; i < extra; i++)
+    {
+        jbi_jtag_io(tms, 0, 0);
     }
 }
@@ -180,18 +210,76 @@
 /******************************************************************/
 
-void usb_blaster_buf_fill(int tdi)
-{
-    int index, shift;
-
-    if (usb_buf_pos >= 504) usb_blaster_buf_flush();
-
-    index = (usb_buf_pos >> 3) + 1;
-    shift = (usb_buf_pos & 7);
-
-    if (shift == 0) usb_buf[index] = 0;
-
-    usb_buf[index] |= ((tdi&0x01) << shift);
-
-    ++usb_buf_pos;
+void usb_blaster_scan(int count, unsigned char *tdi, unsigned char *tdo)
+{
+    DWORD cnt;
+
+    BYTE buf[64];
+    
+    unsigned char extra_bits;
+   
+    int i, len, pos, extra, tdo_bit, read_tdo;
+
+    read_tdo = (tdo != NULL);
+
+    printf("usb_blaster_scan (count read)  -> (%d %d)\n", count, read_tdo);
+    fflush(stdout);
+
+    if (count <= 0) return;
+
+    pos = 0;
+    extra = (count & 7);
+
+    if (extra == 0)
+    {
+        count -= 8;
+        extra = 8;
+    }
+    
+    extra_bits = tdi[count >> 3];
+        
+    while (count > 0)
+    {
+        if (count > 504)
+        {
+            len = 63;
+            count -= 504;
+        }
+        else
+        {
+            len = count >> 3;
+            count = 0;
+        }
+ 
+        if (len > 0)
+        {
+            buf[0] = len | 0x80 | (read_tdo ? 0x40 : 0);
+            memcpy(buf + 1, tdi + pos, len);
+            usb_blaster_buf_write(buf, len + 1, &cnt);            
+            if (read_tdo)
+            {
+                usb_blaster_buf_read(tdo + pos, len, &cnt);
+            }
+            pos += len;
+        }
+    }
+    for (i = 0; i < extra; i++)
+    {
+        tdo_bit = jbi_jtag_io(
+            (i == extra - 1),
+            extra_bits & (1 << (i & 7)),
+            read_tdo);
+
+        if (read_tdo)
+        {
+            if (tdo_bit)
+            {
+                tdo[pos] |= (1 << (i & 7));
+            }
+            else
+            {
+                tdo[pos] &= ~(unsigned int) (1 << (i & 7));
+            }
+        } 
+    }
 }
 
@@ -205,7 +293,7 @@
 */
 
-int jbi_jtag_io(int tms, int tdi, int read_tdo, int flag)
-{
-    BYTE buf[2];
+int jbi_jtag_io(int tms, int tdi, int read_tdo)
+{
+    BYTE buf[3];
     DWORD count;
 
@@ -219,31 +307,15 @@
     }
 
-    if (flag == 0 || read_tdo)
-    {
-/*
-        printf("(tms tdi read flag)  -> (%d %d %d %d)\n", tms, tdi, read_tdo, flag);
-        fflush(stdout);
-*/
-        usb_blaster_buf_flush();
-
-        data = (tdi ? 0x10 : 0) | (tms ? 0x02 : 0);
-
-        buf[0] = data | 0x0C;
-        buf[1] = data | 0x01 | 0x0C | (read_tdo ? 0x40 : 0);
-        usb_blaster_buf_write(buf, 2, &count);
-
-        if (read_tdo)
-        {
-            usb_blaster_buf_read(buf, 1, &count);
-            tdo = buf[0]&0x01;
-/*
-            printf("(tdo)  -> (%d)\n", tdo);
-            fflush(stdout);
-*/
-        }
-    }
-    else
-    {
-        usb_blaster_buf_fill(tdi ? 1 : 0);
+    data = (tdi ? 0x10 : 0) | (tms ? 0x02 : 0);
+
+    buf[0] = data | 0x0C | (read_tdo ? 0x40 : 0);
+    buf[1] = data | 0x0C | 0x01;
+    buf[2] = data | 0x0C;
+    usb_blaster_buf_write(buf, 3, &count);
+
+    if (read_tdo)
+    {
+        usb_blaster_buf_read(buf, 1, &count);
+        tdo = buf[0]&0x01;
     }
 
