[app][mdebug] add JTAG support

- initial gpio implementation
- runs at about 6MHz, not optimized or adjustable yet

6 files changed, 238 insertions, 31 deletions

diff --git a/app/mdebug/jtag.c b/app/mdebug/jtag.c
new file mode 100644
index 00000000..517ab5c1
--- /dev/null
+++ b/app/mdebug/jtag.c
@@ -0,0 +1,116 @@
+/* swdp-m0sub.c
+ *
+ * Copyright 2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <app.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+
+#include <platform.h>
+#include <arch/arm.h>
+#include <kernel/thread.h>
+
+#include <platform/lpc43xx-gpio.h>
+#include <platform/lpc43xx-sgpio.h>
+#include <platform/lpc43xx-clocks.h>
+
+#include "rswdp.h"
+
+#include "lpclink2.h"
+
+static void gpio_init(void) {
+	pin_config(PIN_LED, PIN_MODE(0) | PIN_PLAIN);
+	pin_config(PIN_RESET, PIN_MODE(4) | PIN_PLAIN);
+	pin_config(PIN_RESET_TXEN, PIN_MODE(4) | PIN_PLAIN);
+	pin_config(PIN_TMS_TXEN, PIN_MODE(0) | PIN_PLAIN);
+
+	pin_config(PIN_TDO, PIN_MODE(6) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+	pin_config(PIN_TCK, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+	pin_config(PIN_TDI, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+	pin_config(PIN_TMS, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+
+	gpio_set(GPIO_LED, 0);
+	gpio_set(GPIO_RESET, 1);
+	gpio_set(GPIO_RESET_TXEN, 0);
+	gpio_set(GPIO_TMS_TXEN, 1);
+
+	gpio_config(GPIO_LED, GPIO_OUTPUT);
+	gpio_config(GPIO_RESET, GPIO_OUTPUT);
+	gpio_config(GPIO_RESET_TXEN, GPIO_OUTPUT);
+	gpio_config(GPIO_TMS_TXEN, GPIO_OUTPUT);
+}
+
+#define POS_TDO		10
+#define POS_TCK		11
+#define POS_TDI		12
+#define POS_TMS		14
+
+#define BIT_TDO		(1 << POS_TDO)
+#define BIT_TCK		(1 << POS_TCK)
+#define BIT_TDI		(1 << POS_TDI)
+#define BIT_TMS		(1 << POS_TMS)
+
+void jtag_init(void) {
+	gpio_init();
+
+	writel(BASE_CLK_SEL(CLK_PLL1), BASE_PERIPH_CLK);
+	spin(1000);
+
+	// configure for SGPIO_OUT/OEN control
+	writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(POS_TDO));
+	writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(POS_TCK));
+	writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(POS_TDI));
+	writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(POS_TMS));
+
+	// TCK=0 TDI=0 TMS=0 TDO=input
+	writel(0, SGPIO_OUT);
+	writel(BIT_TCK | BIT_TDI | BIT_TMS, SGPIO_OEN);
+}
+
+static unsigned jtag_tick(unsigned tms, unsigned tdi) {
+	unsigned x = (tms << POS_TMS) | (tdi << POS_TDI);
+	unsigned v;
+	writel(x, SGPIO_OUT);
+	writel(x, SGPIO_OUT);
+	writel(x, SGPIO_OUT);
+	x |= BIT_TCK;
+	v = readl(SGPIO_IN);
+	writel(x, SGPIO_OUT);
+	writel(x, SGPIO_OUT);
+	writel(x, SGPIO_OUT);
+	writel(x, SGPIO_OUT);
+	writel(x, SGPIO_OUT);
+	writel(x, SGPIO_OUT);
+	x ^= BIT_TCK;
+	writel(x, SGPIO_OUT);
+	return (v >> POS_TDO) & 1;
+}
+
+int jtag_io(unsigned count, unsigned tms, unsigned tdi, unsigned *tdo) {
+	unsigned n = 0;
+	unsigned bit = 0;
+	while (count > 0) {
+		n |= (jtag_tick(tms & 1, tdi & 1) << bit);
+		bit++;
+		count--;
+		tms >>= 1;
+		tdi >>= 1;
+	}
+	*tdo = n;
+	return 0;
+}
diff --git a/app/mdebug/lpclink2.h b/app/mdebug/lpclink2.h
new file mode 100644
index 00000000..ab29a8dc
--- /dev/null
+++ b/app/mdebug/lpclink2.h
@@ -0,0 +1,30 @@
+
+// gpio configuration for JTAG
+#define PIN_LED		PIN(1,1)
+#define PIN_RESET	PIN(2,5)
+#define PIN_RESET_TXEN	PIN(2,6)
+#define PIN_TMS_TXEN	PIN(1,5)	// SGPIO15=6
+#define PIN_TMS		PIN(1,6)	// SGPIO14=6
+#define PIN_TCK		PIN(1,17)	// SGPIO11=6
+#define PIN_TDI		PIN(1,18)	// SGPIO12=6
+#define PIN_TDO		PIN(1,14)	// U1_RXD=1, SGPIO10=6
+
+#define GPIO_LED	GPIO(0,8)
+#define GPIO_RESET	GPIO(5,5)
+#define GPIO_RESET_TXEN	GPIO(5,6)
+#define GPIO_TMS_TXEN	GPIO(1,8)
+#define GPIO_TMS	GPIO(1,9)
+#define GPIO_TCK	GPIO(0,12)
+#define GPIO_TDI	GPIO(0,13)
+#define GPIO_TDO	GPIO(1,7)
+
+// alternate names for SWD
+#define PIN_SWDIO_TXEN	PIN_TMS_TXEN
+#define PIN_SWDIO	PIN_TMS
+#define PIN_SWCLK	PIN_TCK
+#define PIN_SWO		PIN_TDO
+
+#define GPIO_SWDIO_TXEN	GPIO_TMS_TXEN
+#define GPIO_SWDIO	GPIO_TMS
+#define GPIO_SWCLK	GPIO_TCK
+
diff --git a/app/mdebug/rswd.c b/app/mdebug/rswd.c
index d335514a..7daa8b71 100644
--- a/app/mdebug/rswd.c
+++ b/app/mdebug/rswd.c
@@ -60,6 +60,10 @@ static void _reboot(void) {
 	platform_halt(HALT_ACTION_REBOOT, HALT_REASON_SW_RESET);
 }
 
+#define MODE_SWD	0
+#define MODE_JTAG	1
+static unsigned mode = MODE_SWD;
+
 /* TODO bounds checking -- we trust the host far too much */
 void process_txn(u32 txnid, u32 *rx, int rxc, u32 *tx) {
 	unsigned msg, op, n;
@@ -113,8 +117,76 @@ void process_txn(u32 txnid, u32 *rx, int rxc, u32 *tx) {
 			}
 			continue;
 		case CMD_ATTACH:
+			if (mode != MODE_SWD) {
+				mode = MODE_SWD;
+				swd_init();
+			}
 			swd_reset();
 			continue;
+		case CMD_JTAG_IO:
+			if (mode != MODE_JTAG) {
+				mode = MODE_JTAG;
+				jtag_init();
+			}
+			tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n);
+			while (n > 0) {
+				unsigned xfer = (n > 32) ? 32 : n;
+				jtag_io(xfer, rx[0], rx[1], tx + txc);
+				rx += 2;
+				rxc -= 2;
+				txc += 1;
+				n -= xfer;
+			}
+			continue;
+		case CMD_JTAG_VRFY:
+			if (mode != MODE_JTAG) {
+				mode = MODE_JTAG;
+				jtag_init();
+			}
+			// (n/32) x 4 words: TMS, TDI, DATA, MASK
+			while (n > 0) {
+				unsigned xfer = (n > 32) ? 32 : n;
+				jtag_io(xfer, rx[0], rx[1], tx + txc);
+				if ((tx[txc] & rx[3]) != rx[2]) {
+					status = ERR_BAD_MATCH;
+					goto done;
+				}
+				rx += 4;
+				rxc -= 4;
+				n -= xfer;
+			}
+			continue;
+		case CMD_JTAG_TX: {
+			unsigned tms = (op & 1) ? 0xFFFFFFFF : 0;
+			if (mode != MODE_JTAG) {
+				mode = MODE_JTAG;
+				jtag_init();
+			}
+			while (n > 0) {
+				unsigned xfer = (n > 32) ? 32 : n;
+				jtag_io(xfer, tms, rx[0], rx);
+				rx++;
+				rxc--;
+				n -= xfer;
+			}
+			continue;
+		}
+		case CMD_JTAG_RX: {
+			unsigned tms = (op & 1) ? 0xFFFFFFFF : 0;
+			unsigned tdi = (op & 2) ? 0xFFFFFFFF : 0;
+			if (mode != MODE_JTAG) {
+				mode = MODE_JTAG;
+				jtag_init();
+			}
+			tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n);
+			while (n > 0) {
+				unsigned xfer = (n > 32) ? 32 : n;
+				jtag_io(xfer, tms, tdi, tx + txc);
+				txc++;
+				n -= xfer;
+			}
+			continue;
+		}
 		case CMD_RESET:
 			swd_hw_reset(n);
 			continue;
