diff --git a/ch32v003evt/LICENSE b/attic/ch32v003evt/LICENSE
similarity index 100%
rename from ch32v003evt/LICENSE
rename to attic/ch32v003evt/LICENSE
diff --git a/ch32v003evt/ch32v003.ld b/attic/ch32v003evt/ch32v003.ld
similarity index 100%
rename from ch32v003evt/ch32v003.ld
rename to attic/ch32v003evt/ch32v003.ld
diff --git a/ch32v003evt/ch32v00x.h b/attic/ch32v003evt/ch32v00x.h
similarity index 100%
rename from ch32v003evt/ch32v00x.h
rename to attic/ch32v003evt/ch32v00x.h
diff --git a/ch32v003evt/ch32v00x_conf.h b/attic/ch32v003evt/ch32v00x_conf.h
similarity index 100%
rename from ch32v003evt/ch32v00x_conf.h
rename to attic/ch32v003evt/ch32v00x_conf.h
diff --git a/ch32v003evt/core_riscv.h b/attic/ch32v003evt/core_riscv.h
similarity index 100%
rename from ch32v003evt/core_riscv.h
rename to attic/ch32v003evt/core_riscv.h
diff --git a/ch32v003evt/embedlibc.c b/attic/ch32v003evt/embedlibc.c
similarity index 100%
rename from ch32v003evt/embedlibc.c
rename to attic/ch32v003evt/embedlibc.c
diff --git a/ch32v003evt/startup_ch32v003.c b/attic/ch32v003evt/startup_ch32v003.c
similarity index 100%
rename from ch32v003evt/startup_ch32v003.c
rename to attic/ch32v003evt/startup_ch32v003.c
diff --git a/ch32v003evt/system_ch32v00x.h b/attic/ch32v003evt/system_ch32v00x.h
similarity index 100%
rename from ch32v003evt/system_ch32v00x.h
rename to attic/ch32v003evt/system_ch32v00x.h
diff --git a/attic/factory_bootloader.bin b/attic/factory_bootloader.bin
new file mode 100644
index 0000000000000000000000000000000000000000..8522877c7bdb9953d844c2fd48838ca7bc5af635
Binary files /dev/null and b/attic/factory_bootloader.bin differ
diff --git a/ch32v003fun/ch32v003fun.c b/ch32v003fun/ch32v003fun.c
index 18ce488f90546ec317a470f20032a03dd1b70ae2..137455e00b6ff83a5db7a2edc71de46b6ad379f1 100644
--- a/ch32v003fun/ch32v003fun.c
+++ b/ch32v003fun/ch32v003fun.c
@@ -664,7 +664,7 @@ mini_pprintf(int (*puts)(char*s, int len, void* buf), void* buf, const char *fmt
 int main() __attribute__((used));
 void SystemInit( void ) __attribute__((used));
 
-void InterruptVector()         __attribute__((naked)) __attribute((section(".init"))) __attribute__((used));
+void InterruptVector()         __attribute__((naked)) __attribute((section(".init"))) __attribute__((used)) __attribute((weak));
 void handle_reset()            __attribute__((naked)) __attribute((section(".text.handle_reset"))) __attribute__((used));
 void DefaultIRQHandler( void ) __attribute__((section(".text.vector_handler"))) __attribute__((naked)) __attribute__((used));
 
@@ -760,7 +760,6 @@ void InterruptVector()
 }
 
 
-
 void handle_reset()
 {
 	asm volatile( "\n\
@@ -770,49 +769,55 @@ void handle_reset()
 .option pop\n\
 	la sp, _eusrstack\n"
 	// Setup the interrupt vector, processor status and INTSYSCR.
-"	li t0, 0x80\n\
-	csrw mstatus, t0\n\
-	li t0, 0x3\n\
-	csrw 0x804, t0\n\
-	la t0, InterruptVector\n\
-	ori t0, t0, 3\n\
-	csrw mtvec, t0\n"
- );
+"	li a0, 0x80\n\
+	csrw mstatus, a0\n\
+	c.li a3, 0x3\n\
+	csrw 0x804, a3\n\
+	la a0, InterruptVector\n\
+	c.or a0, a3\n\
+	csrw mtvec, a0\n" );
 
 	// Careful: Use registers to prevent overwriting of self-data.
 	// This clears out BSS.
-	register uint32_t * tempout = _sbss;
-	register uint32_t * tempend = _ebss;
-	while( tempout < tempend )
-		*(tempout++) = 0;
-
-	// Once we get here, it should be safe to execute regular C code.
-
-	// Load data section from flash to RAM 
-	register uint32_t * tempin = _data_lma;
-	tempout = _data_vma;
-	tempend = _edata;
-	while( tempout != tempend )
-		*(tempout++) = *(tempin++); 
-
-	asm volatile("csrw mepc, %0" : : "r"(main));
+asm volatile(
+"	la a0, _sbss\n\
+	la a1, _ebss\n\
+	c.li a2, 0\n\
+	bge a0, a1, 2f\n\
+1:	c.sw a2, 0(a0)\n\
+	c.addi a0, 4\n\
+	blt a0, a1, 1b\n\
+2:"
+	// This loads DATA from FLASH to RAM.
+"	la a0, _data_lma\n\
+	la a1, _data_vma\n\
+	la a2, _edata\n\
+1:	beq a1, a2, 2f\n\
+	c.lw a3, 0(a0)\n\
+	c.sw a3, 0(a1)\n\
+	c.addi a0, 4\n\
+	c.addi a1, 4\n\
+	bne a1, a2, 1b\n\
+2:\n" );
 
 	// set mepc to be main as the root app.
-	asm volatile( "mret\n" );
+asm volatile(
+"	csrw mepc, %[main]\n"
+"	mret\n" : : [main]"r"(main) );
 }
 
 void SystemInit48HSI( void )
 {
 	// Values lifted from the EVT.  There is little to no documentation on what this does.
-	RCC->CTLR  = RCC_HSION | RCC_PLLON; 				// Use HSI, but enable PLL.
-	RCC->CFGR0 = RCC_HPRE_DIV1 | RCC_PLLSRC_HSI_Mul2;	// PLLCLK = HSI * 2 = 48 MHz; HCLK = SYSCLK = APB1
-	FLASH->ACTLR = FLASH_ACTLR_LATENCY_1;				// 1 Cycle Latency
-	RCC->INTR  = 0x009F0000;                            // Clear PLL, CSSC, HSE, HSI and LSI ready flags.
+	RCC->CTLR  = RCC_HSION | RCC_PLLON;               // Use HSI, but enable PLL.
+	RCC->CFGR0 = RCC_HPRE_DIV1 | RCC_PLLSRC_HSI_Mul2; // PLLCLK = HSI * 2 = 48 MHz; HCLK = SYSCLK = APB1
+	FLASH->ACTLR = FLASH_ACTLR_LATENCY_1;             // 1 Cycle Latency
+	RCC->INTR  = 0x009F0000;                          // Clear PLL, CSSC, HSE, HSI and LSI ready flags.
 
 	// From SetSysClockTo_48MHZ_HSI
-	while((RCC->CTLR & RCC_PLLRDY) == 0);														// Wait till PLL is ready
-	RCC->CFGR0 = ( RCC->CFGR0 & ((uint32_t)~(RCC_SW))) | (uint32_t)RCC_SW_PLL;					// Select PLL as system clock source
-	while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08);									// Wait till PLL is used as system clock source
+	while((RCC->CTLR & RCC_PLLRDY) == 0);                                      // Wait till PLL is ready
+	RCC->CFGR0 = ( RCC->CFGR0 & ((uint32_t)~(RCC_SW))) | (uint32_t)RCC_SW_PLL; // Select PLL as system clock source
+	while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08);                // Wait till PLL is used as system clock source
 }
 
 void SetupUART( int uartBRR )
@@ -833,7 +838,7 @@ void SetupUART( int uartBRR )
 	USART1->CTLR1 |= CTLR1_UE_Set;
 }
 
-
+#ifdef STDOUT_UART
 // For debug writing to the UART.
 int _write(int fd, const char *buf, int size)
 {
@@ -843,6 +848,43 @@ int _write(int fd, const char *buf, int size)
 	}
 	return size;
 }
+#else
+// For debug writing to the debug interface.
+int _write(int fd, const char *buf, int size)
+{
+	#define DMDATA0 ((volatile uint32_t*)0xe00000f4)
+	#define DMDATA1 ((volatile uint32_t*)0xe00000f8)
+
+	while( ((*DMDATA0) & 0x80) );
+
+	int remain = size;
+	while( remain > 0 )
+	{
+		int tosend = remain;
+		if( tosend > 7 ) tosend = 7;
+
+		uint32_t dmd1 = 0;
+		if( tosend > 3 ) memcpy( &dmd1, buf + 3, tosend - 3 );
+		uint32_t d1 = (tosend + 4) | 0x80;
+
+		int subsend = tosend;
+		if( subsend > 3 ) subsend = 3;
+		memcpy( ((uint8_t*)(&d1))+1, buf, subsend );
+		*DMDATA1 = dmd1;
+		*DMDATA0 = d1;
+		remain =- tosend;
+	}
+}
+
+
+void SetupDebugPrintf()
+{
+	// Clear out the sending flag.
+	*DMDATA1 = 0x0;
+}
+
+
+#endif
 
 void DelaySysTick( uint32_t n )
 {
@@ -853,3 +895,4 @@ void DelaySysTick( uint32_t n )
     while(!(SysTick->SR & (1 << 0)));
     SysTick->CTLR &= ~(1 << 0);
 }
+
diff --git a/ch32v003fun/ch32v003fun.h b/ch32v003fun/ch32v003fun.h
index 92ca4daeb4bb96b0b6dcffa2a28fdbf432b1b995..9f25a84910c83e962c15c1fb5b860606776e0e24 100644
--- a/ch32v003fun/ch32v003fun.h
+++ b/ch32v003fun/ch32v003fun.h
@@ -2473,7 +2473,7 @@ extern "C" {
 
 
 /* ch32v00x_adc.c ------------------------------------------------------------*/
-/*
+
 /* ADC DISCNUM mask */
 #define CTLR1_DISCNUM_Reset              ((uint32_t)0xFFFF1FFF)
 
@@ -4841,6 +4841,8 @@ void SystemInit48HSI( void );
 // Call with SetupUART( UART_BRR )
 void SetupUART( int uartBRR );
 
+void SetupDebugPrintf();
+
 #ifdef __cplusplus
 };
 #endif
diff --git a/ch32v003fun/ch32v003fun.ld b/ch32v003fun/ch32v003fun.ld
index d3d27288f1530e420dafd11ea17d1c93401890c9..b83406385be84e630a04d9c24b7dfcdcde3eef43 100644
--- a/ch32v003fun/ch32v003fun.ld
+++ b/ch32v003fun/ch32v003fun.ld
@@ -1,9 +1,5 @@
 ENTRY( InterruptVector )
 
