Bitpacking added

firmware/main.c

@@ -59,18 +59,23 @@ uchar   usbFunctionRead(uchar *data, uchar len)
     return len;
 }
 
+void procByte(const uchar b) {
+	PORTC = (PORTC & 0xF0) | (b & 0x03); // ....i0dd
+	PORTC |= 0x04; // .....1.. TCK
+	if ((b & 4) == 4) {
+		readValue = (PINC & 0x08) >> 3;
+	}
+}
+
 /* usbFunctionWrite() is called when the host sends a chunk of data to the
  * device. For more information see the documentation in usbdrv/usbdrv.h.
  */
 uchar   usbFunctionWrite(uchar *data, uchar len)
 {
 	uchar i;
 	for (i = 0; i < len; i++) {
-		PORTC = (PORTC & 0xF0) | (data[i] & 0x03); // ....i0dd
-		PORTC |= 0x04; // .....1.. TCK
-		if ((data[i] & 4) == 4) {
-			readValue = (PINC & 0x08) >> 3;
-		}
+		if (data[i] & 0x80) procByte(data[i] >> 4);
+		procByte(data[i]);
 	}
     return 1;
 }

ioparport.cpp

@@ -66,7 +66,7 @@ IOParport::IOParport(const char *device_name) : IOBase()
 		return;
 	}
 
-	txonlybuf[0] = 0;
+	memset(txonlybuf, 0, sizeof(txonlybuf));
 	txonlybufpos = 1;
 
 	error=false;
@@ -100,10 +100,17 @@ bool IOParport::txrx(bool tms, bool tdi)
 #define TOB txonlybuf[txonlybufpos]
 
 void IOParport::buftx(bool tms, bool tdi, char startval) {
-	TOB = startval;
-	if (tdi) TOB |= 2;
-	if (tms) TOB |= 1;
-	if (++txonlybufpos == sizeof(txonlybuf)) flushtob();
+	if (TOB & 8) {
+		TOB <<= 4;
+		TOB |= startval;
+		if (tdi) TOB |= 2;
+		if (tms) TOB |= 1;
+		if (++txonlybufpos == sizeof(txonlybuf)) flushtob();
+	} else {
+		TOB = startval | 8;
+		if (tdi) TOB |= 2;
+		if (tms) TOB |= 1;
+	}
 }
 
 void IOParport::tx(bool tms, bool tdi)
@@ -118,6 +125,7 @@ void IOParport::flushtob() {
 		fprintf(stderr, "error writing data: %s\n", usbErrorMessage(err));
 		error = true;
 	}
+	memset(txonlybuf, 0, sizeof(txonlybuf));
 	txonlybufpos = 1;
 	fprintf(stderr, ".");
 }