diff --git a/app/mdebug/rules.mk b/app/mdebug/rules.mk
index 4a70efd6..168d3aaf 100644
--- a/app/mdebug/rules.mk
+++ b/app/mdebug/rules.mk
@@ -5,10 +5,9 @@ MODULE := $(LOCAL_DIR)
 MODULE_SRCS += \
 	$(LOCAL_DIR)/mdebug.c \
 	$(LOCAL_DIR)/rswd.c \
+	$(LOCAL_DIR)/jtag.c \
+	$(LOCAL_DIR)/swd-m0sub.c \
 	$(LOCAL_DIR)/swo-uart1.c
 
-#MODULE_SRCS +=  $(LOCAL_DIR)/swd-sgpio.c
-MODULE_SRCS += $(LOCAL_DIR)/swd-m0sub.c
-
 include make/module.mk
 
diff --git a/app/mdebug/swd-m0sub.c b/app/mdebug/swd-m0sub.c
index de3ec9d2..63ce42a7 100644
--- a/app/mdebug/swd-m0sub.c
+++ b/app/mdebug/swd-m0sub.c
@@ -31,20 +31,7 @@
 
 #include "rswdp.h"
 
-#define PIN_LED		PIN(1,1)
-#define PIN_RESET	PIN(2,5)
-#define PIN_RESET_TXEN	PIN(2,6)
-#define PIN_SWDIO_TXEN	PIN(1,5)	// SGPIO15=6
-#define PIN_SWDIO	PIN(1,6)	// SGPIO14=6
-#define PIN_SWO		PIN(1,14)	// U1_RXD=1
-#define PIN_SWCLK	PIN(1,17)	// SGPIO11=6
-
-#define GPIO_LED	GPIO(0,8)
-#define GPIO_RESET	GPIO(5,5)
-#define GPIO_RESET_TXEN	GPIO(5,6)
-#define GPIO_SWDIO_TXEN	GPIO(1,8)
-#define GPIO_SWDIO	GPIO(1,9)
-#define GPIO_SWCLK	GPIO(0,12)
+#include "lpclink2.h"
 
 static void gpio_init(void) {
 	pin_config(PIN_LED, PIN_MODE(0) | PIN_PLAIN);
@@ -86,17 +73,17 @@ static unsigned parity(unsigned n) {
 #define RESET_CTRL0		0x40053100
 #define M0_SUB_RST		(1 << 12)
 
-#define COMM_CMD	0x18004000
-#define COMM_ARG1	0x18004004
-#define COMM_ARG2	0x18004008
-#define COMM_RESP	0x1800400C
+#define COMM_CMD		0x18004000
+#define COMM_ARG1		0x18004004
+#define COMM_ARG2		0x18004008
+#define COMM_RESP		0x1800400C
 
-#define CMD_ERR		0
-#define CMD_NOP		1
-#define CMD_READ	2
-#define CMD_WRITE	3
-#define CMD_RESET	4
-#define CMD_SETCLOCK	5
+#define M0_CMD_ERR		0
+#define M0_CMD_NOP		1
+#define M0_CMD_READ		2
+#define M0_CMD_WRITE		3
+#define M0_CMD_RESET		4
+#define M0_CMD_SETCLOCK		5
 
 #define RSP_BUSY	0xFFFFFFFF
 
@@ -129,7 +116,7 @@ void swd_init(void) {
 int swd_write(unsigned hdr, unsigned data) {
 	unsigned n;
 	unsigned p = parity(data);
-	writel(CMD_WRITE, COMM_CMD);
+	writel(M0_CMD_WRITE, COMM_CMD);
 	writel((hdr << 8) | (p << 16), COMM_ARG1);
 	writel(data, COMM_ARG2);
 	writel(RSP_BUSY, COMM_RESP);
@@ -142,7 +129,7 @@ int swd_write(unsigned hdr, unsigned data) {
 
 int swd_read(unsigned hdr, unsigned *val) {
 	unsigned n, data, p;
-	writel(CMD_READ, COMM_CMD);
+	writel(M0_CMD_READ, COMM_CMD);
 	writel(hdr << 8, COMM_ARG1);
 	writel(RSP_BUSY, COMM_RESP);
 	DSB;
@@ -163,7 +150,7 @@ int swd_read(unsigned hdr, unsigned *val) {
 
 void swd_reset(void) {
 	unsigned n;
-	writel(CMD_RESET, COMM_CMD);
+	writel(M0_CMD_RESET, COMM_CMD);
 	writel(RSP_BUSY, COMM_RESP);
 	DSB;
 	asm("sev");
@@ -175,7 +162,7 @@ unsigned swd_set_clock(unsigned khz) {
 	if (khz > 8000) {
 		khz = 8000;
 	}
-	writel(CMD_SETCLOCK, COMM_CMD);
+	writel(M0_CMD_SETCLOCK, COMM_CMD);
 	writel(khz/1000, COMM_ARG1);
 	writel(RSP_BUSY, COMM_RESP);
 	DSB;
diff --git a/app/mdebug/swd.h b/app/mdebug/swd.h
index 2179031c..2822406c 100644
--- a/app/mdebug/swd.h
+++ b/app/mdebug/swd.h
@@ -27,6 +27,9 @@ unsigned swd_set_clock(unsigned khz);
 unsigned swo_set_clock(unsigned khz);
 void swd_hw_reset(int assert);
 
+void jtag_init(void);
+int jtag_io(unsigned count, unsigned tms, unsigned tdi, unsigned *tdo);
+
 // swdp_read/write() register codes
 
 // Park Stop Parity Addr3 Addr2 RnW APnDP Start