-__stack_size = 256;
-
-PROVIDE( _stack_size = __stack_size );
-
 MEMORY
 {
 	FLASH (rx) : ORIGIN = 0x00000000, LENGTH = 16K
@@ -144,15 +140,7 @@ SECTIONS
     PROVIDE( _end = _ebss);
 	PROVIDE( end = . );
 
-	.stack ORIGIN(RAM) + LENGTH(RAM) - __stack_size :
-	{
-	    PROVIDE( _heap_end = . );
-	    . = ALIGN(4);
-	    PROVIDE(_susrstack = . );
-	    . = . + __stack_size;
-	    PROVIDE( _eusrstack = .);
-	} >RAM 
-	
+	PROVIDE( _eusrstack = ORIGIN(RAM) + LENGTH(RAM));	
 }
 
 
diff --git a/examples/blink/Makefile b/examples/blink/Makefile
index 0a4310763ff6d7bf7217bf4c8605f652274616fd..aaddf1dbac01fa6f4b4e66bac09eed1932ba8a7f 100644
--- a/examples/blink/Makefile
+++ b/examples/blink/Makefile
@@ -27,7 +27,7 @@ CFLAGS:= \
 	-I/usr/include/newlib \
 	-I$(CH32V003FUN) \
 	-nostdlib \
-	-I.
+	-I. -DTINYVECTOR
 
 LDFLAGS:=-T $(CH32V003FUN)/ch32v003fun.ld -Wl,--gc-sections
 
@@ -44,7 +44,7 @@ $(TARGET).bin : $(TARGET).elf
 	$(PREFIX)-objcopy -O ihex $< $(TARGET).hex
 
 flash : $(TARGET).bin
-	$(MINICHLINK)/minichlink -w $< -r
+	$(MINICHLINK)/minichlink -w $< flash -b
 
 clean :
 	rm -rf $(TARGET).elf $(TARGET).bin $(TARGET).hex $(TARGET).lst $(TARGET).map $(TARGET).hex
diff --git a/examples/blink/blink.bin b/examples/blink/blink.bin
old mode 100644
new mode 100755
index e22d2b54b66c603299ca02592e7849815ac9ac9e..94a16b43785731553baa6a65a8ab89e0914b9772
Binary files a/examples/blink/blink.bin and b/examples/blink/blink.bin differ
diff --git a/examples/blink/blink.c b/examples/blink/blink.c
index 9b99747755972c996826b2746e9da96746a4f782..ea936900bafe0447bad995a5d187431f8ddea486 100644
--- a/examples/blink/blink.c
+++ b/examples/blink/blink.c
@@ -15,19 +15,16 @@ int main()
 	// Enable GPIOD.
 	RCC->APB2PCENR |= RCC_APB2Periph_GPIOD;
 
-	// GPIO D0 Push-Pull, 10MHz Slew Rate Setting
+	// GPIO D0 Push-Pull, 10MHz Output
 	GPIOD->CFGLR &= ~(0xf<<(4*0));
 	GPIOD->CFGLR |= (GPIO_Speed_10MHz | GPIO_CNF_OUT_PP)<<(4*0);
 
-	GPIOD->CFGLR &= ~(0xf<<(4*4));
-	GPIOD->CFGLR |= (GPIO_Speed_10MHz | GPIO_CNF_OUT_PP)<<(4*4);
-
 	while(1)
 	{
-		GPIOD->BSHR = (1<<0) | (1<<4);	 // Turn on GPIOD0
-		Delay_Ms( 100 );
-		GPIOD->BSHR = (1<<(16+0)) | (1<<(16+4)); // Turn off GPIOD0
-		Delay_Ms( 100 );
+		GPIOD->BSHR = 1;	 // Turn on GPIOD0
+		Delay_Ms( 200 );
+		GPIOD->BSHR = 1<<16; // Turn off GPIOD0
+		Delay_Ms( 200 );
 		count++;
 	}
 }
diff --git a/examples/sandbox/Makefile b/examples/sandbox/Makefile
index b584431f9e85926222f105b606a5d9e44e3e1f4c..d626a3809d6818e66b6801b24f555d89a1245ea6 100644
--- a/examples/sandbox/Makefile
+++ b/examples/sandbox/Makefile
@@ -6,7 +6,7 @@ PREFIX:=riscv64-unknown-elf
 
 GPIO_Toggle:=EXAM/GPIO/GPIO_Toggle/User
 
-EVT:=../../ch32v003evt
+CH32V003FUN:=../../ch32v003CH32V003FUN
 MINICHLINK:=../../minichlink
 
 CFLAGS:= \
@@ -15,13 +15,13 @@ CFLAGS:= \
 	-march=rv32ec \
 	-mabi=ilp32e \
 	-I/usr/include/newlib \
-	-I$(EVT) \
+	-I$(CH32V003FUN) \
 	-nostdlib \
 	-I.
 
-LDFLAGS:=-T $(EVT)/ch32v003.ld -Wl,--gc-sections
+LDFLAGS:=-T $(CH32V003FUN)/ch32v003fun.ld -Wl,--gc-sections
 
-SYSTEM_C:=$(EVT)/startup_ch32v003.c $(EVT)/embedlibc.c
+SYSTEM_C:=$(CH32V003FUN)/ch32v003fun.c
 
 $(TARGET).elf : $(SYSTEM_C) $(TARGET).c
 	$(PREFIX)-gcc -o $@ $^ $(CFLAGS) $(LDFLAGS)
diff --git a/examples/fulldemo/Makefile b/examples/uartdemo/Makefile
similarity index 75%
rename from examples/fulldemo/Makefile
rename to examples/uartdemo/Makefile
index 04a369bce39513c63c4f1ad05e6601860ca02f70..9e1925e9012e971f48a1efb4053a822441823952 100644
--- a/examples/fulldemo/Makefile
+++ b/examples/uartdemo/Makefile
@@ -1,4 +1,4 @@
-TARGET:=fulldemo
+TARGET:=uartdemo
 
 all : flash
 
@@ -6,7 +6,7 @@ PREFIX:=riscv64-unknown-elf
 
 GPIO_Toggle:=EXAM/GPIO/GPIO_Toggle/User
 
-EVT:=../../ch32v003evt
+CH32V003FUN:=../../ch32v003fun
 MINICHLINK:=../../minichlink
 
 CFLAGS:= \
@@ -15,13 +15,13 @@ CFLAGS:= \
 	-march=rv32ec \
 	-mabi=ilp32e \
 	-I/usr/include/newlib \
-	-I$(EVT) \
+	-I$(CH32V003FUN) \
 	-nostdlib \
-	-I.
+	-I. -DSTDOUT_UART
 
-LDFLAGS:=-T $(EVT)/ch32v003.ld -Wl,--gc-sections
+LDFLAGS:=-T $(CH32V003FUN)/ch32v003fun.ld -Wl,--gc-sections
 
-SYSTEM_C:=$(EVT)/startup_ch32v003.c $(EVT)/embedlibc.c
+SYSTEM_C:=$(CH32V003FUN)/ch32v003fun.c
 
 $(TARGET).elf : $(SYSTEM_C) $(TARGET).c
 	$(PREFIX)-gcc -o $@ $^ $(CFLAGS) $(LDFLAGS)
@@ -35,7 +35,7 @@ $(TARGET).bin : $(TARGET).elf
 
 flash : $(TARGET).bin
 	make -C $(MINICHLINK) all
-	$(MINICHLINK)/minichlink -w $< -r
+	$(MINICHLINK)/minichlink -w $< flash -b
 
 clean :
 	rm -rf $(TARGET).elf $(TARGET).bin $(TARGET).hex $(TARGET).lst $(TARGET).map $(TARGET).hex
diff --git a/examples/fulldemo/fulldemo.c b/examples/uartdemo/uartdemo.c
similarity index 89%
rename from examples/fulldemo/fulldemo.c
rename to examples/uartdemo/uartdemo.c
index 340da898fbe9fae936d9966bc72d90ca98a5dc51..b7e562c9c81e7666653f36a14a86f8963c7c8246 100644
--- a/examples/fulldemo/fulldemo.c
+++ b/examples/uartdemo/uartdemo.c
@@ -6,9 +6,11 @@
 #define SYSTEM_CORE_CLOCK 48000000
 #define APB_CLOCK SYSTEM_CORE_CLOCK
 
-#include "ch32v00x.h"
+#include "ch32v003fun.h"
 #include <stdio.h>
 
+uint32_t count;
+
 int main()
 {
 	SystemInit48HSI();
@@ -24,9 +26,9 @@ int main()
 	while(1)
 	{
 		GPIOD->BSHR = 1;	 // Turn on GPIOD0
-		puts( "Hello" );
 		Delay_Ms( 50 );
 		GPIOD->BSHR = 1<<16; // Turn off GPIOD0
 		Delay_Ms( 50 );
+		printf( "Count: %d\n", count++ );
 	}
 }
diff --git a/examples/ws2812demo/Makefile b/examples/ws2812demo/Makefile
index 816641bef4916ed5ac99f0b17a7a0f60e57bc614..d387c58f1a82b858685ebcb51b6f2da3ad415f23 100644
--- a/examples/ws2812demo/Makefile
+++ b/examples/ws2812demo/Makefile
@@ -4,9 +4,7 @@ all : flash
 
 PREFIX:=riscv64-unknown-elf
 
-GPIO_Toggle:=EXAM/GPIO/GPIO_Toggle/User
-
-EVT:=../../ch32v003evt
+CH32V003FUN:=../../ch32v003fun
 MINICHLINK:=../../minichlink
 
 CFLAGS:= \
@@ -15,13 +13,13 @@ CFLAGS:= \
 	-march=rv32ec \
 	-mabi=ilp32e \
 	-I/usr/include/newlib \
-	-I$(EVT) \
+	-I$(CH32V003FUN) \
 	-nostdlib \
 	-I.
 
-LDFLAGS:=-T $(EVT)/ch32v003.ld -Wl,--gc-sections
+LDFLAGS:=-T $(CH32V003FUN)/ch32v003fun.ld -Wl,--gc-sections
 
-SYSTEM_C:=$(EVT)/startup_ch32v003.c $(EVT)/embedlibc.c
+SYSTEM_C:=$(CH32V003FUN)/ch32v003fun.c
 
 $(TARGET).elf : $(SYSTEM_C) $(TARGET).c
 	$(PREFIX)-gcc -o $@ $^ $(CFLAGS) $(LDFLAGS)
@@ -35,7 +33,7 @@ $(TARGET).bin : $(TARGET).elf
 
 flash : $(TARGET).bin
 	make -C $(MINICHLINK) all
-	$(MINICHLINK)/minichlink -w $< -r
+	$(MINICHLINK)/minichlink -w $< flash -b
 
 clean :
 	rm -rf $(TARGET).elf $(TARGET).bin $(TARGET).hex $(TARGET).lst $(TARGET).map $(TARGET).hex
