diff --git a/README b/README
index 2c310fb..6ac8a6d 100644
--- a/README
+++ b/README
@@ -34,6 +34,7 @@ Libraries
 
 Design Utilities
 
+- hello_world        outputs an AND gate floorplan to stdout
 - new_fp             creates empty .fp floorplan file
 - fp2bit             converts .fp floorplan into .bit bitstream
 - bit2fp             converts .bit bitstream into .fp floorplan
@@ -59,23 +60,33 @@ Design Principles
 TODO (as of 2012-08, expected time to delivery: months to years
       completion status overall: 1%)
 
+short-term:
+* include mini-jtag host app
+* verify AND gate sample (hello_world)
+* support reading iologic switches
+* support counter sample (including clock, jtag)
+* autotest: fix roundtrip issues in routing_sw test
+* autotest: automate generation of gold standards
+* autotest: protect stderr of diff executable in autotest log
+* autotest: cleanup extensions and switch to new extension system
+* autotest: include samples such as hello_world in testing
+* 3 Debian packages: libfpga, libfpga-doc, fpgatools
+
+mid-term:
 * support chips other than xc6slx9, maybe an ftg256 or fgg484-packaged
   xc6 or the xc7a100
-* many more test cases for autotester
-* smarter autotester that can remember and verify groups of tests,
-  automatically oversee test execution, etc.
-* 3 Debian packages: libfpga, libfpga-doc, fpgatools
-* auto-crc calculation in .bit file
-* many more cases in logic block configuration
+* more cases in switch (98% done) and inter-tile connections (15% done) models
+* more cases in logic block configuration
 * configuration of bram and macc blocks, bram initialization data
-* routing switches
-* many more cases in model of switches and inter-tile connections
 * write standard design elements for libfpga-stdlib library
-* support lm32 or openrisc core, either via libfpga or iverilog backend
 * several places might benefit from a bison parser:
   - switchbox description into bit parser/generator (bit_frames.c)
   - inter-tile wire connections (model_conns.c)
   - configure devices and route wires
+
+long-term:
+* auto-crc calculation in .bit file
+* support lm32 or openrisc core, either via libfpga or iverilog backend
 * ipv6 or vnc in hardware?
 * iverilog fpga backend
 
