--- ep93xx.c.orig	2006-03-07 20:53:56.000000000 +0100
+++ ep93xx.c	2006-03-07 21:24:41.000000000 +0100
@@ -22,6 +22,13 @@
 #include "jtag.h"
 #include "bitbang.h"
 
+#define TDO_BIT		1
+#define TDI_BIT		2
+#define TCK_BIT		4
+#define TMS_BIT		8
+#define TRST_BIT	16
+#define SRST_BIT	32
+
 /* system includes */
 #include <sys/io.h>
 #include <string.h>
@@ -29,6 +36,10 @@
 #include <stdio.h>
 
 static u8 output_value = 0x0;
+static int dev_mem_fd;
+static void *gpio_controller;
+static volatile u8 *gpio_data_register;
+static volatile u8 *gpio_data_direction_register;
 
 /* low level command set
  */
@@ -64,7 +75,7 @@
 
 int ep93xx_read(void)
 {
-	/* if TDO == high return 1, else return 0 */
+	return !!(*gpio_data_register & TDO_BIT);
 }
 
 void ep93xx_write(int tck, int tms, int tdi)
@@ -84,7 +95,7 @@
 	else
 		output_value &= TDI_BIT;
 
-	/* output output_value to gpio port */
+	*gpio_data_register = output_value;
 }
 
 /* (1) assert or (0) deassert reset lines */
@@ -100,7 +111,7 @@
 	else if (srst == 1)
 		output_value &= SRST_BIT;
 	
-	/* output output_value to gpio port */
+	*gpio_data_register = output_value;
 }
 
 int ep93xx_speed(int speed)
@@ -115,12 +126,86 @@
 	return ERROR_OK;
 }
 
+static int set_gonk_mode(void)
+{
+	void *syscon;
+	u32 devicecfg;
+
+	syscon = mmap(NULL, 4096, PROT_READ | PROT_WRITE,
+			MAP_SHARED, dev_mem_fd, 0x80930000);
+	if (syscon == MAP_FAILED) {
+		perror("mmap");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	devicecfg = *((volatile int *)(syscon + 0x80));
+	*((volatile int *)(syscon + 0xc0)) = 0xaa;
+	*((volatile int *)(syscon + 0x80)) = devicecfg | 0x08000000;
+
+	munmap(syscon, 4096);
+
+	return ERROR_OK;
+}
+
 int ep93xx_init(void)
 {
-	/* do necessary initialization of gpio port */
+	int ret;
 
 	bitbang_interface = &ep93xx_bitbang;	
 
+	dev_mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
+	if (dev_mem_fd < 0) {
+		perror("open");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	gpio_controller = mmap(NULL, 4096, PROT_READ | PROT_WRITE,
+				MAP_SHARED, dev_mem_fd, 0x80840000);
+	if (gpio_controller == MAP_FAILED) {
+		perror("mmap");
+		close(dev_mem_fd);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	ret = set_gonk_mode();
+	if (ret != ERROR_OK) {
+		munmap(gpio_controller, 4096);
+		close(dev_mem_fd);
+		return ret;
+	}
+
+#if 0
+	/* Use GPIO port A.  */
+	gpio_data_register = gpio_controller + 0x00;
+	gpio_data_direction_register = gpio_controller + 0x10;
+#elsif 0
+	/* Use GPIO port B.  */
+	gpio_data_register = gpio_controller + 0x04;
+	gpio_data_direction_register = gpio_controller + 0x14;
+#elsif 1
+	/* Use GPIO port C.  */
+	gpio_data_register = gpio_controller + 0x08;
+	gpio_data_direction_register = gpio_controller + 0x18;
+#elsif 0
+	/* Use GPIO port D.  */
+	gpio_data_register = gpio_controller + 0x0c;
+	gpio_data_direction_register = gpio_controller + 0x1c;
+#endif
+
+	/*
+	 * Configure bit 0 (TDO) as an input, and bits 1-5 (TDI, TCK
+	 * TMS, TRST, SRST) as outputs.  Drive TDI and TCK low, and
+	 * TMS/TRST/SRST high.
+	 */
+	output_value = TMS_BIT | TRST_BIT | SRST_BIT;
+	*gpio_data_register = output_value;
+
+	/*
+	 * Configure the direction register.  1 = output, 0 = input.
+	 */
+	*gpio_data_direction_register =
+		TDI_BIT | TCK_BIT | TMS_BIT | TRST_BIT | SRST_BIT;
+
 	return ERROR_OK;
 }