diff --git a/examples/ws2812demo/ws2812bdemo.c b/examples/ws2812demo/ws2812bdemo.c
index 9f2d407e895ec50f13beafbd7a0d588269c8bb6b..c2661ac5bb3f5d627af9311c51edbad9afb37276 100644
--- a/examples/ws2812demo/ws2812bdemo.c
+++ b/examples/ws2812demo/ws2812bdemo.c
@@ -2,7 +2,7 @@
 #define SYSTEM_CORE_CLOCK 48000000
 #define APB_CLOCK SYSTEM_CORE_CLOCK
 
-#include "ch32v00x.h"
+#include "ch32v003fun.h"
 #include <stdio.h>
 #include <string.h>
 
diff --git a/minichlink/Makefile b/minichlink/Makefile
index d2fda917a3368e8f495886feb4e4ba81d53d5bdf..26a4024a445817fa1aa4a8b0dafa976ca3eec374 100644
--- a/minichlink/Makefile
+++ b/minichlink/Makefile
@@ -2,7 +2,7 @@ TOOLS:=minichlink
 
 all : $(TOOLS)
 
-CFLAGS:=-O0 -g3
+CFLAGS:=-O0 -g3 -Wall
 LDFLAGS:=-lpthread -lusb-1.0 -ludev
 
 minichlink : minichlink.c pgm-wch-linke.c pgm-esp32s2-ch32xx.c
@@ -12,6 +12,10 @@ install_udev_rules :
 	cp 99-WCH-LinkE.rules /etc/udev/rules.d/
 	service udev restart
 
+inspect_bootloader : minichlink
+	./minichlink -r test.bin launcher 0x780
+	riscv64-unknown-elf-objdump -S -D test.bin -b binary -m riscv:rv32 | less
+
 clean :
 	rm -rf $(TOOLS)
 	
diff --git a/minichlink/minichlink.c b/minichlink/minichlink.c
index f1aee9c8f51f2a84b9cc2ff997dfbb692c07d834..57dfe80228358f27d21888f15db1f3b7790dc51d 100644
--- a/minichlink/minichlink.c
+++ b/minichlink/minichlink.c
@@ -3,12 +3,16 @@
 // Freely licensable under the MIT/x11, NewBSD Licenses, or
 // public domain where applicable. 
 
+// TODO: Can we make a unified DMPROG for reading + writing?
+
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
 #include "minichlink.h"
+#include "../ch32v003fun/ch32v003fun.h"
 
 static int64_t SimpleReadNumberInt( const char * number, int64_t defaultNumber );
+static int64_t StringToMemoryAddress( const char * number );
 void TestFunction(void * v );
 struct MiniChlinkFunctions MCF;
 
@@ -29,11 +33,10 @@ int main( int argc, char ** argv )
 		return -32;
 	}
 	
-	SetupAutomaticHighLevelFunctions();
+	SetupAutomaticHighLevelFunctions( dev );
 
 	int status;
 	int must_be_end = 0;
-	uint8_t rbuff[1024];
 
 	if( MCF.SetupInterface )
 	{
@@ -42,9 +45,10 @@ int main( int argc, char ** argv )
 			fprintf( stderr, "Could not setup interface.\n" );
 			return -33;
 		}
+		printf( "Interface Setup\n" );
 	}
 
-	//TestFunction( dev );
+//	TestFunction( dev );
 
 	int iarg = 1;
 	const char * lastcommand = 0;
@@ -100,62 +104,70 @@ keep_going:
 				else
 					goto unimplemented;
 				break;
-			case 'r': 
-				if( MCF.HaltMode )
-					MCF.HaltMode( dev, 0 );
-				else
+			case 'b':  //reBoot
+				if( !MCF.HaltMode || MCF.HaltMode( dev, 1 ) )
 					goto unimplemented;
-				must_be_end = 'r';
 				break;
-			case 'R':
-				if( MCF.HaltMode )
-					MCF.HaltMode( dev, 1 );
-				else
+			case 'e':  //rEsume
+				if( !MCF.HaltMode || MCF.HaltMode( dev, 2 ) )
+					goto unimplemented;
+				break;
+			case 'E':  //Erase whole chip.
+				if( MCF.HaltMode ) MCF.HaltMode( dev, 0 );
+				if( !MCF.Erase || MCF.Erase( dev, 0, 0, 1 ) )
+					goto unimplemented;
+				break;
+			case 'h':
+				if( !MCF.HaltMode || MCF.HaltMode( dev, 0 ) )
 					goto unimplemented;
-				must_be_end = 'R';
 				break;
 
 			// disable NRST pin (turn it into a GPIO)
 			case 'd':  // see "RSTMODE" in datasheet
+				if( MCF.HaltMode ) MCF.HaltMode( dev, 0 );
 				if( MCF.ConfigureNRSTAsGPIO )
 					MCF.ConfigureNRSTAsGPIO( dev, 0 );
 				else
 					goto unimplemented;
 				break;
 			case 'D': // see "RSTMODE" in datasheet
+				if( MCF.HaltMode ) MCF.HaltMode( dev, 0 );
 				if( MCF.ConfigureNRSTAsGPIO )
 					MCF.ConfigureNRSTAsGPIO( dev, 1 );
 				else
 					goto unimplemented;
 				break;
-			// PROTECTION UNTESTED!
-			/*
+			case 'T':
+			{
+				if( !MCF.PollTerminal )
+					goto unimplemented;
+				do
+				{
+					uint8_t buffer[128];
+					int r = MCF.PollTerminal( dev, buffer, sizeof( buffer ) );
+					if( r < 0 )
+					{
+						fprintf( stderr, "Terminal dead.  code %d\n", r );
+						return -32;
+					}
+					if( r > 0 )
+					{
+						fwrite( buffer, r, 1, stdout ); 
+					}
+				} while( 1 );
+			}
 			case 'p':
-				wch_link_multicommands( devh, 8,
-					11, "\x81\x06\x08\x02\xf7\xff\xff\xff\xff\xff\xff",
-					4, "\x81\x0b\x01\x01",
-					4, "\x81\x0d\x01\xff",
-					4, "\x81\x0d\x01\x01",
-					5, "\x81\x0c\x02\x09\x01",
-					4, "\x81\x0d\x01\x02",
-					4, "\x81\x06\x01\x01",
-					4, "\x81\x0d\x01\xff" );
-				break;
-			case 'P':
-				wch_link_multicommands( devh, 7,
-					11, "\x81\x06\x08\x03\xf7\xff\xff\xff\xff\xff\xff",
-					4, "\x81\x0b\x01\x01",
-					4, "\x81\x0d\x01\xff",
-					4, "\x81\x0d\x01\x01",
-					5, "\x81\x0c\x02\x09\x01",
-					4, "\x81\x0d\x01\x02",
-					4, "\x81\x06\x01\x01" );
+			{
+				if( MCF.PrintChipInfo )
+					MCF.PrintChipInfo( dev ); 
+				else
+					goto unimplemented;
 				break;
-			*/
-			case 'o':
+			}
+			case 'r':
 			{
-				int i;
-				int transferred;
+				if( MCF.HaltMode ) MCF.HaltMode( dev, 0 );
+
 				if( argchar[2] != 0 )
 				{
 					fprintf( stderr, "Error: can't have char after paramter field\n" ); 
@@ -168,24 +180,32 @@ keep_going:
 					fprintf( stderr, "Error: missing file for -o.\n" ); 
 					goto help;
 				}
-				uint64_t offset = SimpleReadNumberInt( argv[iarg++], -1 );
-				uint64_t amount = SimpleReadNumberInt( argv[iarg++], -1 );
+				const char * fname = argv[iarg++];
+				uint64_t offset = StringToMemoryAddress( argv[iarg++] );
+
+				uint64_t amount = SimpleReadNumberInt( argv[iarg], -1 );
 				if( offset > 0xffffffff || amount > 0xffffffff )
 				{
-					fprintf( stderr, "Error: memory value request out of range.\n" );
+					fprintf( stderr, "Error: memory value request out of range\n" );
 					return -9;
 				}
 
 				// Round up amount.
 				amount = ( amount + 3 ) & 0xfffffffc;
-				FILE * f = fopen( argv[iarg], "wb" );
+				FILE * f = 0;
+				int hex = 0;
+				if( strcmp( fname, "-" ) == 0 )
+					f = stdout;
+				else if( strcmp( fname, "+" ) == 0 )
+					f = stdout, hex = 1;
+				else
+					f = fopen( fname, "wb" );
 				if( !f )
 				{
-					fprintf( stderr, "Error: can't open write file \"%s\"\n", argv[iarg] );
+					fprintf( stderr, "Error: can't open write file \"%s\"\n", fname );
 					return -9;
 				}
 				uint8_t * readbuff = malloc( amount );
-				int readbuffplace = 0;
 
 				if( MCF.ReadBinaryBlob )
 				{
@@ -200,29 +220,102 @@ keep_going:
 					goto unimplemented;
 				}
 
-				fwrite( readbuff, amount, 1, f );
+				if( hex )
+				{
+					int i;
+					for( i = 0; i < amount; i++ )
+					{
+						if( ( i & 0xf ) == 0 )
+						{
+							if( i != 0 ) printf( "\n" );
+							printf( "%08x: ", (uint32_t)(offset + i) );
+						}
+						printf( "%02x ", readbuff[i] );
+					}
+					printf( "\n" );
+				}
+				else
+					fwrite( readbuff, amount, 1, f );
 
 				free( readbuff );
 
-				fclose( f );
+				if( f != stdout ) fclose( f );
 				break;
 			}
 			case 'w':
 			{
+				if( MCF.HaltMode ) MCF.HaltMode( dev, 0 );
+
 				if( argchar[2] != 0 ) goto help;
 				iarg++;
 				argchar = 0; // Stop advancing
-				if( iarg >= argc ) goto help;
+				if( iarg + 1 >= argc ) goto help;
+
 				// Write binary.
-				int i;
-				FILE * f = fopen( argv[iarg], "rb" );
-				fseek( f, 0, SEEK_END );
-				int len = ftell( f );
-				fseek( f, 0, SEEK_SET );
-				char * image = malloc( len );
-				status = fread( image, len, 1, f );
-				fclose( f );
-	
+				int len = 0;
+				uint8_t * image = 0;
+				const char * fname = argv[iarg++];
+
+				if( fname[0] == '-' )
+				{
+					len = strlen( fname + 1 );
+					image = (uint8_t*)strdup( fname + 1 );
+					status = 1;
+				}
+				else if( fname[0] == '+' )
+				{
+					int hl = strlen( fname+1 );
+					if( hl & 1 )
+					{
+						fprintf( stderr, "Error: hex input doesn't align to chars correctly.\n" );
+						return -32;
+					}
+					len = hl/2;
+					image = malloc( len );
+					int i;
+					for( i = 0; i < len; i ++ )
+					{
+						char c1 = fname[i*2+1];
+						char c2 = fname[i*2+2];
+						int v1, v2;
+						if( c1 >= '0' && c1 <= '9' ) v1 = c1 - '0';
+						else if( c1 >= 'a' && c1 <= 'f' ) v1 = c1 - 'a' + 10;
+						else if( c1 >= 'A' && c1 <= 'F' ) v1 = c1 - 'A' + 10;
+						else
+						{
+							fprintf( stderr, "Error: Bad hex\n" );
+							return -32;
+						}
+
+						if( c2 >= '0' && c2 <= '9' ) v2 = c2 - '0';
+						else if( c2 >= 'a' && c2 <= 'f' ) v2 = c2 - 'a' + 10;
+						else if( c2 >= 'A' && c2 <= 'F' ) v2 = c2 - 'A' + 10;
+						else
+						{
+							fprintf( stderr, "Error: Bad hex\n" );
+							return -32;
+						}
+						image[i] = (v1<<4) | v2;
+					}
+					status = 1;
+				}
+				else
+				{
+					FILE * f = fopen( fname, "rb" );
+					fseek( f, 0, SEEK_END );
+					len = ftell( f );
+					fseek( f, 0, SEEK_SET );
+					image = malloc( len );
+					status = fread( image, len, 1, f );
+					fclose( f );
+				}
+
+				uint64_t offset = StringToMemoryAddress( argv[iarg] );
+				if( offset > 0xffffffff )
+				{
+					fprintf( stderr, "Error: Invalid offset (%s)\n", argv[iarg] );
+					exit( -44 );
+				}
 				if( status != 1 )
 				{
 					fprintf( stderr, "Error: File I/O Fault.\n" );
@@ -234,9 +327,10 @@ keep_going:
 					exit( -9 );
 				}
 
+
 				if( MCF.WriteBinaryBlob )
 				{
-					if( MCF.WriteBinaryBlob( dev, 0x08000000, len, image ) )
+					if( MCF.WriteBinaryBlob( dev, offset, len, image ) )
 					{
 						fprintf( stderr, "Error: Fault writing image.\n" );
 						return -13;
@@ -247,8 +341,6 @@ keep_going:
 					goto unimplemented;
 				}
 
-				// Waiting or something on 2.46.2???????  I don't know why the main system does this.
-				//	WCHCHECK( libusb_bulk_transfer( devh, 0x82, rbuff, 1024, &transferred, 2000 ) ); // Ignore respone.
 				free( image );
 				break;
 			}
@@ -257,6 +349,9 @@ keep_going:
 		if( argchar && argchar[2] != 0 ) { argchar++; goto keep_going; }
 	}
 