diff --git a/mini-jtag/Makefile b/mini-jtag/Makefile
new file mode 100644
index 0000000..154e253
--- /dev/null
+++ b/mini-jtag/Makefile
@@ -0,0 +1,112 @@
+#
+# Author: Xiangfu Liu
+#
+# This is free and unencumbered software released into the public domain.
+# For details see the UNLICENSE file at the root of the source tree.
+#
+
+PREFIX ?= /usr/local
+
+CFLAGS += -g
+LDLIBS += `pkg-config libftdi --libs`
+OBJS = mini-jtag.o load-bits.o jtag.o
+
+# ----- Verbosity control -----------------------------------------------------
+
+CC_normal	:= $(CC)
+BUILD_normal	:=
+DEPEND_normal	:= $(CPP) $(CFLAGS) -D__OPTIMIZE__ -MM -MG
+
+CC_quiet	= @echo "  CC       " $@ && $(CC_normal)
+BUILD_quiet	= @echo "  BUILD    " $@ && $(BUILD_normal)
+DEPEND_quiet	= @$(DEPEND_normal)
+
+ifeq ($(V),1)
+    CC		= $(CC_normal)
+    BUILD	= $(BUILD_normal)
+    DEPEND	= $(DEPEND_normal)
+else
+    CC		= $(CC_quiet)
+    BUILD	= $(BUILD_quiet)
+    DEPEND	= $(DEPEND_quiet)
+endif
+
+# ----- Rules -----------------------------------------------------------------
+
+.PHONY:		all clean
+.PHONY:		install uninstall
+
+all: mini-jtag
+
+mini-jtag: $(OBJS)
+	$(CC) $(LDFLAGS) -o $@ $(OBJS) $(LDLIBS)
+
+install: all
+	mkdir -p $(DESTDIR)/$(PREFIX)/bin/
+	install -m 755 mini-jtag $(DESTDIR)/$(PREFIX)/bin/
+
+uninstall:
+	rm -f $(DESTDIR)/$(PREFIX)/bin/mini-jtag
+
+clean:
+	rm -f mini-jtag
+	rm -f *.o
+	rm -f *.d
+	rm -f *~
+	rm -f \#*\#
+
+# ----- Dependencies ----------------------------------------------------------
+
+MKDEP =									\
+	$(DEPEND) $< |							\
+	  sed 								\
+	    -e 's|^$(basename $(notdir $<)).o:|$@:|'			\
+	    -e '/^\(.*:\)\? */{p;s///;s/ *\\\?$$/ /;s/  */:\n/g;H;}'	\
+	    -e '$${g;p;}'						\
+	    -e d >$(basename $@).d;					\
+	  [ "$${PIPESTATUS[*]}" = "0 0" ] ||				\
+	  { rm -f $(basename $@).d; exit 1; }
+
+%.o:		%.c
+		$(CC) $(CFLAGS) -o $@ -c $<
+		@$(MKDEP)
+
+-include $(OBJS:.o=.d)
+
+# ----- Test -------------------------------------------------------------------
+
+%.bit:
+	wget -O $@ http://downloads.qi-hardware.com/people/xiangfu/mini-slx9/firmware/$@ || { rm -f $@; exit 1; }
+
+test-counter: counter.bit mini-jtag
+	@./mini-jtag load $<
+	@echo "Read counter registers (1 ~ 5)"
+	@./mini-jtag read 0		# read version
+	@./mini-jtag read 1		# read counter EN
+	@./mini-jtag read 2		# read counter WE
+	@./mini-jtag read 3		# read counter_set
+	@./mini-jtag read 4		# read counter
+	@echo "Read counter"
+	@./mini-jtag write 1 1		# enable counter
+	@./mini-jtag read 4		# read counter
+	@./mini-jtag write 1 0		# disable counter
+	@echo "Set counter to 50000000"
+	@./mini-jtag write 2 1		# set counter WE
+	@./mini-jtag write 3 50000000	# set counter
+	@./mini-jtag read 3		# read counter_set
+	@./mini-jtag write 2 0		# set counter WE
+	@echo "Read counter"
+	@./mini-jtag write 1 1		# enable counter
+	@./mini-jtag read 4		# read counter
+	@echo "Sleep 1 second for count"
+	@sleep 1
+	@./mini-jtag read 4		# read counter
+	@./mini-jtag write 1 0		# disable counter
+
+test-blinking: blinking.bit mini-jtag
+	./mini-jtag idcode
+	./mini-jtag load $<
+	sleep 1
+	./mini-jtag reset
+	./mini-jtag load $<
+
diff --git a/mini-jtag/jtag.c b/mini-jtag/jtag.c
new file mode 100644
index 0000000..9b1c060
--- /dev/null
+++ b/mini-jtag/jtag.c
@@ -0,0 +1,189 @@
+//
+// Author: Xiangfu Liu
+//
+// This is free and unencumbered software released into the public domain.
+// For details see the UNLICENSE file at the root of the source tree.
+//
+
+#include <ftdi.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include "jtag.h"
+
+int tap_tms(struct ftdi_context *ftdi, int tms, uint8_t bit7)
+{
+	char buf[3];
+	buf[0] = MPSSE_WRITE_TMS|MPSSE_LSB|MPSSE_BITMODE|MPSSE_WRITE_NEG;
+	buf[1] = 0;		/* value = lenght - 1 */
+	buf[2] = (tms ? 0x01 : 0x00) | ((bit7 & 0x01) << 7);
+	if (ftdi_write_data(ftdi, buf, 3) != 3)
+		return -1;
+
+	return 0;
+}
+
+void tap_reset_rti(struct ftdi_context *ftdi)
+{
+	int i;
+	for(i = 0; i < 5; i++)
+		tap_tms(ftdi, 1, 0);
+
+	tap_tms(ftdi, 0, 0);	/* Goto RTI */
+}
+
+int tap_shift_ir(struct ftdi_context *ftdi, uint8_t ir)
+{
+	/* Have to be at RTI status before call this function */
+	int ret = 0;
+	uint8_t buf[3] = {0, 0, 0};
+
+	tap_tms(ftdi, 1, 0);
+	tap_tms(ftdi, 1, 0);
+	tap_tms(ftdi, 0, 0);
+	tap_tms(ftdi, 0, 0);	/* Goto shift IR */
+
+
+	buf[0] = MPSSE_DO_WRITE|MPSSE_LSB|
+		MPSSE_BITMODE|MPSSE_WRITE_NEG;
+	buf[1] = 4;
+	buf[2] = ir;
+	if (ftdi_write_data(ftdi,buf, 3) != 3) {
+		fprintf(stderr, "Write loop failed\n");
+		ret = -1;
+	}
+
+	tap_tms(ftdi, 1, (ir >> 5));
+	tap_tms(ftdi, 1, 0);
+	tap_tms(ftdi, 0, 0); /* Goto RTI */
+
+	return ret;
+}
+
+
+static int shift_last_bits(struct ftdi_context *ftdi,
+	       uint8_t *in, uint8_t len, uint8_t *out)
+{
+	uint8_t buf[3];
+
+	if (!len)
+		return -1;
+
+	buf[0] = MPSSE_LSB|MPSSE_BITMODE|MPSSE_WRITE_NEG;
+
+	if (in)
+		buf[0] |= MPSSE_DO_WRITE;
+
+	if (out)
+		buf[0] |= MPSSE_DO_READ;
+
+	buf[1] = len - 1;
+
+	if (in)
+		buf[2] = *in;
+
+	if (ftdi_write_data(ftdi, buf, 3) != 3) {
+		fprintf(stderr,
+			"Ftdi write failed\n");
+		return -1;
+	}
+
+	if (out)
+		ftdi_read_data(ftdi, out, 1);
+
+	return 0;
+}
+
+int tap_shift_dr_bits(struct ftdi_context *ftdi,
+		      uint8_t *in, uint32_t in_bits,
+		      uint8_t *out)
+{
+	/* Have to be at RTI status before call this function */
+	uint8_t buf[3];
+	uint8_t *buf_bytes;
+
+	uint32_t in_bytes = 0;
+	uint32_t last_bits = 0;
+	uint16_t last_bytes, len, len_pre;
+	int i, t;
+
+	/* Send 3 Clocks with TMS = 1 0 0 to reach SHIFTDR*/
+	tap_tms(ftdi, 1, 0);
+	tap_tms(ftdi, 0, 0);
+	tap_tms(ftdi, 0, 0);
+
+	in_bytes = in_bits / 8;
+	last_bits = in_bits % 8;
+
+	/* If last_bits == 0, the last bit of last byte should send out with TMS */
+	if (in_bytes) {
+		t = in_bytes / FTDI_MAX_RW_SIZE;
+		last_bytes = in_bytes % FTDI_MAX_RW_SIZE;
+
+		for (i = 0; i <= t; i++) {
+			len = (i == t) ? last_bytes : FTDI_MAX_RW_SIZE;
+
+			buf_bytes = malloc(len * sizeof(uint8_t) + 3);
+			if (!buf_bytes) {
+				fprintf(stderr,
+					"Can't malloc memory\n");
+				return -1;
+			}
+			memset(buf_bytes, 0, len + 3);
+
+			buf_bytes[0] = MPSSE_LSB|MPSSE_WRITE_NEG;
+
+			if (in)
+				buf_bytes[0] |= MPSSE_DO_WRITE;
+
+			if (out)
+				buf_bytes[0] |= MPSSE_DO_READ;
+
+			buf_bytes[1] = (len - 1) & 0xff;
+			buf_bytes[2] = ((len - 1) >> 8) & 0xff;
+
+			if (in)
+				memcpy(&buf_bytes[3], (in + i * len_pre), len);
+
+			if (ftdi_write_data(ftdi, buf_bytes, len + 3) != len + 3) {
+				fprintf(stderr,
+					"Ftdi write failed\n");
+				free(buf_bytes);
+				return -1;
+			}
+
+			if (out)
+				ftdi_read_data(ftdi, (out + i * len), len);
+
+			len_pre = len;
+			free(buf_bytes);
+		}
+	}
+
+	if (last_bits) {
+		/* Send last few bits */
+		shift_last_bits(ftdi, &in[in_bytes], last_bits - 1, out);
+		tap_tms(ftdi, 1, (in[in_bytes] >> (last_bits - 1)));
+	} else
+		tap_tms(ftdi, 1, 0);
+
+	tap_tms(ftdi, 1, 0);
+	tap_tms(ftdi, 0, 0); /* Goto RTI */
+
+	return 0;
+}
+
+int ft232_flush(struct ftdi_context *ftdi)
+{
+	char buf[1] = { SEND_IMMEDIATE };
+	if (ftdi_write_data(ftdi, buf, 1) != 1) {
+		fprintf(stderr,
+			 "Can't SEND_IMMEDIATE\n");
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/mini-jtag/jtag.h b/mini-jtag/jtag.h
new file mode 100644
index 0000000..05c9046
--- /dev/null
+++ b/mini-jtag/jtag.h
@@ -0,0 +1,40 @@
+//
+// Author: Xiangfu Liu
+//
+// This is free and unencumbered software released into the public domain.
+// For details see the UNLICENSE file at the root of the source tree.
+//
+
+
+#ifndef JTAG_H
+#define JTAG_H
+
+/* FPGA Boundary-Scan Instructions */
+#define EXTEST	0x0F
+#define SAMPLE	0x01
+#define USER1	0x02
+#define USER2	0x03
+#define USER3	0x1A
+#define USER4	0x1B
+#define CFG_OUT	0x04
+#define CFG_IN	0x05
+#define INTEST	0x07
+#define USERCODE	0x08
+#define IDCODE	0x09
+#define JPROGRAM	0x0B
+#define JSTART	0x0C
+#define JSHUTDOWN	0x0D
+#define BYPASS	0x3F
+
+/* The max read/write size is 65536, we use 65532 here */
+#define FTDI_MAX_RW_SIZE	65532
+
+int tap_tms(struct ftdi_context *ftdi, int tms, uint8_t bit7);
+void tap_reset_rti(struct ftdi_context *ftdi);
+int tap_shift_ir(struct ftdi_context *ftdi, uint8_t ir);
+int tap_shift_dr_bits(struct ftdi_context *ftdi,
+		      uint8_t *in, uint32_t in_bits,
+		      uint8_t *out);
+int ft232_flush(struct ftdi_context *ftdi);
+
+#endif
diff --git a/mini-jtag/load-bits.c b/mini-jtag/load-bits.c
new file mode 100644
index 0000000..303acda
--- /dev/null
+++ b/mini-jtag/load-bits.c
@@ -0,0 +1,111 @@
+//
+// Author: Xiangfu Liu
+//
+// This is free and unencumbered software released into the public domain.
+// For details see the UNLICENSE file at the root of the source tree.
+//
+
+#include <ftdi.h>
+#include <usb.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include "load-bits.h"
+
+int read_section (FILE *bit_file, char *id, uint8_t **data, uint32_t *len)
+{
+    uint8_t buf[4];
+    int lenbytes;
+
+    /* first read 1 bytes, the section key */
+    if (fread (buf, 1, 1, bit_file) != 1)
+	    return -1;
+
+    *id = buf[0];
+
+    /* section 'e' has 4 bytes indicating section length */
+    if (*id == 'e')
+        lenbytes = 4;
+    else
+        lenbytes = 2;
+
+    /* first read 1 bytes */
+    if (fread (buf, 1, lenbytes, bit_file) != lenbytes)
+	    return -1;
+
+    /* second and third is section length */
+    if (*id != 'e')
+        *len = buf[0] << 8 | buf[1];
+    else
+        *len = buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3];
+
+    /* now allocate memory for data */
+    *data = malloc (*len);
+
+    if (fread (*data, 1, *len, bit_file) != *len)
+	    return -1;
+
+    return 0;
+}
+
+int load_bits(FILE *bit_file, struct load_bits *bs)
+{
+    char sid = 0;
+    uint8_t *sdata;
+    uint32_t slen;
+
+    uint8_t buf[128];
+    uint8_t header[] = {
+        0x00, 0x09, 0x0f, 0xf0, 0x0f, 0xf0, 0x0f, 0xf0,
+        0x0f, 0xf0, 0x00, 0x00, 0x01,
+    };
+
+    if (fread (buf, 1, sizeof (header), bit_file) != sizeof (header))
+	    return -1;
+
+    if (memcmp (buf, header, sizeof (header)) != 0)
+	    return -1;
+
+    /* printf("Valid bitfile header found.\n"); */
+
+    while (sid != 'e')
+    {
+        if (read_section (bit_file, &sid, &sdata, &slen) != 0)
+		return -1;
+
+	/* printf("Read section id=%c len=%d.\n", sid, slen); */
+
+        /* make sure that strings are terminated */
+        if (sid != 'e')
+            sdata[slen-1] = '\0';
+
+        switch (sid)
+        {
+            case 'a': bs->design = (char *) sdata; break;
+            case 'b': bs->part_name = (char *) sdata; break;
+            case 'c': bs->date = (char *) sdata; break;
+            case 'd': bs->time = (char *) sdata; break;
+            case 'e': bs->data = sdata; bs->length = slen; break;
+        }
+    }
+
+    return 0;
+}
+
+void bits_free(struct load_bits *bs)
+{
+    if (bs->design)
+        free(bs->design);
+    if (bs->part_name)
+        free(bs->part_name);
+    if (bs->date)
+        free(bs->date);
+    if (bs->time)
+        free(bs->time);
+    if (bs->data)
+        free(bs->data);
+    free (bs);
+}
diff --git a/mini-jtag/load-bits.h b/mini-jtag/load-bits.h
new file mode 100644
index 0000000..3e453a9
--- /dev/null
+++ b/mini-jtag/load-bits.h
@@ -0,0 +1,24 @@
+//
+// Author: Xiangfu Liu
+//
+// This is free and unencumbered software released into the public domain.
+// For details see the UNLICENSE file at the root of the source tree.
+//
+
+#ifndef LOAD_BITS_H
+#define LOAD_BITS_H
+
+struct load_bits {
+    char *design;
+    char *part_name;
+    char *date;
+    char *time;
+    uint32_t   length;
+    uint8_t    *data;
+};
+
+int read_section(FILE *bit_file, char *id, uint8_t **data, uint32_t *len);
+int load_bits(FILE *bit_file, struct load_bits *bs);
+void bits_free(struct load_bits *bs);
+
+#endif /* LOAD_BITS_H */
diff --git a/mini-jtag/mini-jtag.c b/mini-jtag/mini-jtag.c
new file mode 100644
index 0000000..432c2d1
--- /dev/null
+++ b/mini-jtag/mini-jtag.c
@@ -0,0 +1,297 @@
+//
+// Author: Xiangfu Liu
+//
+// This is free and unencumbered software released into the public domain.
+// For details see the UNLICENSE file at the root of the source tree.
+//
+
+#include <ftdi.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include "load-bits.h"
+#include "jtag.h"
+
+/* JTAG/Serial board ID */
+#define VENDOR  0x20b7
+#define PRODUCT 0x0713
+
+uint8_t jtagcomm_checksum(uint8_t *d, int len)
+{
+	int i, j, bytes, bits;
+	uint8_t checksum = 0x01;
+
+	bytes = len / 8;
+	bits = len % 8;
+
+	j = 0;
+	if (bytes)
+		for (j = 0; j < bytes; j++)
+			for (i = 0; i < 8; i++)
+				checksum ^= ((d[j] >> i) & 0x01) ? 1 : 0;
+
+
+	if (bits)
+		for (i = 0; i < bits; i++)
+			checksum ^= ((d[j] >> i) & 0x01) ? 1 : 0;
+
+	return checksum;
+}
+
+void usage(char *name)
+{
+	fprintf(stderr,
+		"\n"
+		"%s - A small JTAG program talk to FPGA chip\n"
+		"Usage:\n"
+		"    idcode\n"
+		"    reset\n"
+		"    load <bits file>\n"
+		"    cr\t\t\t\tRead configure register status\n"
+		"    read|write reg <value>\n"
+		"Report bugs to xiangfu@openmobilefree.net\n"
+		"\n", name);
+}
+
+int main(int argc, char **argv)
+{
+	struct ftdi_context ftdi;
+	uint8_t buf[65536];
+
+	uint8_t conf_buf[] = {SET_BITS_LOW,  0x08, 0x0b,
+			      SET_BITS_HIGH, 0x00, 0x00,
+			      TCK_DIVISOR,   0x00, 0x00,
+			      LOOPBACK_END};
+
+	if (argc < 2) {
+		usage(argv[0]);
+		return 1;
+	}
+
+	if (strcmp (argv[1], "idcode") && strcmp (argv[1], "reset") &&
+	    strcmp (argv[1], "load")  && strcmp (argv[1], "readreg") &&
+	    strcmp (argv[1], "read") && strcmp (argv[1], "write")
+		) {
+		usage(argv[0]);
+		return 1;
+	}
+
+	/* Init */
+	ftdi_init(&ftdi);
+	if (ftdi_usb_open_desc(&ftdi, VENDOR, PRODUCT, 0, 0) < 0) {
+		fprintf(stderr,
+			"Can't open device %04x:%04x\n", VENDOR, PRODUCT);
+		return 1;
+	}
+	ftdi_usb_reset(&ftdi);
+	ftdi_set_interface(&ftdi, INTERFACE_A);
+	ftdi_set_latency_timer(&ftdi, 1);
+	ftdi_set_bitmode(&ftdi, 0xfb, BITMODE_MPSSE);
+	if (ftdi_write_data(&ftdi, conf_buf, 10) != 10) {
+		fprintf(stderr,
+			"Can't configure device %04x:%04x\n", VENDOR, PRODUCT);
+		return 1;
+	}
+
+	buf[0] = GET_BITS_LOW;
+	buf[1] = SEND_IMMEDIATE;
+
+	if (ftdi_write_data(&ftdi, buf, 2) != 2) {
+		fprintf(stderr,
+			"Can't send command to device\n");
+		return 1;
+	}
+	ftdi_read_data(&ftdi, &buf[3], 1);
+	if (!(buf[3] & 0x10)) {
+		fprintf(stderr,
+			"Vref not detected. Please power on target board\n");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "idcode")) {
+		tap_reset_rti(&ftdi);
+		tap_shift_dr_bits(&ftdi, NULL, 32, buf);
+		printf("0x%02x%02x%02x%02x\n",
+		       buf[3], buf[2], buf[1], buf[0]);
+	}
+
+	if (!strcmp (argv[1], "reset")) {
+		tap_reset_rti(&ftdi);
+		tap_shift_ir(&ftdi, JPROGRAM);
+		tap_reset_rti(&ftdi);
+	}
+
+	if (!strcmp (argv[1], "load")) {
+		if(argc < 3) {
+			usage(argv[0]);
+			goto exit;
+		}
+
+		struct load_bits *bs;
+		FILE *pld_file;
+		uint8_t *dr_data;
+		uint32_t u;
+		int i;
+
+		if ((pld_file = fopen(argv[2], "r")) == NULL) {
+			perror("Unable to open file");
+			goto exit;
+		}
+
+		bs = calloc(1, sizeof(*bs));
+		if (!bs) {
+			perror("memory allocation failed");
+			goto exit;
+		}
+
+		if (load_bits(pld_file, bs) != 0) {
+			fprintf(stderr, "%s not supported\n", argv[2]);
+			goto free_bs;
+		}
+
+		printf("Bitstream information:\n");
+		printf("\tDesign: %s\n", bs->design);
+		printf("\tPart name: %s\n", bs->part_name);
+		printf("\tDate: %s\n", bs->date);
+		printf("\tTime: %s\n", bs->time);
+		printf("\tBitstream length: %d\n", bs->length);
+
+		/* copy data into shift register */
+		dr_data = malloc(bs->length * sizeof(char));
+		for (u = 0; u < bs->length; u++) {
+			/* flip bits */
+			dr_data[u] |= ((bs->data[u] & 0x80) ? 1 : 0) << 0;
+			dr_data[u] |= ((bs->data[u] & 0x40) ? 1 : 0) << 1;
+			dr_data[u] |= ((bs->data[u] & 0x20) ? 1 : 0) << 2;
+			dr_data[u] |= ((bs->data[u] & 0x10) ? 1 : 0) << 3;
+			dr_data[u] |= ((bs->data[u] & 0x08) ? 1 : 0) << 4;
+			dr_data[u] |= ((bs->data[u] & 0x04) ? 1 : 0) << 5;
+			dr_data[u] |= ((bs->data[u] & 0x02) ? 1 : 0) << 6;
+			dr_data[u] |= ((bs->data[u] & 0x01) ? 1 : 0) << 7;
+		}
+
+		tap_reset_rti(&ftdi);
+		tap_shift_ir(&ftdi, JPROGRAM);
+		tap_shift_ir(&ftdi, CFG_IN);
+
+		tap_shift_dr_bits(&ftdi, dr_data, bs->length * 8, NULL);
+
+		/* ug380.pdf
+		 * P161: a minimum of 16 clock cycles to the TCK */
+		tap_shift_ir(&ftdi, JSTART);
+		for (i = 0; i < 32; i++)
+			tap_tms(&ftdi, 0, 0);
+
+		tap_reset_rti(&ftdi);
+
+	free_dr:
+		free(dr_data);
+	free_bs:
+		bits_free(bs);
+		fclose(pld_file);
+	}
+
+	if (!strcmp(argv[1], "readreg") && argc == 3) {
+		uint8_t in[14] = {
+			0xaa, 0x99, 0x55, 0x66,
+			0x00, 0x00, 0x20, 0x00,
+			0x20, 0x00, 0x20, 0x00,
+			0x20, 0x00
+		};
+
+		uint16_t reg = 0x2801;	/* type 1 packet (word count = 1) */
+		reg |= ((atoi(argv[2]) & 0x3f) << 5);
+
+		in[4] = (reg & 0xff00) >> 8;
+		in[5] = reg & 0x00;
+
+		tap_reset_rti(&ftdi);
+		tap_shift_ir(&ftdi, CFG_IN);
+		tap_shift_dr_bits(&ftdi, in, 14 * 8, NULL);
+		tap_shift_ir(&ftdi, CFG_OUT);
+		tap_shift_dr_bits(&ftdi, NULL, 2 * 8, buf);
+
+		int i;
+		printf("Read: ");
+		for (i = 1; i >= 0 ; i--)
+			printf("%02x ", buf[i]);
+		printf(" [%d]\n",(uint32_t) (buf[1] << 8  | buf[0]));
+
+		tap_reset_rti(&ftdi);
+	}
+
+	/* TODO:
+	 *   Configuration Memory Read Procedure (IEEE Std 1149.1 JTAG) */
+
+	/* TODO:
+	 *   There is no error check on read/write paramters */
+	if (!strcmp (argv[1], "read") && argc == 3) {
+		int i;
+		uint8_t addr, checksum;
+		uint8_t in[5];
+
+		tap_reset_rti(&ftdi);
+		tap_shift_ir(&ftdi, USER1);
+
+		/* Tell the FPGA what address we would like to read */
+		addr = atoi(argv[2]);
+		addr &= 0xf;
+		in[0] = addr;
+		checksum = jtagcomm_checksum(in, 4);
+
+		in[0] = (checksum << 5) | (0 << 4) | addr;
+
+		tap_shift_dr_bits(&ftdi, in, 6, NULL);
+
+		/* Now read back the register */
+		tap_shift_dr_bits(&ftdi, NULL, 32, buf);
+		printf("Read: ");
+		for (i = 3; i >= 0 ; i--)
+			printf("%02x ", buf[i]);
+		printf(" [%d]\n",(uint32_t) (buf[3] << 24 | buf[2] << 16 |
+					     buf[1] << 8  | buf[0]));
+
+		tap_reset_rti(&ftdi);
+	}
+
+	if (!strcmp(argv[1], "write") && argc == 4) {
+		int i;
+		uint8_t addr, checksum;
+		uint8_t in[5];
+		uint32_t value;
+
+		tap_reset_rti(&ftdi);
+		tap_shift_ir(&ftdi, USER1);
+
+		value = atoi(argv[3]);
+		in[0] = value & 0x000000ff;
+		in[1] = (value & 0x0000ff00) >> 8;
+		in[2] = (value & 0x00ff0000) >> 16;
+		in[3] = (value & 0xff000000) >> 24;
+
+		/* Tell the FPGA what address we would like to read */
+		addr = atoi(argv[2]);
+		addr &= 0xf;
+		in[4] = (1 << 4) | addr;
+		checksum = jtagcomm_checksum(in, 37);
+
+		in[4] |= (checksum << 5);
+		printf("Write: %02x %02x %02x %02x %02x\n",
+		       in[4],in[3],in[2],in[1],in[0]);
+
+		tap_shift_dr_bits(&ftdi, in, 38, buf);
+
+		tap_reset_rti(&ftdi);
+	}
+
+
+exit:
+	/* Clean up */
+	ftdi_usb_reset(&ftdi);
+	ftdi_usb_close(&ftdi);
+	ftdi_deinit(&ftdi);
+	return 0;
+}