--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/jtag.c	Sat Feb 21 23:06:50 2009 +0800
@@ -0,0 +1,80 @@
+#include <stdio.h>
+#include <util/delay.h>
+#include "jtag.h"
+#include "avriotools.h"
+
+/* It is supposed to work at 1Mbps */
+#define CLK_DELAY() do { _delay_ms(0.0004); } while(0)
+
+void jtag_init(void) {
+    pin_mode(&JTAG_PORT, JTAG_TCK, PM_OUTPUT);
+    pin_mode(&JTAG_PORT, JTAG_TMS, PM_OUTPUT);
+    pin_mode(&JTAG_PORT, JTAG_TDI, PM_OUTPUT);
+    pin_mode(&JTAG_PORT, JTAG_TDO, PM_INPUT);
+    pin_lo(JTAG_PORT, JTAG_TCK);
+    pin_lo(JTAG_PORT, JTAG_TMS);
+    pin_lo(JTAG_PORT, JTAG_TDI);
+}
+
+#define TCK_LO() pin_lo(JTAG_PORT, JTAG_TCK)
+#define TCK_HI() pin_hi(JTAG_PORT, JTAG_TCK)
+#define SEND_BIT(pin, bit)			\
+    do {					\
+	if(bit)					\
+	    pin_hi(JTAG_PORT, JTAG_TDI);	\
+	else					\
+	    pin_lo(JTAG_PORT, JTAG_TDI);	\
+	CLK_DELAY();				\
+	TCK_HI();				\
+	CLK_DELAY();				\
+    } while(0)
+#define GET_TDO() (JTAG_PIN & _BV(JTAG_TDO))
+
+void jtag_tms(char *buf, int nbits) {
+    int i;
+    int byteoff, bitoff;
+    int bit;
+    
+    for(i = 0; i < nbits; i++) {
+	byteoff = i / 8;
+	bitoff = i % 8;
+	bit = buf[byteoff] & (1 << bitoff);
+	SEND_BIT(JTAG_TMS, bit);
+	TCK_LO();
+    }
+}
+
+void jtag_shift(char *buf, int nbits) {
+    int i;
+    int byteoff, bitoff;
+    int bit;
+    
+    for(i = 0; i < nbits; i++) {
+	byteoff = i / 8;
+	bitoff = i % 8;
+	bit = buf[byteoff] & (1 << bitoff);
+	SEND_BIT(JTAG_TDI, bit);
+	TCK_LO();
+    }
+}
+
+void jtag_shift_inout(char *ibuf, char *obuf, int nbits) {
+    int i;
+    int byteoff, bitoff;
+    int bit, tdo, bitmask;
+    
+    for(i = 0; i < nbits; i++) {
+	byteoff = i / 8;
+	bitoff = i % 8;
+	bitmask = 1 << bitoff;
+	
+	bit = ibuf[byteoff] & bitmask;
+	SEND_BIT(JTAG_TDI, bit);
+	tdo = GET_TDO();
+	TCK_LO();
+	if(tdo)
+	    obuf[byteoff] |= bitmask;
+	else
+	    obuf[byteoff] &= ~(bitmask);
+    }
+}