+	if( MCF.FlushLLCommands )
+		MCF.FlushLLCommands( dev );
+
 	if( MCF.Exit )
 		MCF.Exit( dev );
 
@@ -271,15 +366,20 @@ help:
 	fprintf( stderr, " -t Disable 3.3V\n" );
 	fprintf( stderr, " -f Disable 5V\n" );
 	fprintf( stderr, " -u Clear all code flash - by power off (also can unbrick)\n" );
-	fprintf( stderr, " -r Release from Reset\n" );
-	fprintf( stderr, " -R Place into Reset\n" );
+	fprintf( stderr, " -b Reboot out of Halt\n" );
+	fprintf( stderr, " -e Resume from halt\n" );
+	fprintf( stderr, " -h Place into Halt\n" );
 	fprintf( stderr, " -D Configure NRST as GPIO **WARNING** If you do this and you reconfig\n" );
 	fprintf( stderr, "      the SWIO pin (PD1) on boot, your part can never again be programmed!\n" );
 	fprintf( stderr, " -d Configure NRST as NRST\n" );
 //	fprintf( stderr, " -P Enable Read Protection (UNTESTED)\n" );
 //	fprintf( stderr, " -p Disable Read Protection (UNTESTED)\n" );
-	fprintf( stderr, " -w [binary image to write]\n" );
-	fprintf( stderr, " -o [memory address, decimal or 0x, try 0x08000000] [size, decimal or 0x, try 16384] [output binary image]\n" );
+	fprintf( stderr, " -w [binary image to write] [address, decimal or 0x, try0x08000000]\n" );
+	fprintf( stderr, " -r [memory address, decimal or 0x, try 0x08000000] [size, decimal or 0x, try 16384] [output binary image]\n" );
+	fprintf( stderr, "   Note: for memory addresses, you can use 'flash' 'launcher' 'bootloader' 'option' 'ram' and say \"ram+0x10\" for instance\n" );
+	fprintf( stderr, "   For filename, you can use - for raw or + for hex.\n" );
+	fprintf( stderr, " -T is a terminal. This MUST be the last argument.  You MUST have resumed or \n" );
+
 	return -1;	
 
 unimplemented:
@@ -292,6 +392,8 @@ unimplemented:
 #define strtoll _strtoi64
 #endif
 
+static int StaticUnlockFlash( void * dev, struct InternalState * iss );
+
 static int64_t SimpleReadNumberInt( const char * number, int64_t defaultNumber )
 {
 	if( !number || !number[0] ) return defaultNumber;
@@ -317,21 +419,77 @@ static int64_t SimpleReadNumberInt( const char * number, int64_t defaultNumber )
 	}
 }
 
+static int64_t StringToMemoryAddress( const char * number )
+{
+	uint32_t base = 0;
+
+	if( strncmp( number, "flash", 5 ) == 0 )       base = 0x08000000, number += 5;
+	if( strncmp( number, "launcher", 8 ) == 0 )    base = 0x1FFFF000, number += 8;
+	if( strncmp( number, "bootloader", 10 ) == 0 ) base = 0x1FFFF000, number += 10;
+	if( strncmp( number, "option", 6 ) == 0 )      base = 0x1FFFF800, number += 6;
+	if( strncmp( number, "user", 4 ) == 0 )        base = 0x1FFFF800, number += 4;
+	if( strncmp( number, "ram", 3 ) == 0 )         base = 0x20000000, number += 3;
+
+	if( base )
+	{
+		if( *number != '+' )
+			return base;
+		number++;
+		return base + SimpleReadNumberInt( number, 0 );
+	}
+	return SimpleReadNumberInt( number, -1 );
+}
+
+static int DefaultWaitForFlash( void * dev )
+{
+	uint32_t rw, timeout = 0;
+	do
+	{
+		rw = 0;
+		MCF.ReadWord( dev, (intptr_t)&FLASH->STATR, &rw ); // FLASH_STATR => 0x4002200C
+		if( timeout++ > 100 ) return -1;
+	} while(rw & 1);  // BSY flag.
+
+	if( rw & FLASH_STATR_WRPRTERR )
+	{
+		fprintf( stderr, "Memory Protection Error\n" );
+		return -44;
+	}
+
+	return 0;
+}
+
+static int DefaultWaitForDoneOp( void * dev )
+{
+	int r;
+	uint32_t rrv;
+	do
+	{
+		r = MCF.ReadReg32( dev, DMABSTRACTCS, &rrv );
+		if( r ) return r;
+	}
+	while( rrv & (1<<12) );
+	if( (rrv >> 8 ) & 7 )
+	{
+		fprintf( stderr, "Fault writing memory (DMABSTRACTS = %08x)\n", rrv );
+		MCF.WriteReg32( dev, DMABSTRACTCS, 0x00000700 );
+		return -9;
+	}
+	return 0;
+}
 
 int DefaultSetupInterface( void * dev )
 {
-	struct ESP32ProgrammerStruct * eps = (struct ESP32ProgrammerStruct *)dev;
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
 
 	if( MCF.Control3v3 ) MCF.Control3v3( dev, 1 );
 	if( MCF.DelayUS ) MCF.DelayUS( dev, 16000 );
-	MCF.WriteReg32( dev, SHDWCFGR, 0x5aa50000 | (1<<10) ); // Shadow Config Reg
-	MCF.WriteReg32( dev, CFGR, 0x5aa50000 | (1<<10) ); // CFGR (1<<10 == Allow output from slave)
-	MCF.WriteReg32( dev, CFGR, 0x5aa50000 | (1<<10) ); // Bug in silicon?  If coming out of cold boot, and we don't do our little "song and dance" this has to be called.
-	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Make the debug module work properly.
-	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Initiate a halt request.
+	MCF.WriteReg32( dev, DMSHDWCFGR, 0x5aa50000 | (1<<10) ); // Shadow Config Reg
+	MCF.WriteReg32( dev, DMCFGR, 0x5aa50000 | (1<<10) ); // CFGR (1<<10 == Allow output from slave)
+	MCF.WriteReg32( dev, DMCFGR, 0x5aa50000 | (1<<10) ); // Bug in silicon?  If coming out of cold boot, and we don't do our little "song and dance" this has to be called.
 
-	// Read back chip ID.
-	uint32_t reg;
+	// Read back chip status.  This is really baskc.
+	uint32_t reg = 0;
 	int r = MCF.ReadReg32( dev, DMSTATUS, &reg );
 	if( r >= 0 )
 	{
@@ -341,108 +499,442 @@ int DefaultSetupInterface( void * dev )
 			fprintf( stderr, "Error: Setup chip failed. Got code %08x\n", reg );
 			return -9;
 		}
-		return 0;
 	}
 	else
 	{
 		fprintf( stderr, "Error: Could not read chip code.\n" );
 		return r;
 	}
+
+	iss->statetag = STTAG( "STRT" );
+	return 0;
 }
 
