[Feature]add MT2731_MP2_MR2_SVN388 baseline version

Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/app/mdebug/fw-m0sub.S b/src/bsp/lk/app/mdebug/fw-m0sub.S
new file mode 100644
index 0000000..b7185b5
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/fw-m0sub.S
@@ -0,0 +1,519 @@
+/* fw-m0sub.S
+ *
+ * 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.
+ */
+
+.syntax unified
+
+m0_vectors:
+	.word 0x18003FF0
+	.word m0_reset + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+	.word m0_fault + 1
+// external IRQs
+	.word m0_fault + 1
+	.word m0_irq + 1
+
+m0_fault:
+	ldr r0, =0x18000000
+	ldr r1, =0xeeee0000
+	mrs r2, xpsr
+	movs r3, #0xFF
+	ands r2, r2, r3
+	orrs r1, r1, r2
+	str r1, [r0]
+	b .
+
+.ltorg
+
+#define REPORT_DELAY	0
+
+#define COMM_BASE	0x18004000
+
+#define	COMM_CMD	0
+#define COMM_ARG0	4
+#define COMM_ARG1	8
+#define COMM_RESP	12
+#define COMM_RETRY	16
+
+
+#define M4_TXEV		0x40043130 // write 0 to clear
+
+#define SGPIO_BASE	(0x40101210)
+#define OFF_IN		0
+#define OFF_OUT		4
+#define OFF_OEN		8
+#define SGPIO_IN	(0x40101210)
+#define SGPIO_OUT	(0x40101214)
+#define SGPIO_OEN	(0x40101218)
+
+#define CLK_BIT		11
+#define DIO_BIT		14
+#define TEN_BIT		15
+#define CLK_MSK		(1 << CLK_BIT)
+#define DIO_MSK		(1 << DIO_BIT)
+#define TEN_MSK		(1 << TEN_BIT)
+
+#define CLK1_OUT	(CLK_MSK | TEN_MSK)
+#define CLK0_OUT	(TEN_MSK)
+#define CLK1_IN		(CLK_MSK)
+#define CLK0_IN		(0)
+
+#define OEN_IN		((1 << CLK_BIT) | (1 << TEN_BIT))
+#define OEN_OUT		((1 << CLK_BIT) | (1 << DIO_BIT) | (1 << TEN_BIT))
+
+#define NOP4	nop ; nop ; nop ; nop
+#define NOP8	NOP4 ; NOP4
+#define NOP16	NOP8 ; NOP8
+
+//#define DELAY	nop ; nop
+//#define DELAY NOP8
+
+// r11 CLK1_OUT     const
+// r10 CLK0_OUT     const
+// r9  delay        subroutine
+// r8  comm_base    addr
+// r7  SGPIO_BASE   addr
+// r6  DIO_MSK      const
+// r5  CLK1_IN      const
+// r4  CLK0_IN      const
+// r3  outbits      data
+
+snooze_2m:
+	nop ; nop ; nop ; nop
+	nop ; nop ; nop ; nop
+	nop ; nop ; nop ; nop
+	nop ; nop ; nop ; nop
+snooze_3m:
+	nop ; nop ; nop ; nop
+	nop ; nop ; nop ; nop
+snooze_4m:
+	nop ; nop ; nop ; nop
+	nop ; nop ; nop ; nop
+snooze_6m:
+	nop ; nop ; nop ; nop
+snooze_8m:
+	bx lr
+
+// delay    0 nops  16MHz
+// delay    2 nops  12MHz
+// delay    4 nops   9.6MHz
+#define DELAY blx r9
+
+// 12 cycles + DELAY x 2
+.macro ONE_BIT_OUT
+	lsls r2, r3, #DIO_BIT	// shift bit 1 to posn
+	ands r2, r2, r6		// isolate bit 1
+	movs r1, r2		// save bit 1
+	add r2, r2, r10		// combine with CLK1
+	DELAY
+	str r2, [r7, #OFF_OUT]	// commit negative egde
+	lsrs r3, r3, #1		// advance to next bit
+	add r1, r1, r11		// combine with CLK1
+	nop
+	nop
+	DELAY
+	str r1, [r7, #OFF_OUT]	// commit positive edge
+.endm
+
+.macro ONE_BIT_IN
+	ands r0, r0, r6		// isolate input bit
+	lsls r0, r0, #(31-DIO_BIT) // move to posn 31
+	lsrs r3, r3, #1		// make room
+	orrs r3, r3, r0		// add bit
+	DELAY
+	str r4, [r7, #OFF_OUT]	// commit negative edge
+	ldr r0, [r7, #OFF_IN]	// sample input
+	nop
+	nop
+	DELAY
+	str r5, [r7, #OFF_OUT]	// commit positive edge
+.endm
+
+// used for the final parity and turn bits on input so this
+// actually only reads one bit
+read_2:
+	push {lr}
+	nop
+	nop
+	nop
+	nop
+	DELAY
+	str r4, [r7, #OFF_OUT]
+	ldr r0, [r7, #OFF_IN]
+	nop
+	nop
+	DELAY
+	str r5, [r7, #OFF_OUT]
+	ands r0, r0, r6		// isolate bit
+	lsrs r0, r0, #DIO_BIT	// shift to bit0
+	nop
+	nop
+	DELAY
+	str r4, [r7, #OFF_OUT]
+	nop
+	nop
+	nop
+	nop
+	DELAY
+	str r5, [r7, #OFF_OUT]
+	pop {pc}
+
+// w0: <15> <parity:1> <cmd:16>
+// w1: <data:32>
+
+
+write_16:
+	push {lr}
+	b _write_16
+write_32:
+	push {lr}
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+_write_16:
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	ONE_BIT_OUT
+	pop {pc}
+write_1:
+	push {lr}
+	ONE_BIT_OUT
+	pop {pc}
+
+read_4:
+	push {lr}
+	b _read_4
+read_32:
+	push {lr}
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+_read_4:
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ONE_BIT_IN
+	ands r0, r0, r6		// isolate input bit
+	lsls r0, r0, #(31-DIO_BIT) // move to posn 31
+	lsrs r3, r3, #1		// make room
+	orrs r3, r3, r0		// add bit
+	pop {pc}
+
+init:
+	ldr r0, =CLK1_OUT
+	mov r11, r0
+	ldr r0, =CLK0_OUT
+	mov r10, r0
+	ldr r0, =(snooze_4m + 1)
+	mov r9, r0
+	ldr r0, =COMM_BASE
+	mov r8, r0
+	ldr r7, =SGPIO_BASE
+	ldr r6, =DIO_MSK
+	ldr r5, =CLK1_IN
+	ldr r4, =CLK0_IN
+	bx lr
+
+#define MAX_RETRY	8192
+
+err_fail:
+	movs r0, #3
+	mov r3, r8
+	str r0, [r3, #COMM_RESP];
+	pop {pc}
+
+err_timeout:
+	movs r0, #2
+	mov r3, r8
+	str r0, [r3, #COMM_RESP];
+	pop {pc}
+
+cmd_read_txn:
+	push {lr}
+
+	ldr r0, =MAX_RETRY
+	//movs r0, #MAX_RETRY
+	mov r12, r0
+
+rd_retry:
+	ldr r3, [r3, #COMM_ARG0]
+	bl write_16
+
+	ldr r3, =OEN_IN
+	str r3, [r7, #OFF_OEN]
+	bl read_4
+
+	lsrs r3, r3, #29
+	cmp r3, #1		// OK
+	beq rd_okay
+
+	ldr r1, =OEN_OUT
+	str r1, [r7, #OFF_OEN]
+
+	cmp r3, #2		// WAIT
+	bne err_fail
+
+	mov r0, r12
+	subs r0, r0, #1
+	mov r12, r0
+	beq err_timeout
+	mov r3, r8
+	b rd_retry
+
+rd_okay:
+	bl read_32
+	bl read_2
+	ldr r1, =OEN_OUT
+	str r1, [r7, #OFF_OEN]
+	mov r1, r11
+	orrs r1, r1, r6
+	str r1, [r7, #OFF_OUT]
+
+	mov r1, r8		// get COMM_BASE
+	str r3, [r1, #COMM_ARG0]
+	str r0, [r1, #COMM_ARG1]
+	movs r0, #0
+	str r0, [r1, #COMM_RESP]
+#if REPORT_DELAY
+	mov r0, r12
+	str r0, [r1, #COMM_RETRY]
+#endif
+	pop {pc}
+	
+
+cmd_write_txn:
+	push {lr}
+
+	ldr r0, =MAX_RETRY
+	mov r12, r0
+
+wr_retry:
+	ldr r3, [r3, #COMM_ARG0]
+	bl write_16
+	push {r3}		// stash parity bit
+
+	ldr r3, =OEN_IN
+	str r3, [r7, #OFF_OEN]
+	bl read_4
+
+	lsrs r3, r3, #29
+	cmp r3, #1		// OK
+	beq wr_okay
+
+	pop {r0}		// discard saved parity bit
+
+	ldr r1, =OEN_OUT
+	str r1, [r7, #OFF_OEN]
+
+	cmp r3, #2		// WAIT
+	bne err_fail
+
+	mov r0, r12
+	subs r0, r0, #1
+	mov r12, r0
+	beq err_timeout
+
+	mov r3, r8
+	b wr_retry
+
+wr_okay:
+	ldr r3, =OEN_OUT
+	str r3, [r7, #OFF_OEN]
+	bl write_1
+
+	mov r3, r8
+	ldr r3, [r3, #COMM_ARG1]
+	bl write_32
+
+	pop {r3}		// recover parity bit
+	bl write_1
+
+	mov r3, r8		// get COMM_BASE
+	movs r0, #0
+	str r0, [r3, #COMM_RESP]
+#if REPORT_DELAY
+	mov r0, r12
+	str r0, [r3, #COMM_RETRY]
+#endif
+	pop {pc}
+
+cmd_reset:
+	push {lr}
+	ldr r3, =0xffffffff
+	mov r12, r3
+	bl write_32
+	mov r3, r12
+	bl write_32
+
+	ldr r3, =0b1110011110011110
+	bl write_16
+
+	mov r3, r12
+	bl write_32
+	mov r3, r12
+	bl write_32
+
+	mov r3, r8
+	movs r0, #0
+	str r0, [r3, #COMM_RESP]
+	pop {pc}
+
+
+m0_irq:
+	push {lr}
+
+	// clear event from m4
+	ldr r0, =M4_TXEV
+	movs r1, #0
+	str r1, [r0]
+
+	mov r3, r8		// get COMM_BASE
+	ldr r0, [r3, #COMM_CMD]
+	cmp r0, #5
+	bls good_cmd
+	movs r0, #0
+good_cmd:
+	lsls r0, r0, #2
+	adr r1, cmd_table
+	ldr r2, [r1, r0]
+	blx r2
+
+	pop {pc}
+
+.align 2
+cmd_table:
+	.word cmd_invalid + 1
+	.word cmd_nop + 1
+	.word cmd_read_txn + 1
+	.word cmd_write_txn + 1
+	.word cmd_reset + 1
+	.word cmd_setclock + 1
+
+cmd_invalid:
+	movs r0, #9
+	str r0, [r3, #COMM_RESP]
+	bx lr
+
+cmd_nop:
+	movs r0, #0
+	str r0, [r3, #COMM_RESP]
+	bx lr
+
+cmd_setclock:
+	ldr r0, [r3, #COMM_ARG0]
+	cmp r0, #8
+	bls good_clock
+	movs r0, #0
+good_clock:
+	lsls r0, r0, #2
+	adr r1, snooze_table
+	ldr r1, [r1, r0]
+	mov r9, r1
+
+	movs r0, #0
+	str r0, [r3, #COMM_RESP]
+	bx lr
+
+.align 2
+snooze_table:
+	.word snooze_2m + 1
+	.word snooze_2m + 1
+	.word snooze_2m + 1
+	.word snooze_3m + 1
+	.word snooze_4m + 1
+	.word snooze_4m + 1
+	.word snooze_6m + 1
+	.word snooze_6m + 1
+	.word snooze_8m + 1
+	
+m0_reset:
+	ldr r0, =0x18000000
+	ldr r1, =0xaaaa0000
+	str r1, [r0]
+
+	bl init
+
+	// enable IRQ1 (Event From M4)
+	ldr r0, =0xE000E100
+	movs r1, #2
+	str r1, [r0]
+
+m0_idle:
+	wfi
+	b m0_idle
diff --git a/src/bsp/lk/app/mdebug/fw-m0sub.h b/src/bsp/lk/app/mdebug/fw-m0sub.h
new file mode 100644
index 0000000..173b358
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/fw-m0sub.h
@@ -0,0 +1,186 @@
+unsigned char zero_bin[] = {
+  0xf0, 0x3f, 0x00, 0x18, 0x41, 0x08, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+  0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+  0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+  0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+  0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18,
+  0x49, 0x00, 0x00, 0x18, 0x49, 0x00, 0x00, 0x18, 0xc5, 0x07, 0x00, 0x18,
+  0x04, 0x48, 0x05, 0x49, 0xef, 0xf3, 0x03, 0x82, 0xff, 0x23, 0x1a, 0x40,
+  0x11, 0x43, 0x01, 0x60, 0xfe, 0xe7, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18,
+  0x00, 0x00, 0xee, 0xee, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+  0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+  0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+  0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+  0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+  0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+  0xc0, 0x46, 0xc0, 0x46, 0x70, 0x47, 0x00, 0xb5, 0xc0, 0x46, 0xc0, 0x46,
+  0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+  0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x80, 0x0b, 0xc0, 0x46,
+  0xc0, 0x46, 0xc8, 0x47, 0x7c, 0x60, 0xc0, 0x46, 0xc0, 0x46, 0xc0, 0x46,
+  0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0xc0, 0xe0,
+  0x00, 0xb5, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00, 0x52, 0x44, 0xc8, 0x47,
+  0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x79, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0x9a, 0x03, 0x32, 0x40, 0x11, 0x00,
+  0x52, 0x44, 0xc8, 0x47, 0x7a, 0x60, 0x5b, 0x08, 0x59, 0x44, 0xc0, 0x46,
+  0xc0, 0x46, 0xc8, 0x47, 0x79, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0x34, 0xe1,
+  0x00, 0xb5, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+  0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+  0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+  0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+  0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+  0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+  0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+  0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+  0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+  0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+  0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+  0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+  0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+  0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+  0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+  0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+  0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+  0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+  0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+  0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+  0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+  0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+  0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+  0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+  0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+  0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+  0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+  0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+  0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+  0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+  0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+  0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+  0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+  0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+  0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+  0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+  0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+  0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+  0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+  0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+  0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+  0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+  0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+  0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+  0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68,
+  0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04,
+  0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46,
+  0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08,
+  0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46,
+  0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43,
+  0xc8, 0x47, 0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47,
+  0x7d, 0x60, 0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47,
+  0x7c, 0x60, 0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60,
+  0x30, 0x40, 0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0xc8, 0x47, 0x7c, 0x60,
+  0x38, 0x68, 0xc0, 0x46, 0xc0, 0x46, 0xc8, 0x47, 0x7d, 0x60, 0x30, 0x40,
+  0x40, 0x04, 0x5b, 0x08, 0x03, 0x43, 0x00, 0xbd, 0x5f, 0x48, 0x83, 0x46,
+  0x5f, 0x48, 0x82, 0x46, 0x5f, 0x48, 0x81, 0x46, 0x5f, 0x48, 0x80, 0x46,
+  0x5f, 0x4f, 0x60, 0x4e, 0x60, 0x4d, 0x61, 0x4c, 0x70, 0x47, 0x03, 0x20,
+  0x43, 0x46, 0xd8, 0x60, 0x00, 0xbd, 0x02, 0x20, 0x43, 0x46, 0xd8, 0x60,
+  0x00, 0xbd, 0x00, 0xb5, 0x5c, 0x48, 0x84, 0x46, 0x5b, 0x68, 0xff, 0xf7,
+  0xeb, 0xfc, 0x52, 0x4b, 0xbb, 0x60, 0xff, 0xf7, 0x79, 0xfe, 0x5b, 0x0f,
+  0x01, 0x2b, 0x09, 0xd0, 0x57, 0x49, 0xb9, 0x60, 0x02, 0x2b, 0xe6, 0xd1,
+  0x60, 0x46, 0x01, 0x38, 0x84, 0x46, 0xe6, 0xd0, 0x43, 0x46, 0xeb, 0xe7,
+  0xff, 0xf7, 0x6c, 0xfe, 0xff, 0xf7, 0xbd, 0xfc, 0x50, 0x49, 0xb9, 0x60,
+  0x59, 0x46, 0x31, 0x43, 0x79, 0x60, 0x41, 0x46, 0x4b, 0x60, 0x88, 0x60,
+  0x00, 0x20, 0xc8, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0x49, 0x48, 0x84, 0x46,
+  0x5b, 0x68, 0xff, 0xf7, 0xc5, 0xfc, 0x08, 0xb4, 0x3e, 0x4b, 0xbb, 0x60,
+  0xff, 0xf7, 0x52, 0xfe, 0x5b, 0x0f, 0x01, 0x2b, 0x0a, 0xd0, 0x01, 0xbc,
+  0x43, 0x49, 0xb9, 0x60, 0x02, 0x2b, 0xbe, 0xd1, 0x60, 0x46, 0x01, 0x38,
+  0x84, 0x46, 0xbe, 0xd0, 0x43, 0x46, 0xe9, 0xe7, 0x3e, 0x4b, 0xbb, 0x60,
+  0xff, 0xf7, 0x32, 0xfe, 0x43, 0x46, 0x9b, 0x68, 0xff, 0xf7, 0xac, 0xfc,
+  0x08, 0xbc, 0xff, 0xf7, 0x2b, 0xfe, 0x43, 0x46, 0x00, 0x20, 0xd8, 0x60,
+  0x00, 0xbd, 0x00, 0xb5, 0x37, 0x4b, 0x9c, 0x46, 0xff, 0xf7, 0xa0, 0xfc,
+  0x63, 0x46, 0xff, 0xf7, 0x9d, 0xfc, 0x35, 0x4b, 0xff, 0xf7, 0x98, 0xfc,
+  0x63, 0x46, 0xff, 0xf7, 0x97, 0xfc, 0x63, 0x46, 0xff, 0xf7, 0x94, 0xfc,
+  0x43, 0x46, 0x00, 0x20, 0xd8, 0x60, 0x00, 0xbd, 0x00, 0xb5, 0x2f, 0x48,
+  0x00, 0x21, 0x01, 0x60, 0x43, 0x46, 0x18, 0x68, 0x05, 0x28, 0x00, 0xd9,
+  0x00, 0x20, 0x80, 0x00, 0x01, 0xa1, 0x0a, 0x58, 0x90, 0x47, 0x00, 0xbd,
+  0xf9, 0x07, 0x00, 0x18, 0xff, 0x07, 0x00, 0x18, 0xff, 0x06, 0x00, 0x18,
+  0x4b, 0x07, 0x00, 0x18, 0x9b, 0x07, 0x00, 0x18, 0x05, 0x08, 0x00, 0x18,
+  0x09, 0x20, 0xd8, 0x60, 0x70, 0x47, 0x00, 0x20, 0xd8, 0x60, 0x70, 0x47,
+  0x58, 0x68, 0x08, 0x28, 0x00, 0xd9, 0x00, 0x20, 0x80, 0x00, 0x03, 0xa1,
+  0x09, 0x58, 0x89, 0x46, 0x00, 0x20, 0xd8, 0x60, 0x70, 0x47, 0xc0, 0x46,
+  0x65, 0x00, 0x00, 0x18, 0x65, 0x00, 0x00, 0x18, 0x65, 0x00, 0x00, 0x18,
+  0x85, 0x00, 0x00, 0x18, 0x95, 0x00, 0x00, 0x18, 0x95, 0x00, 0x00, 0x18,
+  0xa5, 0x00, 0x00, 0x18, 0xa5, 0x00, 0x00, 0x18, 0xad, 0x00, 0x00, 0x18,
+  0x11, 0x48, 0x12, 0x49, 0x01, 0x60, 0xff, 0xf7, 0x45, 0xff, 0x11, 0x48,
+  0x02, 0x21, 0x01, 0x60, 0x30, 0xbf, 0xfd, 0xe7, 0x00, 0x88, 0x00, 0x00,
+  0x00, 0x80, 0x00, 0x00, 0x95, 0x00, 0x00, 0x18, 0x00, 0x40, 0x00, 0x18,
+  0x10, 0x12, 0x10, 0x40, 0x00, 0x40, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00,
+  0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00,
+  0xff, 0xff, 0xff, 0xff, 0x9e, 0xe7, 0x00, 0x00, 0x30, 0x31, 0x04, 0x40,
+  0x00, 0x00, 0x00, 0x18, 0x00, 0x00, 0xaa, 0xaa, 0x00, 0xe1, 0x00, 0xe0
+};
+unsigned int zero_bin_len = 2196;
diff --git a/src/bsp/lk/app/mdebug/jtag.c b/src/bsp/lk/app/mdebug/jtag.c
new file mode 100644
index 0000000..517ab5c
--- /dev/null
+++ b/src/bsp/lk/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/src/bsp/lk/app/mdebug/lpclink2.h b/src/bsp/lk/app/mdebug/lpclink2.h
new file mode 100644
index 0000000..ab29a8d
--- /dev/null
+++ b/src/bsp/lk/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/src/bsp/lk/app/mdebug/mdebug.c b/src/bsp/lk/app/mdebug/mdebug.c
new file mode 100644
index 0000000..f186f39
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/mdebug.c
@@ -0,0 +1,160 @@
+/* mdebug.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 <dev/udc.h>
+
+#include <platform.h>
+#include <arch/arm.h>
+#include <kernel/thread.h>
+#include <kernel/event.h>
+
+#include <platform/lpc43xx-gpio.h>
+
+#include "swd.h"
+
+#define TX_AHEAD 1
+
+static event_t txevt = EVENT_INITIAL_VALUE(txevt, TX_AHEAD, 0);
+static event_t rxevt = EVENT_INITIAL_VALUE(rxevt, 0, 0);
+
+static udc_request_t *txreq;
+static udc_request_t *rxreq;
+static udc_endpoint_t *txept;
+static udc_endpoint_t *rxept;
+
+static volatile int online;
+static volatile int txstatus;
+static volatile int rxstatus;
+static volatile int rxactual;
+
+static void mdebug_notify(udc_gadget_t *gadget, unsigned event) {
+	if (event == UDC_EVENT_ONLINE) {
+		online = 1;
+	} else {
+		online = 0;
+	}
+}
+
+static void rx_complete(udc_request_t *req, unsigned actual, int status) {
+	rxactual = actual;
+	rxstatus = status;
+	event_signal(&rxevt, 0);
+}
+
+static void tx_complete(udc_request_t *req, unsigned actual, int status) {
+	txstatus = status;
+	event_signal(&txevt, 0);
+}
+
+#if TX_AHEAD
+void usb_xmit(void *data, unsigned len) {
+	event_wait(&txevt);
+	event_unsignal(&txevt);
+	txreq->buffer = data;
+	txreq->length = len;
+	txstatus = 1;
+	if (udc_request_queue(txept, txreq)) {
+		printf("txqf\n");
+		event_signal(&txevt, 0);
+	}
+}
+#else
+void usb_xmit(void *data, unsigned len) {
+	event_unsignal(&txevt);
+	txreq->buffer = data;
+	txreq->length = len;
+	txstatus = 1;
+	if (udc_request_queue(txept, txreq) == 0) {
+		event_wait(&txevt);
+	}
+}
+#endif
+
+int usb_recv(void *data, unsigned len) {
+	event_unsignal(&rxevt);
+	rxreq->buffer = data;
+	rxreq->length = len;
+	rxstatus = 1;
+	if (udc_request_queue(rxept, rxreq)) {
+		printf("rxqf\n");
+		return -1;
+	}
+	event_wait(&rxevt);
+	return rxstatus ? rxstatus : rxactual;
+}
+
+static udc_device_t mdebug_device = {
+	.vendor_id = 0x1209,
+	.product_id = 0x5038,
+	.version_id = 0x0100,
+};
+
+static udc_endpoint_t *mdebug_endpoints[2];
+
+static udc_gadget_t mdebug_gadget = {
+	.notify = mdebug_notify,
+	.ifc_class = 0xFF,
+	.ifc_subclass = 0xFF,
+	.ifc_protocol = 0xFF,
+	.ifc_endpoints = 2,
+	.ept = mdebug_endpoints,
+};
+
+static void mdebug_init(const struct app_descriptor *app)
+{
+	swd_init();
+
+	udc_init(&mdebug_device);
+	mdebug_endpoints[0] = txept = udc_endpoint_alloc(UDC_BULK_IN, 512);
+	mdebug_endpoints[1] = rxept = udc_endpoint_alloc(UDC_BULK_OUT, 512);
+	txreq = udc_request_alloc();
+	rxreq = udc_request_alloc();
+	rxreq->complete = rx_complete;
+	txreq->complete = tx_complete;
+	udc_register_gadget(&mdebug_gadget);
+}
+
+void handle_rswd(void);
+void swo_init(udc_endpoint_t *ept);
+void swo_config(unsigned mhz);
+
+static void mdebug_entry(const struct app_descriptor *app, void *args)
+{
+	udc_start();
+	swo_init(txept);
+	swo_config(6000000);
+
+	for (;;) {
+		if (!online) {
+			thread_yield();
+			continue;
+		}
+		handle_rswd();
+	}
+}
+
+APP_START(usbtest)
+	.init = mdebug_init,
+	.entry = mdebug_entry,
+APP_END
+
+
diff --git a/src/bsp/lk/app/mdebug/rswd.c b/src/bsp/lk/app/mdebug/rswd.c
new file mode 100644
index 0000000..7daa8b7
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/rswd.c
@@ -0,0 +1,319 @@
+/* rswd.c
+ *
+ * Copyright 2011-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 <reg.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+
+#include <platform.h>
+
+#include "swd.h"
+#include "rswdp.h"
+
+void usb_xmit(void *data, unsigned len);
+int usb_recv(void *data, unsigned len);
+
+unsigned swdp_trace = 0;
+
+// indicates host knows about v1.0 protocol features
+unsigned host_version = 0;
+
+static u8 optable[16] = {
+	[OP_RD | OP_DP | OP_X0] = RD_IDCODE,
+	[OP_RD | OP_DP | OP_X4] = RD_DPCTRL,
+	[OP_RD | OP_DP | OP_X8] = RD_RESEND,
+	[OP_RD | OP_DP | OP_XC] = RD_BUFFER,
+	[OP_WR | OP_DP | OP_X0] = WR_ABORT,
+	[OP_WR | OP_DP | OP_X4] = WR_DPCTRL,
+	[OP_WR | OP_DP | OP_X8] = WR_SELECT,
+	[OP_WR | OP_DP | OP_XC] = WR_BUFFER,
+	[OP_RD | OP_AP | OP_X0] = RD_AP0,
+	[OP_RD | OP_AP | OP_X4] = RD_AP1,
+	[OP_RD | OP_AP | OP_X8] = RD_AP2,
+	[OP_RD | OP_AP | OP_XC] = RD_AP3,
+	[OP_WR | OP_AP | OP_X0] = WR_AP0,
+	[OP_WR | OP_AP | OP_X4] = WR_AP1,
+	[OP_WR | OP_AP | OP_X8] = WR_AP2,
+	[OP_WR | OP_AP | OP_XC] = WR_AP3,
+};
+
+static const char *board_str = TARGET;
+static const char *build_str = "fw v0.91 (" __DATE__ ", " __TIME__ ")";
+
+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;
+	unsigned txc = 1;
+	unsigned count = 0;
+	unsigned status = 0;
+	void (*func)(void) = 0;
+
+	tx[0] = txnid;
+
+	while (rxc-- > 0) {
+		count++;
+		msg = *rx++;
+		op = RSWD_MSG_OP(msg);
+		n = RSWD_MSG_ARG(msg);
+#if CONFIG_MDEBUG_TRACE
+		printf("> %02x %02x %04x <\n", RSWD_MSG_CMD(msg), op, n);
+#endif
+		switch (RSWD_MSG_CMD(msg)) {
+		case CMD_NULL:
+			continue;
+		case CMD_SWD_WRITE:
+			while (n-- > 0) {
+				rxc--;
+				status = swd_write(optable[op], *rx++);
+				if (status) {
+					goto done;
+				}
+			}
+			continue;
+		case CMD_SWD_READ:
+			tx[txc++] = RSWD_MSG(CMD_SWD_DATA, 0, n);
+			while (n-- > 0) {
+				status = swd_read(optable[op], tx + txc);
+				if (status) {
+					txc++;
+					while (n-- > 0)
+						tx[txc++] = 0xfefefefe;
+					goto done;
+				}
+				txc++;
+			}
+			continue;
+		case CMD_SWD_DISCARD:
+			while (n-- > 0) {
+				u32 tmp;
+				status = swd_read(optable[op], &tmp);
+				if (status) {
+					goto done;
+				}
+			}
+			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;
+		case CMD_DOWNLOAD: {
+			//u32 *addr = (void*) *rx++;
+			rxc--;
+			while (n) {
+				//*addr++ = *rx++;
+				rx++;
+				rxc--;
+			}
+			continue;
+		}
+		case CMD_EXECUTE:
+			//func = (void*) *rx++;
+			rxc--;
+			continue;
+		case CMD_TRACE:
+			swdp_trace = op;
+			continue;
+		case CMD_BOOTLOADER:
+			func = _reboot;
+			continue;
+		case CMD_SET_CLOCK:
+			n = swd_set_clock(n);
+			printf("swdp clock is now %d KHz\n", n);
+			if (host_version >= RSWD_VERSION_1_0) {
+				tx[txc++] = RSWD_MSG(CMD_CLOCK_KHZ, 0, n);
+			}
+			continue;
+		case CMD_SWO_CLOCK:
+			n = swo_set_clock(n);
+			printf("swo clock is now %d KHz\n", n);
+			continue;
+		case CMD_VERSION:
+			host_version = n;
+			tx[txc++] = RSWD_MSG(CMD_VERSION, 0, RSWD_VERSION);
+
+			n = strlen(board_str);
+			memcpy(tx + txc + 1, board_str, n + 1);
+			n = (n + 4) / 4;
+			tx[txc++] = RSWD_MSG(CMD_BOARD_STR, 0, n);
+			txc += n;
+
+			n = strlen(build_str);
+			memcpy(tx + txc + 1, build_str, n + 1);
+			n = (n + 4) / 4;
+			tx[txc++] = RSWD_MSG(CMD_BUILD_STR, 0, n);
+			txc += n;
+
+			tx[txc++] = RSWD_MSG(CMD_RX_MAXDATA, 0, 8192);
+			txc += n;
+			continue;
+		default:
+			printf("unknown command %02x\n", RSWD_MSG_CMD(msg));
+			status = 1;
+			goto done;
+		}
+	}
+
+done:
+	tx[txc++] = RSWD_MSG(CMD_STATUS, status, count);
+
+	/* if we're about to send an even multiple of the packet size
+	 * (64), add a NULL op on the end to create a short packet at
+	 * the end.
+	 */
+	if ((txc & 0xf) == 0)
+		tx[txc++] = RSWD_MSG(CMD_NULL, 0, 0);
+
+#if CONFIG_MDEBUG_TRACE
+	printf("[ send %d words ]\n", txc);
+	for (n = 0; n < txc; n+=4) {
+		printx("%08x %08x %08x %08x\n",
+			tx[n], tx[n+1], tx[n+2], tx[n+3]);
+	}
+#endif
+	usb_xmit(tx, txc * 4);
+
+	if (func) {
+		for (n = 0; n < 1000000; n++) asm("nop");
+		func();
+		for (;;) ;
+	}
+}
+
+// io buffers in AHB SRAM
+static u32 *rxbuffer = (void*) 0x20001000;
+static u32 *txbuffer[2] = {(void*) 0x20003000, (void*) 0x20005000 };
+
+#include <kernel/thread.h>
+
+void handle_rswd(void) {
+	int rxc;
+	int toggle = 0;
+
+#if CONFIG_MDEBUG_TRACE
+	printf("[ rswdp agent v0.9 ]\n");
+	printf("[ built " __DATE__ " " __TIME__ " ]\n");
+#endif
+
+	for (;;) {
+		rxc = usb_recv(rxbuffer, 8192);
+
+#if CONFIG_MDEBUG_TRACE
+		int n;
+		printx("[ recv %d words ]\n", rxc/4);
+		for (n = 0; n < (rxc/4); n+=4) {
+			printx("%08x %08x %08x %08x\n",
+				rxbuffer[n], rxbuffer[n+1],
+				rxbuffer[n+2], rxbuffer[n+3]);
+		}
+#endif
+
+		if ((rxc < 4) || (rxc & 3)) {
+			printf("error, runt frame, or strange frame... %d\n", rxc);
+			continue;
+		}
+
+		rxc = rxc / 4;
+
+		if ((rxbuffer[0] & 0xFFFF0000) != 0xAA770000) {
+			printf("invalid frame %x\n", rxbuffer[0]);
+			continue;
+		}
+
+		process_txn(rxbuffer[0], rxbuffer + 1, rxc - 1, txbuffer[toggle]);
+		toggle ^= 1;
+	}
+}
diff --git a/src/bsp/lk/app/mdebug/rswdp.h b/src/bsp/lk/app/mdebug/rswdp.h
new file mode 100644
index 0000000..0de0aea
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/rswdp.h
@@ -0,0 +1,127 @@
+/* rswdp.h - remote serial wire debug protocol
+ *
+ * Copyright 2011 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.
+ */
+
+/* Remote Serial Wire Debug Protocol */
+
+#ifndef _RSWDP_PROTOCOL_H_
+#define _RSWDP_PROTOCOL_H_
+
+/* Basic framing:
+ * - host and device exchange "transactions" consisting of
+ *   some number of "messages".
+ * - each "message" has a 32bit header and may have 0 or more
+ *   32bit words of payload
+ * - a transaction may not exceed 4K (1024 words)
+ * - a transaction is sent in a series of USB BULK packets
+ * - the final packet must be a short packet unless the
+ *   transaction is exactly 4K in length
+ * - packets must be a multiple of 4 bytes
+ * - the first message in a transaction must be
+ *   CMD_TXN_START or CMD_TXN_ASYNC
+ */
+
+#define RSWD_MSG(cmd,op,n)	((((cmd)&0xFF) << 24) | (((op) & 0xFF)<<16) | ((n) & 0xFFFF))
+#define RSWD_MSG_CMD(n)		(((n) >> 24) & 0xFF)
+#define RSWD_MSG_OP(n)		(((n) >> 16) & 0xFF)
+#define RSWD_MSG_ARG(n)		((n) & 0xFFFF)
+
+#define RSWD_TXN_START(seq)	(0xAA770000 | ((seq) & 0xFFFF))
+#define RSWD_TXN_ASYNC		(0xAA001111)
+
+/* valid: either */
+#define CMD_NULL	0x00 /* used for padding */
+
+/* valid: host to target */
+#define CMD_SWD_WRITE	0x01 /* op=addr arg=count payload: data x count */
+#define CMD_SWD_READ	0x02 /* op=addr arg=count payload: data x count */
+#define CMD_SWD_DISCARD	0x03 /* op=addr arg=count payload: none (discards) */
+#define CMD_ATTACH	0x04 /* do swdp reset/connect handshake */
+#define CMD_RESET	0x05 /* arg=1 -> assert RESETn, otherwise deassert */
+#define CMD_DOWNLOAD	0x06 /* arg=wordcount, payload: addr x 1, data x n */
+#define CMD_EXECUTE	0x07 /* payload: addr x 1 */
+#define CMD_TRACE	0x08 /* op=tracebits n=0 */
+#define CMD_BOOTLOADER	0x09 /* return to bootloader for reflashing */
+#define CMD_SET_CLOCK	0x0A /* set SWCLK rate to n khz */
+#define CMD_SWO_CLOCK	0x0B /* set SWOCLK rate to n khz, 0 = disable SWO */
+#define CMD_JTAG_IO	0x0C /* op=0, arg=bitcount, data x (count/32) * 2 */
+			     /* tms, tdi word pairs per 32bits */
+#define CMD_JTAG_TX	0x0D /* tms=op.0, arg=bitcount, data x (count/32) words of tdi */
+#define CMD_JTAG_RX	0x0E /* tms=op.0, tdi=op.1, arg=bitcount, return (count/32) words */
+#define CMD_JTAG_VRFY	0x0F /* arg=bitcount, tms/tdi data/mask, error if tdo&mask != data */
+
+/* valid: target to host */
+#define CMD_STATUS	0x10 /* op=errorcode, arg=commands since last TXN_START */
+#define CMD_SWD_DATA	0x11 /* op=0 arg=count, payload: data x count */
+#define CMD_SWO_DATA	0x12 /* op=0 arg=count, payload: ((count+3)/4) words */
+#define CMD_JTAG_DATA	0x14 /* op=0 arg=bitcount, payload: (arg/32) words */
+
+/* valid: target to host async */
+#define CMD_DEBUG_PRINT	0x20 /* arg*4 bytes of ascii debug output */
+
+/* valid: bidirectional query/config messages */
+#define CMD_VERSION	0x30 /* arg=bcdversion (0x0100 etc) */
+#define CMD_BUILD_STR	0x31 /* arg=wordcount, payload = asciiz */
+#define CMD_BOARD_STR	0x32 /* arg=wordcount, payload = asciiz */
+#define CMD_RX_MAXDATA	0x33 /* arg=bytes, declares senders rx buffer size */
+#define CMD_CLOCK_KHZ	0x34 /* arg=khz, reports active clock rate */
+
+/* CMD_STATUS error codes */
+#define ERR_NONE	0
+#define ERR_INTERNAL	1
+#define ERR_TIMEOUT	2
+#define ERR_IO		3
+#define ERR_PARITY	4
+#define ERR_BAD_MATCH	5
+
+#define RSWD_VERSION		0x0102
+
+#define RSWD_VERSION_1_0	0x0100
+#define RSWD_VERSION_1_1	0x0101
+#define RSWD_VERSION_1_2	0x0102
+
+// Pre-1.0
+//  - max packet size fixed at 2048 bytes
+//
+// Version 1.0
+// - CMD_VERSION, CMD_BUILD_STR, CMD_BOARD_STR, CMD_RX_MAXDATA,
+//   CMD_CLOCK_KHZ added
+//
+// Version 1.1
+// - CMD_SWO_DATA arg is now byte count, not word count
+//
+// Version 1.2
+// - CMD_JTAG_IO, CMD_JTAG_RX, CMD_JTAG_TX, CMD_JTAG_VRFY, CMD_JTAG_DATA added
+
+/* CMD_SWD_OP operations - combine for direct AP/DP io */
+#define OP_RD 0x00
+#define OP_WR 0x01
+#define OP_DP 0x00
+#define OP_AP 0x02
+#define OP_X0 0x00
+#define OP_X4 0x04
+#define OP_X8 0x08
+#define OP_XC 0x0C
+
+/* DP registers */
+#define DP_IDCODE	(OP_DP|OP_X0)
+#define DP_ABORT	(OP_DP|OP_X0)
+#define DP_DPCTRL	(OP_DP|OP_X4)
+#define DP_RESEND	(OP_DP|OP_X8)
+#define DP_SELECT	(OP_DP|OP_X8)
+#define DP_BUFFER	(OP_DP|OP_XC)
+
+#endif
diff --git a/src/bsp/lk/app/mdebug/rules.mk b/src/bsp/lk/app/mdebug/rules.mk
new file mode 100644
index 0000000..168d3aa
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/rules.mk
@@ -0,0 +1,13 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+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
+
+include make/module.mk
+
diff --git a/src/bsp/lk/app/mdebug/swd-m0sub.c b/src/bsp/lk/app/mdebug/swd-m0sub.c
new file mode 100644
index 0000000..63ce42a
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/swd-m0sub.c
@@ -0,0 +1,184 @@
+/* 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_SWDIO_TXEN, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+	pin_config(PIN_SWDIO, PIN_MODE(6) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+	pin_config(PIN_SWCLK, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+
+	pin_config(PIN_SWO, PIN_MODE(1) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+
+	gpio_set(GPIO_LED, 0);
+	gpio_set(GPIO_RESET, 1);
+	gpio_set(GPIO_RESET_TXEN, 0);
+
+	gpio_config(GPIO_LED, GPIO_OUTPUT);
+	gpio_config(GPIO_RESET, GPIO_OUTPUT);
+	gpio_config(GPIO_RESET_TXEN, GPIO_OUTPUT);
+}
+
+
+/* returns 1 if the number of bits set in n is odd */
+static unsigned parity(unsigned n) {
+        n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
+        n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
+        n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
+        n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
+        n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
+        return n & 1;
+}
+
+#include "fw-m0sub.h"
+
+#define M0SUB_ZEROMAP		0x40043308
+#define M0SUB_TXEV		0x40043314 // write 0 to clear
+#define M4_TXEV			0x40043130 // write 0 to clear
+
+#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 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
+
+void swd_init(void) {
+	gpio_init();
+
+	writel(BASE_CLK_SEL(CLK_PLL1), BASE_PERIPH_CLK);
+	spin(1000);
+
+	// SGPIO15 SWDIO_TXEN
+	writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(15));
+	// SGPIO14 SWDIO
+	writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(14));
+	// SGPIO11 SWCLK
+	writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(11));
+
+	// all outputs enabled and high
+	writel((1 << 11) | (1 << 14) | (1 << 15), SGPIO_OUT);
+	writel((1 << 11) | (1 << 14) | (1 << 15), SGPIO_OEN);
+
+	writel(0, M4_TXEV);
+	writel(M0_SUB_RST, RESET_CTRL0);
+	writel(0x18000000, M0SUB_ZEROMAP);
+	writel(0xffffffff, 0x18004000);
+	memcpy((void*) 0x18000000, zero_bin, sizeof(zero_bin));
+	DSB;
+	writel(0, RESET_CTRL0);
+}
+
+int swd_write(unsigned hdr, unsigned data) {
+	unsigned n;
+	unsigned p = parity(data);
+	writel(M0_CMD_WRITE, COMM_CMD);
+	writel((hdr << 8) | (p << 16), COMM_ARG1);
+	writel(data, COMM_ARG2);
+	writel(RSP_BUSY, COMM_RESP);
+	DSB;
+	asm("sev");
+	while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
+	//printf("wr s=%d\n", n);
+	return n;
+}
+
+int swd_read(unsigned hdr, unsigned *val) {
+	unsigned n, data, p;
+	writel(M0_CMD_READ, COMM_CMD);
+	writel(hdr << 8, COMM_ARG1);
+	writel(RSP_BUSY, COMM_RESP);
+	DSB;
+	asm("sev");
+	while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
+	if (n) {
+		return n;
+	}
+	data = readl(COMM_ARG1);
+	p = readl(COMM_ARG2);
+	if (p != parity(data)) {
+		return ERR_PARITY;
+	}
+	//printf("rd s=%d p=%d d=%08x\n", n, p, data);
+	*val = data;
+	return 0;
+}
+
+void swd_reset(void) {
+	unsigned n;
+	writel(M0_CMD_RESET, COMM_CMD);
+	writel(RSP_BUSY, COMM_RESP);
+	DSB;
+	asm("sev");
+	while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
+}
+
+unsigned swd_set_clock(unsigned khz) {
+	unsigned n;
+	if (khz > 8000) {
+		khz = 8000;
+	}
+	writel(M0_CMD_SETCLOCK, COMM_CMD);
+	writel(khz/1000, COMM_ARG1);
+	writel(RSP_BUSY, COMM_RESP);
+	DSB;
+	asm("sev");
+	while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
+
+	// todo: accurate value
+	return khz;
+}
+
+void swd_hw_reset(int assert) {
+	if (assert) {
+		gpio_set(GPIO_RESET, 0);
+		gpio_set(GPIO_RESET_TXEN, 1);
+	} else {
+		gpio_set(GPIO_RESET, 1);
+		gpio_set(GPIO_RESET_TXEN, 0);
+	}
+}
diff --git a/src/bsp/lk/app/mdebug/swd-sgpio.c b/src/bsp/lk/app/mdebug/swd-sgpio.c
new file mode 100644
index 0000000..5221387
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/swd-sgpio.c
@@ -0,0 +1,366 @@
+/* swdp-sgpio.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 <dev/udc.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>
+
+#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)
+
+static unsigned sgpio_div = 31; // 6MHz
+
+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_SWDIO_TXEN, PIN_MODE(0) | PIN_PLAIN);
+	pin_config(PIN_SWDIO, PIN_MODE(0) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+	pin_config(PIN_SWCLK, PIN_MODE(0) | PIN_PLAIN | PIN_FAST);
+	pin_config(PIN_SWO, PIN_MODE(1) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+
+	gpio_set(GPIO_LED, 0);
+	gpio_set(GPIO_RESET, 1);
+	gpio_set(GPIO_RESET_TXEN, 0);
+	gpio_set(GPIO_SWDIO, 0);
+	gpio_set(GPIO_SWDIO_TXEN, 1);
+	gpio_set(GPIO_SWCLK, 0);
+
+	gpio_config(GPIO_LED, GPIO_OUTPUT);
+	gpio_config(GPIO_RESET, GPIO_OUTPUT);
+	gpio_config(GPIO_RESET_TXEN, GPIO_OUTPUT);
+	gpio_config(GPIO_SWDIO, GPIO_OUTPUT);
+	gpio_config(GPIO_SWDIO_TXEN, GPIO_OUTPUT);
+	gpio_config(GPIO_SWCLK, GPIO_OUTPUT);
+}
+
+/* returns 1 if the number of bits set in n is odd */
+static unsigned parity(unsigned n) {
+        n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
+        n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
+        n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
+        n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
+        n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
+        return n & 1;
+}
+
+static void sgpio_txn(unsigned slices) {
+	// clear any previous status bits
+	writel(0xFFFF, SLICE_XHG_STS_CLR);
+	// kick the txn
+	writel(slices, SLICE_CTRL_ENABLE);
+	writel(slices, SLICE_CTRL_DISABLE);
+	// wait for all slices to complete
+	while ((readl(SLICE_XHG_STS) & slices) != slices) ;
+	// shut down clocks
+	writel(0, SLICE_CTRL_ENABLE);
+	writel(0, SLICE_CTRL_DISABLE);
+}
+
+// SWDIO       SLICE_H      SLICE_P        SLICE_D      SLICE_O     SWDIO
+// SGPIO14 -> [31....0] -> [31....0]      [31....0] -> [31....0] -> SGPIO14
+//
+//                                         SLICE_M     SWDIO_TXEN
+//                                        [31....7] -> SGPIO15
+//
+//                                         SLICE_F     SWDIO_OE
+//                                        [31....0] -> SGPIO14_OE
+
+
+// configures all slices, muxes, etc
+// ensures that outputs are enabled and SWDIO and SWCLK are high
+static void sgpio_init(void) {
+	writel(BASE_CLK_SEL(CLK_PLL1), BASE_PERIPH_CLK);
+
+	// make sure everything's shut down
+	writel(0, SLICE_CTRL_ENABLE);
+	writel(0, SLICE_CTRL_DISABLE);
+	writel(0xFFFF, SLICE_XHG_STS_CLR);
+
+	// SWDIO_TXEN (SGPIO15)
+	// M[31..7] -> OUT
+	writel(CFG_OUT_M8B | CFG_OE_GPIO, SGPIO_OUT_CFG(15));
+	writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE, SLICE_CFG1(SLC_M));
+	writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_M));
+
+	// SWDIO (SGPIO14)
+	// IN -> H[31..0] -> P[31..0]
+	//       D[31..0] -> O[31..0] -> OUT
+	//                   F[31..0] -> OE
+	writel(CFG_OUT_M2C | CFG_OE_M1, SGPIO_OUT_CFG(14));
+	writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_PIN, SLICE_CFG1(SLC_H));
+	writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_H));
+	writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE | CONCAT_2_SLICE, SLICE_CFG1(SLC_P));
+	writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_P));
+	writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE, SLICE_CFG1(SLC_D));
+	writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_D));
+	writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE | CONCAT_2_SLICE, SLICE_CFG1(SLC_O));
+	writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_O));
+	writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE, SLICE_CFG1(SLC_F));
+	writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_F));
+
+	// SWDCLK (SGPIO11)
+	// SLICE_N CLK -> OUT
+	writel(CFG_OUT_CLK | CFG_OE_GPIO, SGPIO_OUT_CFG(11));
+	writel(CLK_USE_SLICE | QUAL_ENABLE, SLICE_CFG1(SLC_N));
+	writel(CLK_GEN_INTERNAL | SHIFT_1BPC | INV_CLK_OUT, SLICE_CFG2(SLC_N));
+
+	// ensure output and enables idle high
+	writel(1, SLICE_REG(SLC_F));
+	writel(1, SLICE_REG(SLC_O));
+	writel(1 << 7, SLICE_REG(SLC_M));
+
+	// enable all outputs
+	writel((1 << 11) | (1 << 14) | (1 << 15), SGPIO_OEN);
+
+	// select SGPIOs via pin mux
+	pin_config(PIN_SWDIO_TXEN, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+	pin_config(PIN_SWDIO, PIN_MODE(6) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
+	pin_config(PIN_SWCLK, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
+}
+
+static void sgpio_swd_clock_setup(unsigned div) {
+	writel(div, SLICE_PRESET(SLC_D));
+	writel(div, SLICE_PRESET(SLC_F));
+	writel(div, SLICE_PRESET(SLC_H));
+	writel(div, SLICE_PRESET(SLC_M));
+	writel(div, SLICE_PRESET(SLC_N));
+	writel(div, SLICE_PRESET(SLC_O));
+	writel(div, SLICE_PRESET(SLC_P));
+}
+
+static void sgpio_swd_reset(unsigned div) {
+	// shift out 64 clocks while DATA is 1
+	writel(0, SLICE_COUNT(SLC_N));
+	writel(POS_POS(63) | POS_RESET(63), SLICE_POS(SLC_N));
+	sgpio_txn(1 << SLC_N);
+
+	// shift out 16bit jtag->swd escape pattern
+	writel(0, SLICE_COUNT(SLC_N));
+	writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_N));
+
+	writel(0b1110011110011110, SLICE_REG(SLC_O));
+	writel(1, SLICE_SHADOW(SLC_O));
+	writel(div, SLICE_COUNT(SLC_O));
+	writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_O));
+	sgpio_txn((1 << SLC_N) | (1 << SLC_O));
+
+	// shift out 64 clocks while DATA is 1
+	writel(0, SLICE_COUNT(SLC_N));
+	writel(POS_POS(63) | POS_RESET(63), SLICE_POS(SLC_N));
+	sgpio_txn(1 << SLC_N);
+};
+
+// shift out 8 0s then the 8bit header, then disable outputs
+// and shift in the turnaround bit (ignored) and 3 bit ack
+// leaves output enables low
+//
+// todo: make leader optional/adjustable
+static int sgpio_swd_header(unsigned div, uint32_t hdr) {
+	unsigned timeout = 16;
+	unsigned ack;
+
+	for (;;) {
+		// 16 bits tx_en, then stop, disabling tx_en
+		writel(0xFFFF << 7, SLICE_REG(SLC_M));
+		writel(0, SLICE_SHADOW(SLC_M));
+		writel(div, SLICE_COUNT(SLC_M));
+		writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_M));
+
+		// 16 bits output, then stop, disabling OE
+		writel(0xFFFF, SLICE_REG(SLC_F));
+		writel(0, SLICE_SHADOW(SLC_F));
+		writel(div, SLICE_COUNT(SLC_F));
+		writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_F));
+
+		// 16 bits data out
+		writel(hdr << 8, SLICE_REG(SLC_O));
+		writel(1, SLICE_SHADOW(SLC_O));
+		writel(div, SLICE_COUNT(SLC_O));
+		writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_O));
+
+		// 20 bits data in
+		writel(0, SLICE_COUNT(SLC_H));
+		writel(POS_POS(19) | POS_RESET(19), SLICE_POS(SLC_H));
+		writel(0, SLICE_REG(SLC_H));
+
+		// 20 bits clock
+		writel(0, SLICE_COUNT(SLC_N));
+		writel(POS_POS(19) | POS_RESET(19), SLICE_POS(SLC_N));
+
+		sgpio_txn((1<<SLC_M)|(1<<SLC_F)|(1<<SLC_O)|(1<<SLC_H)|(1<<SLC_N));
+
+		if ((ack = readl(SLICE_SHADOW(SLC_H)) >> 29) == 1) {
+			// OKAY
+			if (timeout < 16) printf("[%d]\n",16-timeout);
+			return 0;
+		}
+
+		// re-enable oe, tx_en, and make data high
+		writel(1, SLICE_REG(SLC_O));
+		writel(1 << 7, SLICE_REG(SLC_M));
+		writel(1, SLICE_REG(SLC_F));
+	
+		// technically we should do a Turn cycle here,
+		// but we rely on the fact that we prefix all ops
+		// with some leader 0s and can be lazy
+
+		if (ack == 2) {
+			// WAIT
+			if (timeout == 0) {
+				return -1;
+			}
+			timeout--;
+		} else {
+			printf("ERR %d\n", ack);
+			// FAULT or invalid response
+			return -1;
+		}
+	}
+}
+
+static int sgpio_swd_read(unsigned div, uint32_t hdr, uint32_t *_data) {
+	uint32_t data, p;
+	
+	//printf("rd(%d,%02x)\n", div, hdr);
+	if (sgpio_swd_header(div, hdr)) {
+		return -1;
+	}
+
+	// 34 bits in -> H -> P
+	writel(0, SLICE_COUNT(SLC_H));
+	writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_H));
+	writel(0, SLICE_REG(SLC_H));
+	writel(0, SLICE_COUNT(SLC_P));
+	writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_P));
+	writel(0, SLICE_REG(SLC_P));
+
+	writel(0, SLICE_COUNT(SLC_N));
+	writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_N));
+	sgpio_txn((1<<SLC_H)|(1<<SLC_P)|(1<<SLC_N));
+
+	// re-enable oe, tx_en, and make data high
+	writel(1, SLICE_REG(SLC_O));
+	writel(1 << 7, SLICE_REG(SLC_M));
+	writel(1, SLICE_REG(SLC_F));
+
+	p = readl(SLICE_SHADOW(SLC_H));
+	data = (p << 2) | (readl(SLICE_SHADOW(SLC_P)) >> 30);
+	p = (p >> 30) & 1;
+
+	//printf("RD %08x %d\n", data, p);
+	if (parity(data) != p) {
+		printf("parity error\n");
+		return -1;
+	}
+	*_data = data;
+	return 0;
+}
+
+static int sgpio_swd_write(unsigned div, uint32_t hdr, uint32_t data) {
+	uint32_t p = parity(data);
+
+	//printf("wr(%d,%02x,%08x) p=%d\n", div, hdr, data, p);
+	if (sgpio_swd_header(div, hdr)) {
+		return -1;
+	}
+
+	// 34 bits  D -> O -> out
+	writel(div, SLICE_COUNT(SLC_D));
+	writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_D));
+	writel((p << 1) | (data >> 31), SLICE_REG(SLC_D));
+	writel(div, SLICE_COUNT(SLC_O));
+	writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_O));
+	writel((data << 1) | 1, SLICE_REG(SLC_O));
+
+	writel(0, SLICE_COUNT(SLC_N));
+	writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_N));
+	
+	// re-enable oe, tx_en
+	writel(1 << 7, SLICE_REG(SLC_M));
+	writel(1, SLICE_REG(SLC_F));
+
+	sgpio_txn((1<<SLC_D)|(1<<SLC_O)|(1<<SLC_N));
+
+	return 0;
+}
+
+void swd_init(void) {
+	gpio_init();
+	sgpio_init();
+}
+
+void swd_reset(void) {
+	unsigned div = sgpio_div;
+	sgpio_swd_clock_setup(div);
+	sgpio_swd_reset(div);
+}
+
+int swd_read(unsigned reg, unsigned *val) {
+	unsigned div = sgpio_div;
+	sgpio_swd_clock_setup(div);
+	return sgpio_swd_read(div, reg, val);
+}
+
+int swd_write(unsigned reg, unsigned val) {
+	unsigned div = sgpio_div;
+	sgpio_swd_clock_setup(div);
+	return sgpio_swd_write(div, reg, val);
+}
+
+unsigned swd_set_clock(unsigned khz) {
+	unsigned div;
+	if (khz < 2000) khz = 2000;
+	if (khz > 48000) khz = 48000;
+	div = 192000 / khz;
+	sgpio_div = div - 1;
+	return 192000 / div;
+}
+
+void swd_hw_reset(int assert) {
+	if (assert) {
+		gpio_set(GPIO_RESET, 0);
+		gpio_set(GPIO_RESET_TXEN, 1);
+	} else {
+		gpio_set(GPIO_RESET, 1);
+		gpio_set(GPIO_RESET_TXEN, 0);
+	}
+}
+
diff --git a/src/bsp/lk/app/mdebug/swd.h b/src/bsp/lk/app/mdebug/swd.h
new file mode 100644
index 0000000..2822406
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/swd.h
@@ -0,0 +1,58 @@
+/* swd.h 
+ *
+ * Copyright 2011 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.
+ */
+
+#ifndef _SWDP_H_
+#define _SWDP_H_
+
+void swd_init(void);
+void swd_reset(void);
+int swd_write(unsigned reg, unsigned val);
+int swd_read(unsigned reg, unsigned *val);
+
+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
+
+#define RD_IDCODE	0b10100101
+#define RD_DPCTRL	0b10001101
+#define RD_RESEND	0b10010101
+#define RD_BUFFER	0b10111101
+
+#define WR_ABORT	0b10000001
+#define WR_DPCTRL	0b10101001
+#define WR_SELECT	0b10110001
+#define WR_BUFFER	0b10011001
+
+#define RD_AP0		0b10000111
+#define RD_AP1		0b10101111
+#define RD_AP2		0b10110111
+#define RD_AP3		0b10011111
+
+#define WR_AP0		0b10100011
+#define WR_AP1		0b10001011
+#define WR_AP2		0b10010011
+#define WR_AP3		0b10111011
+
+#endif
+
diff --git a/src/bsp/lk/app/mdebug/swo-uart1.c b/src/bsp/lk/app/mdebug/swo-uart1.c
new file mode 100644
index 0000000..7803a1d
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/swo-uart1.c
@@ -0,0 +1,151 @@
+/* swo-uart1.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 <string.h>
+#include <debug.h>
+#include <reg.h>
+#include <kernel/thread.h>
+
+#include <dev/udc.h>
+#include <arch/arm/cm.h>
+
+#include <platform/lpc43xx-uart.h>
+#include <platform/lpc43xx-gpdma.h>
+#include <platform/lpc43xx-clocks.h>
+
+#include "rswdp.h"
+
+#define UART_BASE	UART1_BASE
+#define BASE_UART_CLK	BASE_UART1_CLK
+
+extern uint8_t __lpc43xx_main_clock_sel;
+extern uint32_t __lpc43xx_main_clock_mhz;
+
+#define TXNSIZE		128
+#define TXNCOUNT	4
+
+typedef struct swo_txn {
+	unsigned buf[TXNSIZE/4 + 2];
+	struct swo_txn *next;
+	udc_request_t *req;
+	unsigned busy;
+	unsigned num;
+} txn_t;
+
+static txn_t TXN[TXNCOUNT];
+static txn_t *txwr = TXN;
+static udc_endpoint_t *txept;
+
+void swo_start_dma(void *ptr);
+
+static void tx_done(udc_request_t *req, unsigned actual, int status) {
+	txn_t *txn = req->context;
+	txn->busy = 0;
+	if (txwr == txn) {
+		// writer wants to write here, and is waiting...
+		swo_start_dma(txn->buf + 2);
+	}
+}
+
+void lpc43xx_DMA_IRQ(void) {
+	txn_t *txn = txwr;
+	arm_cm_irq_entry();
+	writel(0xFF, DMA_INTTCCLR);
+	writel(0xFF, DMA_INTERRCLR);
+	if (udc_request_queue(txept, txn->req)) {
+		// failed, usb probably offline, just re-use the buffer
+	} else {
+		txn->busy = 1;
+		txwr = txn = txn->next;
+	}
+	if (!txn->busy) {
+		// if busy, when the usb txn completes, it will start dma then
+		swo_start_dma(txn->buf + 2);
+	}
+	arm_cm_irq_exit(0);
+}
+
+void swo_init(udc_endpoint_t *_txept) {
+	int n;
+	txept = _txept;
+	for (n = 0; n < TXNCOUNT; n++) {
+		TXN[n].req = udc_request_alloc();
+		TXN[n].req->context = TXN + n;
+		TXN[n].req->buffer = TXN[n].buf;
+		TXN[n].req->length = TXNSIZE + 8;
+		TXN[n].req->complete = tx_done;
+		TXN[n].num = n;
+		TXN[n].busy = 0;
+		TXN[n].next = TXN + (n + 1);
+		TXN[n].buf[0] = RSWD_TXN_ASYNC;
+		TXN[n].buf[1] = RSWD_MSG(CMD_SWO_DATA, 0, TXNSIZE);
+	}
+	TXN[n-1].next = TXN;
+
+	// configure peripheral 4 as uart1_rx
+	writel((readl(DMAMUX_REG) & DMAMUX_M(4)) | DMAMUX_P(4, P4_UART1_RX), DMAMUX_REG);
+	writel(DMA_CONFIG_EN, DMA_CONFIG);
+	NVIC_EnableIRQ(DMA_IRQn);
+
+	// kick off the process with an initial DMA
+	swo_start_dma(txwr->buf + 2);
+}
+
+void swo_start_dma(void *ptr) {
+	writel(UART1_BASE + REG_RBR, DMA_SRC(0));
+	writel((u32) ptr, DMA_DST(0));
+	writel(0, DMA_LLI(0));
+	writel(DMA_XFER_SIZE(TXNSIZE) |
+		DMA_SRC_BURST(BURST_1) | DMA_DST_BURST(BURST_4) |
+		DMA_SRC_BYTE | DMA_DST_WORD | DMA_SRC_MASTER1 | DMA_DST_MASTER0 |
+		DMA_DST_INCR | DMA_PROT1 | DMA_TC_IE,
+		DMA_CTL(0));
+	writel(DMA_ENABLE | DMA_SRC_PERIPH(4) | DMA_FLOW_P2M_DMAc | DMA_TC_IRQ_EN,
+		DMA_CFG(0));
+}
+void swo_config(unsigned mhz) {
+	if (mhz > 0) {
+		uint32_t div = __lpc43xx_main_clock_mhz / 16 / mhz;
+		writel(BASE_CLK_SEL(__lpc43xx_main_clock_sel), BASE_UART_CLK);
+		writel(LCR_DLAB, UART_BASE + REG_LCR);
+		writel(div & 0xFF, UART_BASE + REG_DLL);
+		writel((div >> 8) & 0xFF, UART_BASE + REG_DLM);
+		writel(LCR_WLS_8 | LCR_SBS_1, UART_BASE + REG_LCR);
+		writel(FCR_FIFOEN | FCR_RX_TRIG_1 | FCR_DMAMODE, UART_BASE + REG_FCR);
+	}
+}
+
+unsigned swo_set_clock(unsigned khz) {
+	if (khz >= 12000) {
+		khz = 12000;
+	} else if (khz >= 8000) {
+		khz = 8000;
+	} else if (khz >= 6000) {
+		khz = 6000;
+	} else if (khz >= 4000) {
+		khz = 4000;
+	} else if (khz >= 3000) {
+		khz = 3000;
+	} else if (khz >= 2000) {
+		khz = 2000;
+	} else {
+		khz = 1000;
+	}
+	swo_config(khz * 1000);
+	return khz;
+}
+