-static int WriteWord( void * dev, uint32_t address_to_write, uint32_t data )
+static int DefaultWriteWord( void * dev, uint32_t address_to_write, uint32_t data )
 {
-	int r;
-	MCF.WriteReg32( dev, DMPROGBUF0, 0x0072a023 ); // sw x7,0(x5)
-	MCF.WriteReg32( dev, DMPROGBUF0, 0x00100073 ); // ebreak
-	MCF.WriteReg32( dev, DMDATA0, address_to_write );
-	MCF.WriteReg32( dev, DMCOMMAND, 0x00231005 ); // Copy data to x5
-	uint32_t rrv;
-	do
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
+	int ret = 0;
+
+	int is_flash = 0;
+	if( ( address_to_write & 0xff000000 ) == 0x08000000 || ( address_to_write & 0x1FFFF800 ) == 0x1FFFF000 )
 	{
-		r = MCF.ReadReg32( dev, DMABSTRACTCS, &rrv );
-		if( r ) return r;
+		// Is flash.
+		is_flash = 1;
 	}
-	while( rrv & (1<<12) );
-	
-	MCF.WriteReg32( dev, DMDATA0, data );
-	MCF.WriteReg32( dev, DMCOMMAND, 0x00271007 ); // Copy data to x7, and execute program.
-	do
+
+	if( iss->statetag != STTAG( "WRSQ" ) || is_flash != iss->lastwriteflags )
 	{
-		r = MCF.ReadReg32( dev, DMABSTRACTCS, &rrv );
-		if( r ) return r;
+		int did_disable_req = 0;
+		if( iss->statetag != STTAG( "WRSQ" ) )
+		{
+			MCF.WriteReg32( dev, DMABSTRACTAUTO, 0x00000000 ); // Disable Autoexec.
+			did_disable_req = 1;
+			// Different address, so we don't need to re-write all the program regs.
+			// c.lw x9,0(x10) // Get the address to write to. 
+			// c.sw x8,0(x9)  // Write to the address.
+			MCF.WriteReg32( dev, DMPROGBUF0, 0xc0804104 );
+			// c.addi x9, 4
+			// c.sw x9,0(x10)
+			MCF.WriteReg32( dev, DMPROGBUF1, 0xc1040491 );
+
+			if( iss->statetag != STTAG( "RDSQ" ) )
+			{
+				MCF.WriteReg32( dev, DMDATA0, 0xe00000f4 );   // DATA0's location in memory.
+				MCF.WriteReg32( dev, DMCOMMAND, 0x0023100b ); // Copy data to x11
+				MCF.WriteReg32( dev, DMDATA0, 0xe00000f8 );   // DATA1's location in memory.
+				MCF.WriteReg32( dev, DMCOMMAND, 0x0023100a ); // Copy data to x10
+				MCF.WriteReg32( dev, DMDATA0, 0x40022010 ); //FLASH->CTLR
+				MCF.WriteReg32( dev, DMCOMMAND, 0x0023100c ); // Copy data to x12
+				MCF.WriteReg32( dev, DMDATA0, CR_PAGE_PG|CR_BUF_LOAD);
+				MCF.WriteReg32( dev, DMCOMMAND, 0x0023100d ); // Copy data to x13
+			}
+		}
+
+		if( iss->lastwriteflags != is_flash || iss->statetag != STTAG( "WRSQ" ) )
+		{
+			// If we are doing flash, we have to ack, otherwise we don't want to ack.
+			if( is_flash )
+			{
+				// After writing to memory, also hit up page load flag.
+				// c.sw x13,0(x12) // Acknowledge the page write.
+				// c.ebreak
+				MCF.WriteReg32( dev, DMPROGBUF2, 0x9002c214 );
+			}
+			else
+			{
+				MCF.WriteReg32( dev, DMPROGBUF2, 0x00019002 ); // c.ebreak
+			}
+		}
+
+		MCF.WriteReg32( dev, DMDATA1, address_to_write );
+		MCF.WriteReg32( dev, DMDATA0, data );
+
+		if( did_disable_req )
+		{
+			MCF.WriteReg32( dev, DMCOMMAND, 0x00271008 ); // Copy data to x8, and execute program.
+			MCF.WriteReg32( dev, DMABSTRACTAUTO, 1 ); // Enable Autoexec.
+		}
+		iss->lastwriteflags = is_flash;
+
+
+		iss->statetag = STTAG( "WRSQ" );
+		iss->currentstateval = address_to_write;
+
+		if( is_flash )
+			ret |= MCF.WaitForDoneOp( dev );
 	}
-	while( rrv & (1<<12) );
-	if( (rrv >> 8 ) & 7 )
+	else
 	{
-		fprintf( stderr, "Fault writing memory (DMABSTRACTS = %08x)\n", rrv );
+		if( address_to_write != iss->currentstateval )
+		{
+			MCF.WriteReg32( dev, DMABSTRACTAUTO, 0 ); // Disable Autoexec.
+			MCF.WriteReg32( dev, DMDATA1, address_to_write );
+			MCF.WriteReg32( dev, DMABSTRACTAUTO, 1 ); // Enable Autoexec.
+		}
+		MCF.WriteReg32( dev, DMDATA0, data );
+		if( is_flash )
+		{
+			MCF.DelayUS( dev, 100 );
+		}
+		else
+		{
+			ret |= MCF.WaitForDoneOp( dev );
+		}
 	}
+
+
+	iss->currentstateval += 4;
+
 	return 0;
 }
 
+
 int DefaultWriteBinaryBlob( void * dev, uint32_t address_to_write, uint32_t blob_size, uint8_t * blob )
 {
-	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Make the debug module work properly.
-	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Initiate a halt request.
-	MCF.WriteReg32( dev, DMCONTROL, 0x00000001 ); // Clear Halt Request.
+	// NOTE IF YOU FIX SOMETHING IN THIS FUNCTION PLEASE ALSO UPDATE THE PROGRAMMERS.
+	//  this is only fallback functionality for really realy basic programmers.
+
+	uint32_t rw;
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
+	int is_flash = 0;
+
+	if( blob_size == 0 ) return 0;
+
+	if( (address_to_write & 0xff000000) == 0x08000000 || (address_to_write & 0xff000000) == 0x00000000 ) 
+	{
+		// Need to unlock flash.
+		// Flash reg base = 0x40022000,
+		// FLASH_MODEKEYR => 0x40022024
+		// FLASH_KEYR => 0x40022004
+
+		if( !iss->flash_unlocked )
+		{
+			if( ( rw = StaticUnlockFlash( dev, iss ) ) )
+				return rw;
+		}
+
+		is_flash = 1;
+
+		printf( "Erasing TO %08x %08x\n", address_to_write, blob_size );
+		MCF.Erase( dev, address_to_write, blob_size, 0 );
+	}
+
+	MCF.FlushLLCommands( dev );
+	MCF.DelayUS( dev, 100 ); // Why do we need this?
+
+	uint32_t wp = address_to_write;
+	uint32_t ew = wp + blob_size;
+	int group = -1;
+
+	while( wp < ew )
+	{
+		if( is_flash )
+		{
+			group = (wp & 0xffffffc0);
+			MCF.WriteWord( dev, (intptr_t)&FLASH->CTLR, CR_PAGE_PG ); // THIS IS REQUIRED.
+			MCF.WriteWord( dev, (intptr_t)&FLASH->CTLR, CR_BUF_RST | CR_PAGE_PG );
+
+			int j;
+			for( j = 0; j < 16; j++ )
+			{
+				int index = (wp-address_to_write);
+				uint32_t data = 0xffffffff;
+				if( index + 3 < blob_size )
+					data = ((uint32_t*)blob)[index/4];
+				else if( (int32_t)(blob_size - index) > 0 )
+				{
+					printf( "%d %d\n", blob_size, index );
+					memcpy( &data, &blob[index], blob_size - index );
+				}
+				MCF.WriteWord( dev, wp, data );
+				wp += 4;
+			}
+			MCF.WriteWord( dev, (intptr_t)&FLASH->ADDR, group );
+			MCF.WriteWord( dev, (intptr_t)&FLASH->CTLR, CR_PAGE_PG|CR_STRT_Set );
+			if( MCF.WaitForFlash ) MCF.WaitForFlash( dev );
+		}
+		else
+		{
+			int index = (wp-address_to_write);
+			uint32_t data = 0xffffffff;
+			if( index + 3 < blob_size )
+				data = ((uint32_t*)blob)[index/4];
+			else if( (int32_t)(blob_size - index) > 0 )
+				memcpy( &data, &blob[index], blob_size - index );
+			printf( "WRITING %08x => %08x\n", data, wp );
+			MCF.WriteWord( dev, wp, data );
+			wp += 4;
+		}
+	}
+
+	if( is_flash )
+	{
+		if( MCF.WaitForFlash && MCF.WaitForFlash( dev ) ) goto timedout;
+	}
+	return 0;
+timedout:
+	fprintf( stderr, "Timed out\n" );
+	return -5;
 }
 
-static int ReadWord( void * dev, uint32_t address_to_read, uint32_t * data )
+static int DefaultReadWord( void * dev, uint32_t address_to_read, uint32_t * data )
 {
-	int r;
-	MCF.WriteReg32( dev, DMPROGBUF0, 0x0002a303 ); // lw x6,0(x5)
-	MCF.WriteReg32( dev, DMPROGBUF0, 0x00100073 ); // ebreak
-	MCF.WriteReg32( dev, DMDATA0, address_to_read );
-	MCF.WriteReg32( dev, DMCOMMAND, 0x00271005 ); // Copy data to x5
-	uint32_t rrv;
-	do
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
+
+	if( iss->statetag != STTAG( "RDSQ" ) || address_to_read != iss->currentstateval )
 	{
-		r = MCF.ReadReg32( dev, DMABSTRACTCS, &rrv );
-		if( r ) return r;
+		if( iss->statetag != STTAG( "RDSQ" ) )
+		{
+			MCF.WriteReg32( dev, DMABSTRACTAUTO, 0 ); // Disable Autoexec.
+
+			// c.lw x8,0(x10) // Pull the address from DATA1
+			// c.lw x9,0(x8)  // Read the data at that location.
+			MCF.WriteReg32( dev, DMPROGBUF0, 0x40044100 );
+			// c.addi x8, 4
+			// c.sw x9, 0(x11) // Write back to DATA0
+			MCF.WriteReg32( dev, DMPROGBUF1, 0xc1840411 );
+			// c.sw x8, 0(x10) // Write addy to DATA1
+			// c.ebreak
+			MCF.WriteReg32( dev, DMPROGBUF2, 0x9002c100 );
+
+			if( iss->statetag != STTAG( "WRSQ" ) )
+			{
+				MCF.WriteReg32( dev, DMDATA0, 0xe00000f4 );   // DATA0's location in memory.
+				MCF.WriteReg32( dev, DMCOMMAND, 0x0023100b ); // Copy data to x11
+				MCF.WriteReg32( dev, DMDATA0, 0xe00000f8 );   // DATA1's location in memory.
+				MCF.WriteReg32( dev, DMCOMMAND, 0x0023100a ); // Copy data to x10
+				MCF.WriteReg32( dev, DMDATA0, 0x40022010 ); //FLASH->CTLR
+				MCF.WriteReg32( dev, DMCOMMAND, 0x0023100c ); // Copy data to x12
+				MCF.WriteReg32( dev, DMDATA0, CR_PAGE_PG|CR_BUF_LOAD);
+				MCF.WriteReg32( dev, DMCOMMAND, 0x0023100d ); // Copy data to x13
+				printf( "REGS CONNED B\n" );
+			}
+			MCF.WriteReg32( dev, DMABSTRACTAUTO, 1 ); // Enable Autoexec.
+		}
+
+		MCF.WriteReg32( dev, DMDATA1, address_to_read );
+		MCF.WriteReg32( dev, DMCOMMAND, 0x00241000 ); // Only execute.
+
+		iss->statetag = STTAG( "RDSQ" );
+		iss->currentstateval = address_to_read;
+
+		MCF.WaitForDoneOp( dev );
 	}
-	while( rrv & (1<<12) );
-	printf( "RRV: %08x\n", rrv );
-	MCF.WriteReg32( dev, DMCOMMAND, 0x00221006 ); // Copy x7 to data0
-	do
+
+	iss->currentstateval += 4;
+
+	return MCF.ReadReg32( dev, DMDATA0, data );
+}
+
+static int StaticUnlockFlash( void * dev, struct InternalState * iss )
+{
+	uint32_t rw;
+	MCF.ReadWord( dev, (intptr_t)&FLASH->CTLR, &rw ); 
+	if( rw & 0x8080 ) 
 	{
-		r = MCF.ReadReg32( dev, DMABSTRACTCS, &rrv );
-		if( r ) return r;
+
+		MCF.WriteWord( dev, (intptr_t)&FLASH->KEYR, 0x45670123 );
+		MCF.WriteWord( dev, (intptr_t)&FLASH->KEYR, 0xCDEF89AB );
+		MCF.WriteWord( dev, (intptr_t)&FLASH->OBKEYR, 0x45670123 );
+		MCF.WriteWord( dev, (intptr_t)&FLASH->OBKEYR, 0xCDEF89AB );
+		MCF.WriteWord( dev, (intptr_t)&FLASH->MODEKEYR, 0x45670123 );
+		MCF.WriteWord( dev, (intptr_t)&FLASH->MODEKEYR, 0xCDEF89AB );
+
+		MCF.ReadWord( dev, (intptr_t)&FLASH->CTLR, &rw );
+		if( rw & 0x8080 ) 
+		{
+			fprintf( stderr, "Error: Flash is not unlocked (CTLR = %08x)\n", rw );
+			return -9;
+		}
 	}
-	while( rrv & (1<<12) );
-	printf( "RRV: %08x\n", rrv );
-	if( (rrv >> 8 ) & 7 )
+	iss->flash_unlocked = 1;
+	return 0;
+}
+
+int DefaultErase( void * dev, uint32_t address, uint32_t length, int type )
+{
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
+	uint32_t rw;
+
+	if( !iss->flash_unlocked )
 	{
-		fprintf( stderr, "Fault writing memory (DMABSTRACTS = %08x)\n", rrv );
+		if( ( rw = StaticUnlockFlash( dev, iss ) ) )
+			return rw;
 	}
-	
-	return MCF.ReadReg32( dev, DMDATA0, data );
+
+	if( type == 1 )
+	{
+		// Whole-chip flash
+		iss->statetag = STTAG( "XXXX" );
+		printf( "Whole-chip erase\n" );
+		MCF.WriteWord( dev, (intptr_t)&FLASH->CTLR, 0 );
+		MCF.WriteWord( dev, (intptr_t)&FLASH->CTLR, FLASH_CTLR_MER  );
+		MCF.WriteWord( dev, (intptr_t)&FLASH->CTLR, CR_STRT_Set|FLASH_CTLR_MER );
+		if( MCF.WaitForFlash && MCF.WaitForFlash( dev ) ) return -11;		
+		MCF.WriteWord( dev, (intptr_t)&FLASH->CTLR, 0 );
+	}
+	else
+	{
+		// 16.4.7, Step 3: Check the BSY bit of the FLASH_STATR register to confirm that there are no other programming operations in progress.
+		// skip (we make sure at the end)
+
+		int chunk_to_erase = address;
+
+		while( chunk_to_erase < address + length )
+		{
+			// Step 4:  set PAGE_ER of FLASH_CTLR(0x40022010)
+			MCF.WriteWord( dev, (intptr_t)&FLASH->CTLR, CR_PAGE_ER ); // Actually FTER
+
+			// Step 5: Write the first address of the fast erase page to the FLASH_ADDR register.
+			MCF.WriteWord( dev, (intptr_t)&FLASH->ADDR, chunk_to_erase  );
+
+			// Step 6: Set the STAT bit of FLASH_CTLR register to '1' to initiate a fast page erase (64 bytes) action.
+			MCF.WriteWord( dev, (intptr_t)&FLASH->CTLR, CR_STRT_Set|CR_PAGE_ER );
+			if( MCF.WaitForFlash && MCF.WaitForFlash( dev ) ) return -99;
+			chunk_to_erase+=64;
+		}
+	}
+	return 0;
 }
 
 int DefaultReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t read_size, uint8_t * blob )
 {
-	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Make the debug module work properly.
-	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Initiate a halt request.
-	MCF.WriteReg32( dev, DMCONTROL, 0x00000001 ); // Clear Halt Request.
-	
-	
+	uint32_t rpos = address_to_read_from;
+	uint32_t rend = address_to_read_from + read_size;
+	while( rpos < rend )
+	{
+		uint32_t rw;
+		int r = DefaultReadWord( dev, rpos, &rw );
+		if( r ) return r;
+		int remain = rend - rpos;
+		if( remain > 3 ) remain = 4;
+		memcpy( blob, &rw, remain );
+		blob += 4;
+		rpos += 4;
+	}
+	return 0;
 }
 
-void TestFunction(void * dev )
+
+static int DefaultHaltMode( void * dev, int mode )
+{
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
+	switch ( mode )
+	{
+	case 0:
+		MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Make the debug module work properly.
+		MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Initiate a halt request.
+		MCF.WriteReg32( dev, DMCONTROL, 0x00000001 ); // Clear Halt Request.
+		MCF.FlushLLCommands( dev );
+		break;
+	case 1:
+		MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Make the debug module work properly.
+		MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Initiate a halt request.
+		MCF.WriteReg32( dev, DMCONTROL, 0x80000003 ); // Reboot.
+		MCF.WriteReg32( dev, DMCONTROL, 0x40000001 ); // resumereq
+		MCF.FlushLLCommands( dev );
+		break;
+	case 2:
+		MCF.WriteReg32( dev, DMCONTROL, 0x40000001 ); // resumereq
+		MCF.FlushLLCommands( dev );
+		break;
+	}
+	iss->processor_in_mode = mode;
+	return 0;
+}
+
+// Returns positive if received text.
+// Returns negative if error.
+// Returns 0 if no text waiting.
+// maxlen MUST be at least 8 characters.  We null terminate.
+int DefaultPollTerminal( void * dev, uint8_t * buffer, int maxlen )
 {
-	uint32_t rv;
 	int r;
-	
-	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Make the debug module work properly.
-	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Initiate a halt request.
-	MCF.WriteReg32( dev, DMCONTROL, 0x00000001 ); // Clear Halt Request.
+	uint32_t rr;
+	r = MCF.ReadReg32( dev, DMDATA0, &rr );
+	if( r < 0 ) return r;
 
-	r = ReadWord( dev, 0x08000004, &rv );
-	printf( "%d %08x\n", r, rv );
-}
+	if( maxlen < 8 ) return -9;
+
+	// DMDATA1:
+	//  bits 0..5 = # printf chars.
+	//  bit  6 = host-wants-to-say-something.
+	//  bit  7 = host-acknowledge.
+	if( rr & 0x80 )
+	{
+		int ret = 0;
+		int num_printf_chars = (rr & 0x3f)-4;  // Actaully can't be more than 32.
 
+		if( num_printf_chars > 0 && num_printf_chars <= 7)
+		{
+			if( num_printf_chars > 3 )
+			{
+				uint32_t r2;
+				r = MCF.ReadReg32( dev, DMDATA1, &r2 );
+				memcpy( buffer+3, &r2, num_printf_chars - 3 );
+			}
+			int firstrem = num_printf_chars;
+			if( firstrem > 3 ) firstrem = 3;
+			memcpy( buffer, ((uint8_t*)&rr)+1, firstrem );
+			buffer[num_printf_chars] = 0;
+			ret = num_printf_chars;
+		}
+		MCF.WriteReg32( dev, DMDATA0, 0x00 ); // Write that we acknowledge the data.
+		return ret;
+	}
+	else
+	{
+		return 0;
+	}
+}
+int DefaultPrintChipInfo( void * dev )
+{
+	uint32_t reg;
+	MCF.HaltMode( dev, 0 );
 
+	if( MCF.ReadWord( dev, 0x1FFFF800, &reg ) ) goto fail;	
+	printf( "USER/RDPR: %08x\n", reg );
+/*	if( MCF.ReadWord( dev, 0x1FFFF804, &reg ) ) goto fail;	
+	printf( "NDATA: %08x\n", reg );
+	if( MCF.ReadWord( dev, 0x1FFFF808, &reg ) ) goto fail;	
+	printf( "WRPR01: %08x\n", reg );
+	if( MCF.ReadWord( dev, 0x1FFFF80c, &reg ) ) goto fail;	
+	printf( "WRPR23: %08x\n", reg );*/
+	if( MCF.ReadWord( dev, 0x1FFFF7E0, &reg ) ) goto fail;
+	printf( "Flash Size: %d kB\n", (reg&0xffff) );
+	if( MCF.ReadWord( dev, 0x1FFFF7E8, &reg ) ) goto fail;	
+	printf( "R32_ESIG_UNIID1: %08x\n", reg );
+	if( MCF.ReadWord( dev, 0x1FFFF7EC, &reg ) ) goto fail;	
+	printf( "R32_ESIG_UNIID2: %08x\n", reg );
+	if( MCF.ReadWord( dev, 0x1FFFF7F0, &reg ) ) goto fail;	
+	printf( "R32_ESIG_UNIID3: %08x\n", reg );
+	return 0;
+fail:
+	fprintf( stderr, "Error: Failed to get chip details\n" );
+	return -11;
+}
 
-int SetupAutomaticHighLevelFunctions()
+int SetupAutomaticHighLevelFunctions( void * dev )
 {
 	// Will populate high-level functions from low-level functions.
 	if( MCF.WriteReg32 == 0 || MCF.ReadReg32 == 0 ) return -5;
@@ -456,6 +948,81 @@ int SetupAutomaticHighLevelFunctions()
 		MCF.WriteBinaryBlob = DefaultWriteBinaryBlob;
 	if( !MCF.ReadBinaryBlob )
 		MCF.ReadBinaryBlob = DefaultReadBinaryBlob;
+	if( !MCF.WriteWord )
+		MCF.WriteWord = DefaultWriteWord;
+	if( !MCF.ReadWord )
+		MCF.ReadWord = DefaultReadWord;
+	if( !MCF.Erase )
+		MCF.Erase = DefaultErase;
+	if( !MCF.HaltMode )
+		MCF.HaltMode = DefaultHaltMode;
+	if( !MCF.PollTerminal )
+		MCF.PollTerminal = DefaultPollTerminal;
+	if( !MCF.WaitForFlash )
+		MCF.WaitForFlash = DefaultWaitForFlash;
+	if( !MCF.WaitForDoneOp )
+		MCF.WaitForDoneOp = DefaultWaitForDoneOp;
+	if( !MCF.PrintChipInfo )
+		MCF.PrintChipInfo = DefaultPrintChipInfo;
+
+	struct InternalState * iss = malloc( sizeof( struct InternalState ) );
+	iss->statetag = 0;
+	iss->currentstateval = 0;
+
+	((struct ProgrammerStructBase*)dev)->internal = iss;
+	return 0;
+}
+
+
+
 
+void TestFunction(void * dev )
+{
+	uint32_t rv;
+	int r;
+	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Make the debug module work properly.
+	MCF.WriteReg32( dev, DMCONTROL, 0x80000001 ); // Initiate a halt request.
+	MCF.WriteReg32( dev, DMCONTROL, 0x00000001 ); // Clear Halt Request.
+
+	r = MCF.WriteWord( dev, 0x20000100, 0xdeadbeef );
+	r = MCF.WriteWord( dev, 0x20000104, 0xcafed0de );
+	r = MCF.WriteWord( dev, 0x20000108, 0x12345678 );
+	r = MCF.WriteWord( dev, 0x20000108, 0x00b00d00 );
+	r = MCF.WriteWord( dev, 0x20000104, 0x33334444 );
+
+	r = MCF.ReadWord( dev, 0x20000100, &rv );
+	printf( "**>>> %d %08x\n", r, rv );
+	r = MCF.ReadWord( dev, 0x20000104, &rv );
+	printf( "**>>> %d %08x\n", r, rv );
+	r = MCF.ReadWord( dev, 0x20000108, &rv );
+	printf( "**>>> %d %08x\n", r, rv );
+
+
+	r = MCF.ReadWord( dev, 0x00000300, &rv );
+	printf( "F %d %08x\n", r, rv );
+	r = MCF.ReadWord( dev, 0x00000304, &rv );
+	printf( "F %d %08x\n", r, rv );
+	r = MCF.ReadWord( dev, 0x00000308, &rv );
+	printf( "F %d %08x\n", r, rv );
+
+	uint8_t buffer[256];
+	int i;
+	for( i = 0; i < 256; i++ ) buffer[i] = 0;
+	MCF.WriteBinaryBlob( dev, 0x08000300, 256, buffer );
+	MCF.ReadBinaryBlob( dev, 0x08000300, 256, buffer );
+	for( i = 0; i < 256; i++ )
+	{
+		printf( "%02x ", buffer[i] );
+		if( (i & 0xf) == 0xf ) printf( "\n" );
+	}
+
+	for( i = 0; i < 256; i++ ) buffer[i] = i;
+	MCF.WriteBinaryBlob( dev, 0x08000300, 256, buffer );
+	MCF.ReadBinaryBlob( dev, 0x08000300, 256, buffer );
+	for( i = 0; i < 256; i++ )
+	{
+		printf( "%02x ", buffer[i] );
+		if( (i & 0xf) == 0xf ) printf( "\n" );
+	}
 }
 
diff --git a/minichlink/minichlink.h b/minichlink/minichlink.h
index 730409c059a232492dcdda79fb34f0834ad87a3d..0b4957c77e751fca250156eb52594e9bdfc2f8eb 100644
--- a/minichlink/minichlink.h
+++ b/minichlink/minichlink.h
@@ -21,13 +21,59 @@ struct MiniChlinkFunctions
 
 	int (*Exit)( void * dev );
 
-	int (*HaltMode)( void * dev, int one_if_halt_zero_if_resume );
+	int (*HaltMode)( void * dev, int mode ); //0 for halt, 1 for reset, 2 for resume
 	int (*ConfigureNRSTAsGPIO)( void * dev, int one_if_yes_gpio );
 
 	// WARNING: Reading/writing must be at 32-bit boundaries for 32-bit sizes.
 	// WARNING: Writing binary blobs may write groups of 64-bytes.
 	int (*WriteBinaryBlob)( void * dev, uint32_t address_to_write, uint32_t blob_size, uint8_t * blob );
 	int (*ReadBinaryBlob)( void * dev, uint32_t address_to_read_from, uint32_t read_size, uint8_t * blob );
+	int (*Erase)( void * dev, uint32_t address, uint32_t length, int type ); //type = 0 for fast, 1 for whole-chip
+
+	// MUST be 4-byte-aligned.
+	int (*WriteWord)( void * dev, uint32_t address_to_write, uint32_t data ); // Flags = 1 for "doing a fast FLASH write."
+	int (*ReadWord)( void * dev, uint32_t address_to_read, uint32_t * data );
+
+	int (*WaitForFlash)( void * dev );
+	int (*WaitForDoneOp)( void * dev );
+
+	int (*PrintChipInfo)( void * dev );
+
+	// TODO: What about 64-byte block-writes?
+	// TODO: What about 64-byte block-reads?
+	// TODO: What about byte read/write?
+	// TODO: What about half read/write?
+
+	// Returns positive if received text.
+	// Returns negative if error.
+	// Returns 0 if no text waiting.
+	int (*PollTerminal)( void * dev, uint8_t * buffer, int maxlen );
+};
+
+/** If you are writing a driver, the minimal number of functions you can implement are:
+	WriteReg32
+	ReadReg32
+	FlushLLCommands
+*/
+
+// Convert a 4-character string to an int.
+#define STTAG( x ) (*((uint32_t*)(x)))
+
+struct InternalState;
+
+struct ProgrammerStructBase
+{
+	struct InternalState * internal;
+	// You can put other things here.
+};
+
+struct InternalState
+{
+	uint32_t statetag;
+	uint32_t currentstateval;
+	uint32_t flash_unlocked;
+	int lastwriteflags;
+	int processor_in_mode;
 };
 
 
@@ -48,9 +94,9 @@ struct MiniChlinkFunctions
 #define DMPROGBUF6     0x26
 #define DMPROGBUF7     0x27
 
-#define CPBR       0x7C
-#define CFGR       0x7D
-#define SHDWCFGR   0x7E
+#define DMCPBR       0x7C
+#define DMCFGR       0x7D
+#define DMSHDWCFGR   0x7E
 
 extern struct MiniChlinkFunctions MCF;
 
@@ -59,7 +105,7 @@ void * TryInit_WCHLinkE();
 void * TryInit_ESP32S2CHFUN();
 
 // Returns 0 if ok, populated, 1 if not populated.
-int SetupAutomaticHighLevelFunctions();
+int SetupAutomaticHighLevelFunctions( void * dev );
 
 #endif
 
diff --git a/minichlink/pgm-esp32s2-ch32xx.c b/minichlink/pgm-esp32s2-ch32xx.c
index c51dcbb5a855c91021026475fa2df5ffe572596e..21d5c9348239870d7189204a9cae938fecc0acb9 100644
--- a/minichlink/pgm-esp32s2-ch32xx.c
+++ b/minichlink/pgm-esp32s2-ch32xx.c
@@ -4,7 +4,10 @@
 
 struct ESP32ProgrammerStruct
 {
+	void * internal;
+
 	hid_device * hd;
+	uint32_t state;
 	uint8_t commandbuffer[256];
 	int commandplace;
 	uint8_t reply[256];
@@ -15,7 +18,7 @@ int ESPFlushLLCommands( void * dev );
 
 static inline int SRemain( struct ESP32ProgrammerStruct * e )
 {
-	return sizeof( e->commandbuffer ) - e->commandplace - 1; //Need room for EOF.
+	return sizeof( e->commandbuffer ) - e->commandplace - 2; //Need room for EOF.
 }
 
 static inline void Write4LE( struct ESP32ProgrammerStruct * e, uint32_t val )
@@ -54,24 +57,25 @@ static int ESPWriteReg32( void * dev, uint8_t reg_7_bit, uint32_t value )
 
 	Write1( eps, (reg_7_bit<<1) | 1 );
 	Write4LE( eps, value );
+	return 0;
 }
 
 int ESPReadReg32( void * dev, uint8_t reg_7_bit, uint32_t * commandresp )
 {
 	struct ESP32ProgrammerStruct * eps = (struct ESP32ProgrammerStruct *)dev;
-	if( SRemain( eps ) < 1 ) ESPFlushLLCommands( eps );
+	ESPFlushLLCommands( eps );
 
 	Write1( eps, (reg_7_bit<<1) | 0 );
 
-	int len = ESPFlushLLCommands( eps );
+	ESPFlushLLCommands( eps );
 
-	if( eps->replylen < 5 )
+	if( eps->replylen < 6 )
 	{
 		return -9;
 	}
 	else
 	{
-		memcpy( commandresp, eps->reply, 4 );
+		memcpy( commandresp, eps->reply+2, 4 );
 		return 0;
 	}
 }
@@ -85,19 +89,25 @@ int ESPFlushLLCommands( void * dev )
 		fprintf( stderr, "Error: Command buffer overflow\n" );
 		return -5; 
 	}
+
+	if( eps->commandplace == 1 ) return 0;
+
+	int r;
+
 	eps->commandbuffer[0] = 0xad; // Key report ID
 	eps->commandbuffer[eps->commandplace] = 0xff;
-	int r = hid_send_feature_report( eps->hd, eps->commandbuffer, 255 );
+	r = hid_send_feature_report( eps->hd, eps->commandbuffer, 255 );
 	eps->commandplace = 1;
 	if( r < 0 )
 	{
 		fprintf( stderr, "Error: Got error %d when sending hid feature report.\n", r );
 		return r;
 	}
-
+retry:
 	eps->reply[0] = 0xad; // Key report ID
 	r = hid_get_feature_report( eps->hd, eps->reply, sizeof( eps->reply ) );
-printf( "RRLEN: %d\n", r );
+	if( eps->reply[0] == 0xff ) goto retry;
+//printf( ">:::%d: %02x %02x %02x %02x %02x %02x\n", eps->replylen, eps->reply[0], eps->reply[1], eps->reply[2], eps->reply[3], eps->reply[4], eps->reply[5] );
 	if( r < 0 )
 	{
 		fprintf( stderr, "Error: Got error %d when sending hid feature report.\n", r );
@@ -120,9 +130,10 @@ int ESPControl3v3( void * dev, int bOn )
 		Write2LE( eps, 0x03fe );
 	else
 		Write2LE( eps, 0x02fe );
+	return 0;
 }
 
-int ESPDelayUS( void * dev, int microseconds )
+static int ESPDelayUS( void * dev, int microseconds )
 {
 	struct ESP32ProgrammerStruct * eps = (struct ESP32ProgrammerStruct *)dev;
 	if( SRemain( eps ) < 6 )
@@ -130,6 +141,7 @@ int ESPDelayUS( void * dev, int microseconds )
 
 	Write2LE( eps, 0x04fe );
 	Write4LE( eps, microseconds );
+	return 0;
 }
 
 
@@ -138,6 +150,7 @@ int ESPExit( void * dev )
 	struct ESP32ProgrammerStruct * eps = (struct ESP32ProgrammerStruct *)dev;
 	hid_close( eps->hd );
 	free( eps );
+	return 0;
 }
 
 void * TryInit_ESP32S2CHFUN()
diff --git a/minichlink/pgm-wch-linke.c b/minichlink/pgm-wch-linke.c
index a9d5b85ad2e78ba2af0132b883fdd10db2140eef..da38118989beacf961d29edde9fc8ed4b830eaf3 100644
--- a/minichlink/pgm-wch-linke.c
+++ b/minichlink/pgm-wch-linke.c
@@ -9,8 +9,14 @@
 #include "libusb.h"
 #include "minichlink.h"
 
+struct LinkEProgrammerStruct
+{
+	void * internal;
+	libusb_device_handle * devh;
+};
+
 #define WCHTIMEOUT 5000
-#define WCHCHECK(x) if( status = x ) { fprintf( stderr, "Bad USB Operation on " __FILE__ ":%d (%d)\n", __LINE__, status ); exit( status ); }
+#define WCHCHECK(x) if( (status = x) ) { fprintf( stderr, "Bad USB Operation on " __FILE__ ":%d (%d)\n", __LINE__, status ); exit( status ); }
 
 const uint8_t * bootloader = (const uint8_t*)
 "\x21\x11\x22\xca\x26\xc8\x93\x77\x15\x00\x99\xcf\xb7\x06\x67\x45" \
@@ -48,14 +54,15 @@ const uint8_t * bootloader = (const uint8_t*)
 
 int bootloader_len = 512;
 
-void wch_link_command( libusb_device_handle * devh, const uint8_t * command, int commandlen, int * transferred, uint8_t * reply, int replymax )
+void wch_link_command( libusb_device_handle * devh, const void * command_v, int commandlen, int * transferred, uint8_t * reply, int replymax )
 {
+	uint8_t * command = (uint8_t*)command_v;
 	uint8_t buffer[1024];
 	int got_to_recv = 0;
 	int status;
 	int transferred_local;
 	if( !transferred ) transferred = &transferred_local;
-	status = libusb_bulk_transfer( devh, 0x01, (char*)command, commandlen, transferred, WCHTIMEOUT );
+	status = libusb_bulk_transfer( devh, 0x01, command, commandlen, transferred, WCHTIMEOUT );
 	if( status ) goto sendfail;
 
 	got_to_recv = 1;
@@ -64,7 +71,7 @@ void wch_link_command( libusb_device_handle * devh, const uint8_t * command, int
 		reply = buffer; replymax = sizeof( buffer );
 	}
 	
-	status = libusb_bulk_transfer( devh, 0x81, (char*)reply, replymax, transferred, WCHTIMEOUT );
+	status = libusb_bulk_transfer( devh, 0x81, reply, replymax, transferred, WCHTIMEOUT );
 	if( status ) goto sendfail;
 	return;
 sendfail:
@@ -105,7 +112,6 @@ static inline libusb_device_handle * wch_link_base_setup( int inhibit_startup )
 	libusb_device *found = NULL;
 	ssize_t cnt = libusb_get_device_list(ctx, &list);
 	ssize_t i = 0;
-	int err = 0;
 	for (i = 0; i < cnt; i++) {
 		libusb_device *device = list[i];
 		struct libusb_device_descriptor desc;
@@ -135,21 +141,22 @@ static inline libusb_device_handle * wch_link_base_setup( int inhibit_startup )
 	return devh;
 }
 
-static int LESetupInterface( void * dev )
+static int LESetupInterface( void * d )
 {
+	libusb_device_handle * dev = ((struct LinkEProgrammerStruct*)d)->devh;
 	uint8_t rbuff[1024];
 	uint32_t transferred = 0;
 
 	// Place part into reset.
-	wch_link_command( (libusb_device_handle *)dev, "\x81\x0d\x01\x01", 4, &transferred, rbuff, 1024 );	// Reply is: "\x82\x0d\x04\x02\x08\x02\x00"
+	wch_link_command( dev, "\x81\x0d\x01\x01", 4, (int*)&transferred, rbuff, 1024 );	// Reply is: "\x82\x0d\x04\x02\x08\x02\x00"
 
 	// TODO: What in the world is this?  It doesn't appear to be needed.
-	wch_link_command( (libusb_device_handle *)dev, "\x81\x0c\x02\x09\x01", 5, 0, 0, 0 ); //Reply is: 820c0101
+	wch_link_command( dev, "\x81\x0c\x02\x09\x01", 5, 0, 0, 0 ); //Reply is: 820c0101
 
 	// This puts the processor on hold to allow the debugger to run.
-	wch_link_command( (libusb_device_handle *)dev, "\x81\x0d\x01\x02", 4, 0, 0, 0 ); // Reply: Ignored, 820d050900300500
+	wch_link_command( dev, "\x81\x0d\x01\x02", 4, 0, 0, 0 ); // Reply: Ignored, 820d050900300500
 
-	wch_link_command( (libusb_device_handle *)dev, "\x81\x11\x01\x09", 4, &transferred, rbuff, 1024 ); // Reply: Chip ID + Other data (see below)
+	wch_link_command( dev, "\x81\x11\x01\x09", 4, (int*)&transferred, rbuff, 1024 ); // Reply: Chip ID + Other data (see below)
 	if( transferred != 20 )
 	{
 		fprintf( stderr, "Error: could not get part status\n" );
@@ -162,8 +169,10 @@ static int LESetupInterface( void * dev )
 	return 0;
 }
 
-static int LEControl3v3( void * dev, int bOn )
+static int LEControl3v3( void * d, int bOn )
 {
+	libusb_device_handle * dev = ((struct LinkEProgrammerStruct*)d)->devh;
+printf( "3v3: %d\n", bOn );
 	if( bOn )
 		wch_link_command( (libusb_device_handle *)dev, "\x81\x0d\x01\x09", 4, 0, 0, 0 );
 	else
@@ -171,8 +180,11 @@ static int LEControl3v3( void * dev, int bOn )
 	return 0;
 }
 
-static int LEControl5v( void * dev, int bOn )
+static int LEControl5v( void * d, int bOn )
 {
+	libusb_device_handle * dev = ((struct LinkEProgrammerStruct*)d)->devh;
+printf( "  5: %d\n", bOn );
+
 	if( bOn )
 		wch_link_command( (libusb_device_handle *)dev, "\x81\x0d\x01\x0b", 4, 0, 0, 0 );
 	else
@@ -182,25 +194,38 @@ static int LEControl5v( void * dev, int bOn )
 
 static int LEUnbrick( void * dev )
 {
-	 wch_link_command( (libusb_device_handle *)dev, "\x81\x0d\x01\x0f\x09", 5, 0, 0, 0 );
+	printf( "Sending unbrick\n" );
+	wch_link_command( (libusb_device_handle *)dev, "\x81\x0d\x01\x0f\x09", 5, 0, 0, 0 );
+	printf( "Done unbrick\n" );
+	return 0;
 }
 
-static int LEHaltMode( void * dev, int one_if_halt_zero_if_resume )
+static int LEHaltMode( void * d, int mode )
 {
-	if( one_if_halt_zero_if_resume )
+	libusb_device_handle * dev = ((struct LinkEProgrammerStruct*)d)->devh;
+
+	if( mode == 0 )
 	{
+		printf( "Holding in reset\n" );
 		// Part one "immediately" places the part into reset.  Part 2 says when we're done, leave part in reset.
 		wch_link_multicommands( (libusb_device_handle *)dev, 2, 4, "\x81\x0d\x01\x02", 4, "\x81\x0d\x01\x01" );
 	}
-	else
+	else if( mode == 1 )
 	{
 		// This is clearly not the "best" method to exit reset.  I don't know why this combination works.
 		wch_link_multicommands( (libusb_device_handle *)dev, 3, 4, "\x81\x0b\x01\x01", 4, "\x81\x0d\x01\x02", 4, "\x81\x0d\x01\xff" );
 	}
+	else
+	{
+		return -93;
+	}
+	return 0;
 }
 
-static int LEConfigureNRSTAsGPIO( void * dev, int one_if_yes_gpio )
+static int LEConfigureNRSTAsGPIO( void * d, int one_if_yes_gpio )
 {
+	libusb_device_handle * dev = ((struct LinkEProgrammerStruct*)d)->devh;
+
 	if( one_if_yes_gpio )
 	{
 		wch_link_multicommands( (libusb_device_handle *)dev, 2, 11, "\x81\x06\x08\x02\xff\xff\xff\xff\xff\xff\xff", 4, "\x81\x0b\x01\x01" );
@@ -209,10 +234,16 @@ static int LEConfigureNRSTAsGPIO( void * dev, int one_if_yes_gpio )
 	{
 		wch_link_multicommands( (libusb_device_handle *)dev, 2, 11, "\x81\x06\x08\x02\xf7\xff\xff\xff\xff\xff\xff", 4, "\x81\x0b\x01\x01" );
 	}
+	return 0;
 }
 
-static int LEReadBinaryBlob( void * dev, uint32_t offset, uint32_t amount, uint8_t * readbuff )
+
+static int LEReadBinaryBlob( void * d, uint32_t offset, uint32_t amount, uint8_t * readbuff )
 {
+	libusb_device_handle * dev = ((struct LinkEProgrammerStruct*)d)->devh;
+
+	LEHaltMode( d, 0 );
+
 	int i;
 	int status;
 	uint8_t rbuff[1024];
@@ -265,8 +296,12 @@ static int LEReadBinaryBlob( void * dev, uint32_t offset, uint32_t amount, uint8
 	return 0;
 }
 
-static int LEWriteBinaryBlob( void * dev, uint32_t address_to_write, uint32_t len, uint8_t * blob )
+static int LEWriteBinaryBlob( void * d, uint32_t address_to_write, uint32_t len, uint8_t * blob )
 {
+	libusb_device_handle * dev = ((struct LinkEProgrammerStruct*)d)->devh;
+
+	LEHaltMode( d, 0 );
+
 	int i;
 	int status;
 	uint8_t rbuff[1024];
@@ -327,9 +362,12 @@ static int LEWriteBinaryBlob( void * dev, uint32_t address_to_write, uint32_t le
 	return 0;
 }
 
-int LEExit( void * dev )
+int LEExit( void * d )
 {
+	libusb_device_handle * dev = ((struct LinkEProgrammerStruct*)d)->devh;
+
 	wch_link_command( (libusb_device_handle *)dev, "\x81\x0d\x01\xff", 4, 0, 0, 0);
+	return 0;
 }
 
 void * TryInit_WCHLinkE()
@@ -338,6 +376,11 @@ void * TryInit_WCHLinkE()
 	wch_linke_devh = wch_link_base_setup(0);
 	if( !wch_linke_devh ) return 0;
 
+
+	struct LinkEProgrammerStruct * ret = malloc( sizeof( struct LinkEProgrammerStruct ) );
+	memset( ret, 0, sizeof( *ret ) );
+	ret->devh = wch_linke_devh;
+
 	MCF.WriteReg32 = 0;
 	MCF.ReadReg32 = 0;
 
@@ -350,7 +393,7 @@ void * TryInit_WCHLinkE()
 	MCF.WriteBinaryBlob = LEWriteBinaryBlob;
 	MCF.ReadBinaryBlob = LEReadBinaryBlob;
 	MCF.Exit = LEExit;
-	return wch_linke_devh;
+	return ret;
 };