diff --git a/inc/extralibs/ch20x_30x_flash.h b/inc/extralibs/ch20x_30x_flash.h
new file mode 100644
index 0000000..6dc44f0
--- /dev/null
+++ b/inc/extralibs/ch20x_30x_flash.h
@@ -0,0 +1,158 @@
+/*
+  Flash library for v20x/v30x. Use the following functions:
+
+  * ch20x_30x_flash_cmd_read(): Read from flash
+  * ch20x_30x_flash_cmd_write(): Write to flash
+  * ch20x_30x_flash_cmd_erase(): Erase flash
+  * ch20x_30x_flash_cmd_verify(): Verify written data.
+
+  Flash erase operations must be in page granularity: addr must be page aligned,
+  and length must be multiple of the page size (256 bytes).
+
+  For flash write operations, addr must be page aligned, and both buf and len
+  must be aligned to 32 bits.
+
+  Read and verify operations do not have alignment restrictions.
+*/
+
+#include <stdint.h>
+#include <string.h> // For memcmp()
+
+#include "ch32fun.h"
+
+#define CH20X_30X_FLASH_PAGE_LEN 256
+#define CH20X_30X_FLASH_PAGE_U32LEN (CH20X_30X_FLASH_PAGE_LEN / sizeof(uint32_t))
+#define CH20X_30X_FLASH_ALIGN_MASK (CH20X_30X_FLASH_PAGE_LEN - 1)
+
+enum {
+	FLASH_OK = 0,           // Success
+	FLASH_ERR_PARAM,        // Parameter error (check alignment!)
+	FLASH_ERR_CLK,          // Clock is too high (use HPRE or lower system clock below 120MHz)
+	FLASH_ERR_VERIFY,       // Verify has failed
+	FLASH_ERR_PROTECT,	// Write protection error
+};
+
+void _ch20x_30x_flash_unlock(void)
+{
+	__disable_irq();
+	FLASH->KEYR = FLASH_KEY1;
+	FLASH->KEYR = FLASH_KEY2;
+
+	FLASH->MODEKEYR = FLASH_KEY1;
+	FLASH->MODEKEYR = FLASH_KEY2;
+}
+
+void _ch20x_30x_flash_lock(void)
+{
+	FLASH->CTLR = FLASH_CTLR_LOCK | FLASH_CTLR_FAST_LOCK;
+	__enable_irq();
+}
+
+void ch20x_30x_flash_cmd_read(uintptr_t addr, uint8_t *buf, uint32_t len)
+{
+	memcpy(buf, (void*)addr, len);
+}
+
+uint32_t _ch20x_30x_flash_busy_poll(void)
+{
+	while (FLASH->STATR & FLASH_STATR_BSY);
+
+	const uint32_t err = FLASH->STATR & FLASH_STATR_WRPRTERR;
+	// Clear status flags (EOP/WRPRTERR)
+	FLASH->STATR = FLASH_STATR_EOP | FLASH_STATR_WRPRTERR;
+
+	return err ? FLASH_ERR_PROTECT : FLASH_OK;
+}
+
+// Writes one page at most
+uint32_t _ch20x_30x_flash_page_write(uintptr_t addr, const uint32_t *data, uint32_t u32len)
+{
+	volatile uint32_t *dst = (volatile uint32_t*)addr;
+
+	FLASH->CTLR = CR_PAGE_PG;  // FTPG, quick program 64-dword page
+	// Write data to buffer (must point to appropriate address
+	for (uint_fast8_t i = 0; i < u32len; i++) {
+		dst[i] = data[i];
+		while (FLASH->STATR & 0x02); // WRBSY
+	}
+
+	// Perform buffer to flash program
+	FLASH->CTLR = CR_PAGE_PG | CR_PG_STRT;
+
+	// Busy poll and return
+	return _ch20x_30x_flash_busy_poll();
+}
+
+// addr must be page aligned (256 bytes).
+// buf and len must be aligned to 32 bits
+uint32_t ch20x_30x_flash_cmd_write(uintptr_t addr, const uint8_t *buf, uint32_t len )
+{
+	uint32_t err = FLASH_OK;
+
+	// Check SYSCLK / HB prescaler is not greater than 120 MHz
+#if (FUNCONF_SYSTEM_CORE_CLOCK > 120000000)
+	if (!(RCC->CFGR0 & RCC_HPRE_3)) {
+		return FLASH_ERR_CLK;
+	}
+#endif
+	if (addr & CH20X_30X_FLASH_ALIGN_MASK || (uintptr_t)buf & 3 || len & 3) {
+		return FLASH_ERR_PARAM;
+	}
+
+	_ch20x_30x_flash_unlock();
+	while (!err && len) {
+		const uint32_t to_write = len < CH20X_30X_FLASH_PAGE_LEN ?
+			len : CH20X_30X_FLASH_PAGE_LEN;
+		err = _ch20x_30x_flash_page_write(addr, (uint32_t*)buf,
+				to_write / sizeof(uint32_t));
+		addr += to_write;
+		buf += to_write;
+		len -= to_write;
+	}
+	FLASH->CTLR = 0;  // Clear FTPG
+	_ch20x_30x_flash_lock();
+
+	return err;
+}
+
+uint32_t _ch20x_30x_flash_page_erase(uintptr_t addr)
+{
+	FLASH->CTLR = CR_PAGE_ER; // FTER, erase a full page
+	FLASH->ADDR = addr;
+	FLASH->CTLR = CR_STRT_Set | CR_PAGE_ER;
+
+	// Busy poll and return
+	return _ch20x_30x_flash_busy_poll();
+}
+
+// addr and len must be page aligned (256 bytes)
+uint32_t ch20x_30x_flash_cmd_erase(uintptr_t addr, uint32_t len)
+{
+	uint32_t err = FLASH_OK;
+
+	// Check SYSCLK / HB prescaler is not greater than 120 MHz
+#if (FUNCONF_SYSTEM_CORE_CLOCK > 120000000)
+	if (!(RCC->CFGR0 & RCC_HPRE_3)) {
+		return FLASH_ERR_CLK;
+	}
+#endif
+	if (addr & CH20X_30X_FLASH_ALIGN_MASK || len & CH20X_30X_FLASH_ALIGN_MASK) {
+		return FLASH_ERR_PARAM;
+	}
+
+	const uint32_t pages = len / CH20X_30X_FLASH_PAGE_LEN;
+	_ch20x_30x_flash_unlock();
+	for (uint32_t i = 0; !err && i < pages; i++) {
+		err = _ch20x_30x_flash_page_erase(addr);
+		addr += CH20X_30X_FLASH_PAGE_LEN;
+	}
+	FLASH->CTLR = 0; // Clear FTER
+	_ch20x_30x_flash_lock();
+
+	return err;
+}
+
+uint32_t ch20x_30x_flash_cmd_verify(uintptr_t addr, uint8_t *buf, int len)
+{
+	return memcmp((void*)addr, buf, len) ? FLASH_ERR_VERIFY : FLASH_OK;
+}
diff --git a/inc/extralibs/ch32v00x_touch.h b/inc/extralibs/ch32v00x_touch.h
new file mode 100644
index 0000000..8cf068b
--- /dev/null
+++ b/inc/extralibs/ch32v00x_touch.h
@@ -0,0 +1,264 @@
+#ifndef _CH32V00X_TOUCH_H
+#define _CH32V00X_TOUCH_H
+
+/** ADC-based Capactive Touch Control.
+
+	see cap_touch_adc.c for an example.
+
+	// Enable GPIOD, C and ADC
+	RCC->APB2PCENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOC | RCC_APB2Periph_ADC1;
+	InitTouchADC();
+
+	// Then do this any time you want to read some touches.
+	sum[0] += ReadTouchPin( GPIOA, 2, 0, iterations );
+	sum[1] += ReadTouchPin( GPIOA, 1, 1, iterations );
+	sum[2] += ReadTouchPin( GPIOC, 4, 2, iterations );
+	sum[3] += ReadTouchPin( GPIOD, 2, 3, iterations );
+	sum[4] += ReadTouchPin( GPIOD, 3, 4, iterations );
+	sum[5] += ReadTouchPin( GPIOD, 5, 5, iterations );
+	sum[6] += ReadTouchPin( GPIOD, 6, 6, iterations );
+	sum[7] += ReadTouchPin( GPIOD, 4, 7, iterations );
+*/
+
+
+
+#define TOUCH_ADC_SAMPLE_TIME 2  // Tricky: Don't change this without a lot of experimentation.
+
+// Can either be 0 or 1.
+// If 0: Measurement low and rises high.  So more pressed is smaller number.
+// If 1: Higher number = harder press. Good to pair with TOUCH_FLAT.
+// If you are doing more prox, use mode 0, otherwise, use mode 1.
+#define TOUCH_SLOPE     1
+
+// If you set this to 1, it will glitch the line, so it will only read
+// anything reasonable if the capacitance can overcome that initial spike.
+// Typically, it seems if you use this you probbly don't need to do
+// any pre-use calibration.
+#define TOUCH_FLAT      0
+
+// Macro used for force-alingining ADC timing
+#define FORCEALIGNADC \
+	asm volatile( \
+		"\n\
+		.balign 4\n\
+		andi a2, %[cyccnt], 3\n\
+		c.slli a2, 1\n\
+		c.addi a2, 12\n\
+		auipc a1, 0\n\
+		c.add  a2, a1\n\
+		jalr a2, 1\n\
+		.long 0x00010001\n\
+		.long 0x00010001\n\
+		"\
+		:: [cyccnt]"r"(SysTick->CNT) : "a1", "a2"\
+	);
+
+
+static void InitTouchADC( );
+void InitTouchADC( )
+{
+#ifdef CH32V003
+
+#define EXTRA_ADC_SETUP_ONLY_ON_00X
+
+
+	// ADCCLK = 24 MHz => RCC_ADCPRE = 0: divide sys clock by 2
+	RCC->CFGR0 &= ~(0x1F<<11);
+
+	// Set up single conversion on chl 2
+	ADC1->RSQR1 = 0;
+	ADC1->RSQR2 = 0;
+
+	// turn on ADC and set rule group to sw trig
+	ADC1->CTLR2 |= ADC_ADON | ADC_EXTSEL;
+
+	// Reset calibration
+	ADC1->CTLR2 |= ADC_RSTCAL;
+	while(ADC1->CTLR2 & ADC_RSTCAL);
+	
+	// Calibrate
+	ADC1->CTLR2 |= ADC_CAL;
+	while(ADC1->CTLR2 & ADC_CAL);
+
+#elif defined( CH32V00x )
+
+#define EXTRA_ADC_SETUP_ONLY_ON_00X ADC1->CTLR2 = ADC_ADON | ADC_EXTSEL;
+
+	// ADCCLK = 48 MHz => RCC_ADCPRE divide by 2
+	// RCC_CFGR0_ADC_CLK_MODE = 0 -- HB clock divides to make ADC clock -- vs 1 = No ADC Division.
+	// RCC_CFGR0_ADC_CLK_ADJ = 0 -- 1/2 duty cycle high level ADC clock
+	RCC->CFGR0 = (RCC->CFGR0 & ~(RCC_ADCPRE | RCC_CFGR0_ADC_CLK_MODE | RCC_HPRE));
+
+	// Set up single conversion on chl 2
+	ADC1->RSQR1 = 0;
+	ADC1->RSQR2 = 0;
+
+	// Turn on ADC module  Not sure why it needs to be done twice.
+	ADC1->CTLR2 = ADC_ADON | ADC_EXTSEL_SWSTART;
+	ADC1->CTLR2 = ADC_ADON | ADC_EXTSEL_SWSTART;
+
+	// enable scanning
+	ADC1->CTLR1 = ADC_SCAN | ADC_BUFEN;
+
+	// Reset calibration
+	ADC1->CTLR2 |= ADC_RSTCAL;
+	while(ADC1->CTLR2 & ADC_RSTCAL);
+	
+	// Calibrate
+	ADC1->CTLR2 |= ADC_CAL;
+	while(ADC1->CTLR2 & ADC_CAL);
+
+#else	
+	#error ADC Code for touch not written for this chip.
+#endif
+}
+
+// Run from RAM to get even more stable timing.
+// This function call takes about 8.1uS to execute.
+static uint32_t ReadTouchPin( GPIO_TypeDef * io, int portpin, int adcno, int iterations ) __attribute__((noinline, section(".srodata")));
+uint32_t ReadTouchPin( GPIO_TypeDef * io, int portpin, int adcno, int iterations )
+{
+	uint32_t ret = 0;
+
+	__disable_irq();
+	FORCEALIGNADC
+	ADC1->RSQR3 = adcno;
+	ADC1->SAMPTR2 = TOUCH_ADC_SAMPLE_TIME<<(3*adcno);
+	__enable_irq();
+
+	uint32_t CFGBASE = io->CFGLR & (~(0xf<<(4*portpin)));
+	uint32_t CFGFLOAT = ((GPIO_CFGLR_IN_PUPD)<<(4*portpin)) | CFGBASE;
+	uint32_t CFGDRIVE = (GPIO_CFGLR_OUT_2Mhz_PP)<<(4*portpin) | CFGBASE;
+
+	// If we run multiple times with slightly different wait times, we can
+	// reduce the impact of the ADC's DNL.
+
+
+#if TOUCH_FLAT == 1
+#define RELEASEIO io->BSHR = 1<<(portpin+16*TOUCH_SLOPE); io->CFGLR = CFGFLOAT;
+#else
+#define RELEASEIO io->CFGLR = CFGFLOAT; io->BSHR = 1<<(portpin+16*TOUCH_SLOPE);
+#endif
+
+#define INNER_LOOP( n ) \
+	{ \
+		/* Only lock IRQ for a very narrow window. */                           \
+		__disable_irq();                                                        \
+		FORCEALIGNADC                                                           \
+                                                                                \
+		EXTRA_ADC_SETUP_ONLY_ON_00X                                             \
+                                                                                \
+		/* Tricky - we start the ADC BEFORE we transition the pin.  By doing    \
+			this We are catching it onthe slope much more effectively.  */      \
+		ADC1->CTLR2 = ADC_SWSTART | ADC_ADON | ADC_EXTSEL;                      \
+                                                                                \
+		ADD_N_NOPS( n )                                                         \
+                                                                                \
+		RELEASEIO                                                               \
+																			    \
+		/* Sampling actually starts here, somewhere, so we can let other        \
+			interrupts run */                                                   \
+		__enable_irq();                                                         \
+		while(!(ADC1->STATR & ADC_EOC));       \
+		io->CFGLR = CFGDRIVE;                                                   \
+		io->BSHR = 1<<(portpin+(16*(1-TOUCH_SLOPE)));                          \
+		ret += ADC1->RDATAR;                                                    \
+	}
+
+	int i;
+	for( i = 0; i < iterations; i++ )
+	{
+		// Wait a variable amount of time based on loop iteration, in order
+		// to get a variety of RC points and minimize DNL.
+
+		INNER_LOOP( 0 );
+		INNER_LOOP( 2 );
+		INNER_LOOP( 4 );
+	}
+
+	return ret;
+}
+
+// Run from RAM to get even more stable timing.
+// This function call takes about 8.1uS to execute.
+static uint32_t ReadTouchPinSafe( GPIO_TypeDef * io, int portpin, int adcno, int iterations ) __attribute__((noinline, section(".srodata")));
+uint32_t ReadTouchPinSafe( GPIO_TypeDef * io, int portpin, int adcno, int iterations )
+{
+	uint32_t ret = 0;
+
+	ADC1->RSQR3 = adcno;
+	ADC1->SAMPTR2 = TOUCH_ADC_SAMPLE_TIME<<(3*adcno);
+
+	// If we run multiple times with slightly different wait times, we can
+	// reduce the impact of the ADC's DNL.
+
+#define INNER_LOOP_SAFE( n ) \
+	{ \
+		/* Only lock IRQ for a very narrow window. */                           \
+		__disable_irq();                                                        \
+		                                                                        \
+		FORCEALIGNADC                                                           \
+                                                                                \
+		EXTRA_ADC_SETUP_ONLY_ON_00X                                             \
+                                                                                \
+		/* Tricky - we start the ADC BEFORE we transition the pin.  By doing    \
+			this We are catching it onthe slope much more effectively.  */      \
+		ADC1->CTLR2 = ADC_SWSTART | ADC_ADON | ADC_EXTSEL;                      \
+                                                                                \
+		ADD_N_NOPS( n )                                                         \
+                                                                                \
+		io->CFGLR = ((GPIO_CFGLR_IN_PUPD)<<(4*portpin)) | (io->CFGLR & (~(0xf<<(4*portpin))));                                                 \
+        io->BSHR = 1<<(portpin+16*TOUCH_SLOPE);                                \
+																			    \
+		/* Sampling actually starts here, somewhere, so we can let other        \
+			interrupts run */                                                   \
+		__enable_irq();                                                         \
+		while(!(ADC1->STATR & ADC_EOC));                                        \
+		__disable_irq();                                                        \
+		io->CFGLR = (GPIO_CFGLR_OUT_2Mhz_PP)<<(4*portpin) | (io->CFGLR & (~(0xf<<(4*portpin))));                                                  \
+		__enable_irq();                                                         \
+		io->BSHR = 1<<(portpin+(16*(1-TOUCH_SLOPE)));                          \
+		ret += ADC1->RDATAR;                                                    \
+	}
+
+	int i;
+	for( i = 0; i < iterations; i++ )
+	{
+		// Wait a variable amount of time based on loop iteration, in order
+		// to get a variety of RC points and minimize DNL.
+
+		INNER_LOOP_SAFE( 0 );
+		INNER_LOOP_SAFE( 2 );
+		INNER_LOOP_SAFE( 4 );
+	}
+
+	return ret;
+}
+
+
+#endif
+
+/*
+ * MIT License
+ * 
+ * Copyright (c) 2023 Valve Corporation
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
diff --git a/inc/extralibs/ch5xx_flash.h b/inc/extralibs/ch5xx_flash.h
new file mode 100644
index 0000000..72ca529
--- /dev/null
+++ b/inc/extralibs/ch5xx_flash.h
@@ -0,0 +1,311 @@
+/*
+  Flash library for ch5xx
+
+  The FUNCONF_CH5XXFLASHLIB_SECTION macro is available for tight RAM situations,
+  some critical functions need to be ran from RAM. This takes 480 bytes when
+  compiled on my machine. With the macro you could do something like this:
+
+  in funconfig.h:
+  #define FUNCONF_CH5XXFLASHLIB_SECTION __attribute__( ( section( ".manual_ram" ) ) )
+
+  in the project's linker file:
+  .manual_ram_inflash :
+  {
+    . = ALIGN(4);
+    PROVIDE(_flash_manual_ram_start = .);
+  } >FLASH AT>FLASH
+
+  .manual_ram :
+  {
+    . = ALIGN(4);
+    PROVIDE(_manual_ram_vma_start = .);
+    *(.manual_ram*)
+    . = ALIGN(4);
+    PROVIDE(_manual_ram_vma_end = .);
+  } >RAM AT>FLASH
+
+  in any C source:
+  extern const uint32_t _manual_ram_vma_start;
+  extern const uint32_t _manual_ram_vma_end;
+  extern const uint32_t _flash_manual_ram_start;
+
+  void load_manual_ram_section( void )
+  {
+    const size_t manual_ram_size = (size_t)( &_manual_ram_vma_end - &_manual_ram_vma_start );
+    memcpy( (void *)&_manual_ram_vma_start, (void *)&_flash_manual_ram_start, manual_ram_size );
+  }
+
+  // when you don't use the function, you can use the ram area as scratch
+  uint8_t *scratch = (uint8_t *)&_manual_ram_vma_start;
+
+  // when need to write to flash:
+  load_manual_ram_section();
+
+  // all ch5xx_flash_cmd_* work now:
+  ch5xx_flash_cmd_erase(...);
+
+  // when done, just continue using the "scratch" buffer for whatever
+*/
+#ifdef FUNCONF_CH5XXFLASHLIB_SECTION
+#undef __HIGH_CODE
+#define __HIGH_CODE FUNCONF_CH5XXFLASHLIB_SECTION
+#endif
+
+__HIGH_CODE
+uint8_t ch5xx_flash_rom_in() {
+	while((int8_t)R8_FLASH_CTRL < 0);
+	return R8_FLASH_DATA;
+}
+
+__HIGH_CODE
+void ch5xx_flash_rom_out(uint8_t val) {
+	while((int8_t)R8_FLASH_CTRL < 0);
+	R8_FLASH_DATA = val;
+}
+
+__HIGH_CODE
+void ch5xx_flash_rom_begin(uint8_t cmd) {
+	R8_FLASH_CTRL = 0;
+	R8_FLASH_CTRL = 0x05;
+	asm volatile ("nop\nnop");
+	R8_FLASH_DATA = cmd;
+	if(cmd == 0xff) {
+		while((int8_t)R8_FLASH_CTRL < 0);
+		R8_FLASH_DATA = cmd;
+		while((int8_t)R8_FLASH_CTRL < 0);
+	}
+}
+
+__HIGH_CODE
+void ch5xx_flash_rom_end() {
+	while((int8_t)R8_FLASH_CTRL < 0);
+	R8_FLASH_CTRL = 0;
+}
+
+__HIGH_CODE
+void ch5xx_flash_rom_open(uint8_t op) {
+	__disable_irq();
+
+	SYS_SAFE_ACCESS(
+		R8_GLOB_ROM_CFG = op;
+	);
+
+
+	R8_FLASH_CTRL = 0x04; // close
+	ch5xx_flash_rom_begin(0xff);
+	ch5xx_flash_rom_end();
+}
+
+__HIGH_CODE
+void ch5xx_flash_rom_open_read() {
+	ch5xx_flash_rom_open(RB_ROM_CTRL_EN);
+}
+
+__HIGH_CODE
+void ch5xx_flash_rom_open_erase_write() {
+	ch5xx_flash_rom_open(RB_ROM_CTRL_EN | RB_ROM_CODE_WE);
+}
+
+__HIGH_CODE
+void ch5xx_flash_rom_close() {
+	ch5xx_flash_rom_end();
+
+	SYS_SAFE_ACCESS(
+		R8_GLOB_ROM_CFG &= RB_ROM_CTRL_EN;
+	);
+
+	__enable_irq();
+}
+
+__HIGH_CODE
+void ch5xx_flash_rom_addr(uint8_t cmd, uint32_t addr) {
+	uint8_t repeat = 5;
+	if((cmd & 0xbf) != 0x0b) {
+		ch5xx_flash_rom_begin(0x06);
+		ch5xx_flash_rom_end();
+		repeat = 3;
+	}
+	ch5xx_flash_rom_begin(cmd);
+	for(int i = 0; i < repeat; i++) {
+		ch5xx_flash_rom_out((uint8_t)(addr >> 0x10));
+		addr <<= 8;
+	}
+}
+
+__HIGH_CODE
+uint8_t ch5xx_flash_rom_wait() {
+	uint8_t status;
+	ch5xx_flash_rom_end();
+	for(int i = 0; i < 524288; i++) {
+		ch5xx_flash_rom_begin(0x05);
+		ch5xx_flash_rom_in();
+		status = ch5xx_flash_rom_in();
+		ch5xx_flash_rom_end();
+		if(!(status & 0x01)) { // check write in progress bit
+			return status | 0x01;
+		}
+	}
+	return 0; // timeout
+}
+
+void ch5xx_flash_cmd_unlock(uint8_t cmd) {
+	// cmd: 0 - unlock all, 0x44 - lock boot code, 0x50 - lock all code and flash, 0x3c - lock something, unclear what exactly
+	ch5xx_flash_rom_open_erase_write();
+	ch5xx_flash_rom_wait();
+	ch5xx_flash_rom_begin(0x06);
+	ch5xx_flash_rom_end();
+	ch5xx_flash_rom_begin(0x01);
+	ch5xx_flash_rom_out(cmd);
+	ch5xx_flash_rom_out(0x02);
+	ch5xx_flash_rom_wait();
+	ch5xx_flash_rom_close();
+}
+
+void ch5xx_flash_cmd_reset() {
+	ch5xx_flash_rom_open_erase_write();
+	ch5xx_flash_rom_begin(0x66);
+	ch5xx_flash_rom_end();
+	ch5xx_flash_rom_begin(0x99);
+	ch5xx_flash_rom_close();
+}
+
+void ch5xx_flash_cmd_read(uint32_t addr, uint8_t *buf, int len) {
+	uint32_t *buf32 = (uint32_t*)buf;
+	int len32 = (len +3) >> 2;
+
+	for(int i = 0; i < len32; i++) {
+		buf32[i] = *((uint32_t*)addr);
+		addr += 4;
+	}
+}
+
+uint32_t ch5xx_flash_cmd_write(uint32_t addr, uint8_t *buf, int len ) {
+	uint32_t *buf32 = (uint32_t*)buf;
+	int len32 = (len +3) >> 2;
+
+	ch5xx_flash_rom_open_erase_write();
+
+	while(len32 > 0) {
+		ch5xx_flash_rom_addr(0x02, addr);
+
+		while(1) {
+			uint32_t data = *buf32++;
+			R32_FLASH_DATA = data;
+			for(int i = 4; i > 0; i--) {
+				while((int8_t)R8_FLASH_CTRL < 0);
+				R8_FLASH_CTRL = 0x15;
+			}
+
+			len32--;
+			addr += 4;
+			// Check for page boundary (typically 256 bytes)
+			if (len32 == 0 || (addr & 0xff) == 0) break;
+		}
+
+		if (ch5xx_flash_rom_wait() == 0)
+			return -1;
+	}
+
+	ch5xx_flash_rom_close();
+	return 0;
+}
+
+uint32_t ch5xx_flash_cmd_erase(uint32_t addr, int len) {
+	addr &= 0xfffff000; // start at a Sector boundary
+	len = (len + (addr & 0xfff) + 0xfff) & 0xfffff000; // adjust length to Sector boundaries
+
+	ch5xx_flash_rom_open_erase_write();
+	uint32_t step;
+
+	while (len > 0) {
+		uint8_t cmd;
+		if (len >= 65536 && !(addr & (65536 -1))) {
+			cmd = 0xd8; // Block Erase
+			step = 65536;
+		}
+#ifdef CH570_CH572
+		else if (len >= 32768 && !(addr & (32768 -1))) {
+			cmd = 0x52; // Half-Block Erase
+			step = 32768;
+		}
+		else {
+			cmd = 0x20; // Sector Erase
+			step = 4096;
+		}
+#else
+		else if (len >= 4096 && (addr & (4096 -1))) {
+			cmd = 0x20; // Sector Erase
+			step = 4096;
+		}
+		else {
+			cmd = 0x81; // Page Erase
+			step = 256;
+		}
+#endif
+
+		ch5xx_flash_rom_addr(cmd, addr);
+		if (ch5xx_flash_rom_wait() == 0) {
+			ch5xx_flash_rom_close();
+			return (uint32_t)-1;
+		}
+		addr += step;
+		len -= step;
+	}
+
+	ch5xx_flash_rom_close();
+	return 0;
+}
+
+uint32_t ch5xx_flash_cmd_verify(uint32_t addr, uint8_t *buf, int len) {
+	uint32_t *buf32 = (uint32_t*)buf;
+	int len32 = (len +3) >> 2;
+
+	ch5xx_flash_rom_open_read();
+	ch5xx_flash_rom_addr(0x0b, addr); // Fast Read command
+
+	for (int i = 0; i < len32; i++) {
+		ch5xx_flash_rom_in(); // Dummy/Data read
+		if ((i & 0x03) == 0) {
+			if (R32_FLASH_DATA != *buf32++) {
+				ch5xx_flash_rom_close();
+				return -1;
+			}
+		}
+	}
+
+	ch5xx_flash_rom_close();
+	return 0;
+}
+
+void ch5xx_flash_cmd_powerup() {
+	ch5xx_flash_rom_open_erase_write();
+	ch5xx_flash_rom_begin(0xab);
+	ch5xx_flash_rom_close();
+}
+
+void ch5xx_flash_cmd_powerdown() {
+	ch5xx_flash_rom_open_erase_write();
+	ch5xx_flash_rom_begin(0xb9);
+	ch5xx_flash_rom_close();
+}
+
+void ch5xx_flash_cmd_get_rom_info(uint32_t addr, uint8_t *buf) {
+	uint32_t *buf32 = (uint32_t*)buf;
+
+	ch5xx_flash_rom_open_read();
+	ch5xx_flash_rom_addr(0x0b, addr);
+	for (int i = 0; i < 8; i++) {
+		ch5xx_flash_rom_in();
+		if(i == 3) {
+			buf32[0] = R32_FLASH_DATA;
+		}
+	}
+	uint32_t val = R32_FLASH_DATA;
+	if (!((addr << 18) & 0x80000000)) {
+		buf32[1] = val;
+	}
+	else {
+		((uint16_t*)buf32)[2] = (uint16_t)val; // Store 16-bit at offset 4
+	}
+	ch5xx_flash_rom_close();
+}
diff --git a/inc/extralibs/ch5xx_lowpower.h b/inc/extralibs/ch5xx_lowpower.h
new file mode 100644
index 0000000..e816a85
--- /dev/null
+++ b/inc/extralibs/ch5xx_lowpower.h
@@ -0,0 +1,121 @@
+/*
+ * Low Power Sleep and Idle library for ch5xx
+ */
+#include "ch32fun.h"
+#include "rtc.h"
+
+void LSIEnable() {
+	SYS_SAFE_ACCESS(
+#ifdef CH570_CH572
+		R8_LSI_CONFIG |= RB_CLK_LSI_PON; //turn on LSI
+#else
+		R8_CK32K_CONFIG &= ~(RB_CLK_OSC32K_XT | RB_CLK_XT32K_PON); // turn off LSE
+		R8_CK32K_CONFIG |= RB_CLK_INT32K_PON; // turn on LSI
+#endif
+	);
+}
+
+void DCDCEnable() {
+#ifndef CH570_CH572 // CH570/2 has no DCDC.
+	SYS_SAFE_ACCESS(
+		R16_AUX_POWER_ADJ |= RB_DCDC_CHARGE;
+		R16_POWER_PLAN |= RB_PWR_DCDC_PRE;
+	);
+
+	RTC_WAIT_TICKS(2);
+
+	SYS_SAFE_ACCESS(
+		R16_POWER_PLAN |= RB_PWR_DCDC_EN;
+	);
+#endif
+}
+
+void SleepInit() {
+	SYS_SAFE_ACCESS(
+		R8_RTC_MODE_CTRL |= RB_RTC_TRIG_EN;  //enable RTC trigger
+   		R8_SLP_WAKE_CTRL |= RB_SLP_RTC_WAKE; // enable wakeup control
+	);
+	NVIC_EnableIRQ(RTC_IRQn);
+}
+
+// enter idle state
+void LowPowerIdleWFI() {
+	NVIC->SCTLR &= ~(1 << 2); // don't deep sleep
+	__WFI();
+}
+
+void LowPowerIdle(uint32_t cyc) {
+	RTC_setAlarm(cyc);
+	LowPowerIdleWFI();
+}
+
+// This macro defines which power pin 
+// to use. If not defined correctly, sleep current
+// will be higher than expected.
+#ifndef FUNCONF_POWERED_BY_V5PIN
+#define FUNCONF_POWERED_BY_V5PIN 0
+#endif
+
+void LowPowerSleepWFI(uint16_t power_plan) {
+#ifdef CH570_CH572
+#if (FUNCONF_POWERED_BY_V5PIN == 1)
+	power_plan |= RB_PWR_LDO5V_EN;
+#endif
+#if ( CLK_SOURCE_CH5XX == CLK_SOURCE_PLL_75MHz ) || ( CLK_SOURCE_CH5XX == CLK_SOURCE_PLL_100MHz )
+	//if system clock is higher than 60Mhz, it need to be reduced before sleep.
+	SYS_SAFE_ACCESS(
+		R8_CLK_SYS_CFG = CLK_SOURCE_PLL_60MHz;
+	);
+#endif
+#else // !570/2
+	SYS_SAFE_ACCESS(
+		R8_BAT_DET_CTRL = 0;
+		R8_XT32K_TUNE = (R16_RTC_CNT_32K > 0x3fff) ? (R8_XT32K_TUNE & 0xfc) | 0x01 : R8_XT32K_TUNE;
+		R8_XT32M_TUNE = (R8_XT32M_TUNE & 0xfc) | 0x03;
+	);
+#endif
+
+	NVIC->SCTLR |= (1 << 2); //deep sleep
+	SYS_SAFE_ACCESS(
+		R8_SLP_POWER_CTRL |= RB_RAM_RET_LV;
+		R16_POWER_PLAN = RB_PWR_PLAN_EN | RB_PWR_CORE | power_plan;
+		R8_PLL_CONFIG |= (1 << 5);
+	);
+	
+	__WFI();
+
+#ifdef CH570_CH572
+#if ( CLK_SOURCE_CH5XX == CLK_SOURCE_PLL_75MHz ) || ( CLK_SOURCE_CH5XX == CLK_SOURCE_PLL_100MHz )
+	//machine delay for a while.
+	uint16_t i = 400;
+	do {
+		asm volatile("nop");
+	} while(i--);
+
+	//get system clock back to normal
+	SYS_SAFE_ACCESS(
+		R8_CLK_SYS_CFG = CLK_SOURCE_CH5XX;
+	);
+#endif
+#else
+	SYS_SAFE_ACCESS(
+		R16_POWER_PLAN &= ~RB_XT_PRE_EN;
+		R8_PLL_CONFIG &= ~(1 << 5);
+		R8_XT32M_TUNE = (R8_XT32M_TUNE & 0xfc) | 0x01;
+	);
+#endif
+}
+
+void LowPowerSleep(uint32_t cyc, uint16_t power_plan) {
+	RTC_setAlarm(cyc);
+	LowPowerSleepWFI(power_plan);
+}
+
+void LowPower(uint32_t time, uint16_t power_plan) {
+	if( time > 500 ) {
+		LowPowerSleep( time, power_plan );
+	}
+	else {
+		LowPowerIdle( time );
+	}
+}
diff --git a/inc/extralibs/fsusb.c b/inc/extralibs/fsusb.c
index 4134cae..52cf0b2 100644
--- a/inc/extralibs/fsusb.c
+++ b/inc/extralibs/fsusb.c
@@ -1,6 +1,7 @@
 #include "fsusb.h"
 #include "ch32fun.h"
 #include <string.h>
+#include <stdio.h>
 
 #ifdef __DMA_SAFE
 // CH573 needs all buffers that will touch DMA be allocated at specific memory location
@@ -11,6 +12,12 @@ __DMA_SAFE
 struct _USBState USBFSCTX;
 volatile uint8_t usb_debug = 0;
 
+#if defined( FUNCONF_USE_USBPRINTF ) && FUNCONF_USE_USBPRINTF
+
+uint8_t usb_inputbuffer[USBFS_PACKET_SIZE]; // this can be extended if polling rate is low
+int usb_inbuf_idx;
+#endif
+
 #if !defined CH5xx && FUSB_USE_DMA7_COPYs
 static inline void copyBuffer( uint8_t * dest, const uint8_t * src, int len )
 {
@@ -45,6 +52,10 @@ static inline void copyBufferComplete() { while( DMA1_Channel7->CNTR ); }
 #define USBFS_IRQHandler USB_IRQHandler
 #endif
 
+#define ANYPRINTF	(defined( FUNCONF_USE_DEBUGPRINTF ) && FUNCONF_USE_DEBUGPRINTF) || \
+					(defined( FUNCONF_USE_UARTPRINTF ) && FUNCONF_USE_UARTPRINTF) || \
+					(defined( FUNCONF_USE_USBPRINTF ) && FUNCONF_USE_USBPRINTF)
+
 #if FUSB_USE_HPE
 // There is an issue with some registers apparently getting lost with HPE, just do it the slow way.
 void USBFS_IRQHandler() __attribute__((section(".text.vector_handler")))  __attribute((interrupt));
@@ -70,24 +81,25 @@ void USBFS_IRQHandler()
 	int len = 0;
 	struct _USBState * ctx = &USBFSCTX;
 	uint8_t * ctrl0buff = CTRL0BUFF;
-	
 	// TODO: Check if needs to be do-while to re-check.
 	if( intfgst & CRB_UIF_TRANSFER )
 	{
 		
 		int token = ( intfgst & CMASK_UIS_TOKEN) >> 12;
 		int ep = ( intfgst & CMASK_UIS_ENDP ) >> 8;
+#if ANYPRINTF
 		if( usb_debug ) printf("[USB] TRANSFER, token = %02x, ep = %d, bmRequestType = %02x, bRequest = %02x\n", token, ep, pUSBFS_SetupReqPak->bmRequestType, pUSBFS_SetupReqPak->bRequest);
+#endif
 		switch ( token )
 		{
 		case CUIS_TOKEN_IN:
 			if( ep )
 			{
-				if( ep < FUSB_CONFIG_EPS )
+				if( ctx->endpoints[ep].mode )
 				{
 #if FUSB_USER_HANDLERS
-					len = HandleInRequest( ctx, ep, ctx->ENDPOINTS[ ep ], 0 );
-#endif    
+					len = HandleInRequest( ctx, ep, ctx->endpoints[ep].in, 0 );
+#endif
 					UEP_CTRL_TX(ep) ^= USBFS_UEP_T_TOG;
 					if( len )
 					{
@@ -99,7 +111,7 @@ void USBFS_IRQHandler()
 					{
 						UEP_CTRL_TX(ep) = ( UEP_CTRL_TX(ep) & ~USBFS_UEP_T_RES_MASK ) | USBFS_UEP_T_RES_NAK;
 					}
-					ctx->USBFS_Endp_Busy[ ep ] = 0;
+					ctx->endpoints[ep].busy = 0;
 				}
 			}
 			else
@@ -107,7 +119,7 @@ void USBFS_IRQHandler()
 				/* end-point 0 data in interrupt */
 				if( ctx->USBFS_SetupReqLen == 0 )
 				{
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 					UEP_CTRL_TX(0) = USBFS_UEP_R_RES_ACK | USBFS_UEP_R_TOG;
 					// R8_UEP0_CTRL = (R8_UEP0_CTRL & ~MASK_UEP_R_RES ) | UEP_R_RES_ACK;
 #else
@@ -142,7 +154,7 @@ void USBFS_IRQHandler()
 						UEP_CTRL_TX(0) ^= USBFS_UEP_T_TOG;
 						ctx->USBFS_SetupReqLen -= len;
 						ctx->pCtrlPayloadPtr += len;
-					}	
+					}
 #endif
 #if FUSB_USER_HANDLERS
 					if( len && USBFSCTX.USBFS_SetupReqCode != HID_GET_REPORT )
@@ -231,7 +243,7 @@ void USBFS_IRQHandler()
 						}
 						else
 						{
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 							UEP_CTRL_TX(0) ^= USBFS_UEP_R_TOG;
 #else
 							UEP_CTRL_RX(0) ^= USBFS_UEP_R_TOG;
@@ -241,13 +253,13 @@ void USBFS_IRQHandler()
 					break;
 
 				default:
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x)|| defined (CH32V10x)
 					UEP_CTRL_TX(ep) ^= USBFS_UEP_R_TOG;
 #else
 					UEP_CTRL_RX(ep) ^= USBFS_UEP_R_TOG;
 #endif
 #if FUSB_USER_HANDLERS
-					HandleDataOut( ctx, ep, ctx->ENDPOINTS[ep], len );
+					HandleDataOut( ctx, ep, ctx->endpoints[ep].out, len );
 #endif
 					break;
 			}
@@ -255,11 +267,11 @@ void USBFS_IRQHandler()
 
 		/* Setup stage processing */
 		case CUIS_TOKEN_SETUP:
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 #if !defined (CH32V10x)
 			if (!(USBFS->INT_ST & 0x80)) goto replycomplete;
 #endif
-			UEP_CTRL_TX(0) = USBFS_UEP_T_RES_NAK | USBFS_UEP_T_TOG | USBFS_UEP_R_RES_NAK | USBFS_UEP_R_TOG | CHECK_USBFS_UEP_AUTO_TOG;
+			UEP_CTRL_TX(0) = USBFS_UEP_T_RES_NAK | USBFS_UEP_T_TOG | USBFS_UEP_R_RES_NAK | USBFS_UEP_R_TOG | CHECK_USBFS_UEP_R_AUTO_TOG;
 #else
 			UEP_CTRL_TX(0) = USBFS_UEP_T_RES_NAK | CHECK_USBFS_UEP_T_AUTO_TOG | USBFS_UEP_T_TOG;
 			UEP_CTRL_RX(0) = USBFS_UEP_R_RES_NAK | CHECK_USBFS_UEP_R_AUTO_TOG | USBFS_UEP_R_TOG;
@@ -271,13 +283,15 @@ void USBFS_IRQHandler()
 			int USBFS_SetupReqLen = USBFSCTX.USBFS_SetupReqLen    = pUSBFS_SetupReqPak->wLength;
 			int USBFS_SetupReqIndex = pUSBFS_SetupReqPak->wIndex;
 			int USBFS_IndexValue = USBFSCTX.USBFS_IndexValue = ( pUSBFS_SetupReqPak->wIndex << 16 ) | pUSBFS_SetupReqPak->wValue;
+#if ANYPRINTF
 			if( usb_debug ) printf( "[USB] SETUP: %02x %02x %02d %02x %04x\n", USBFS_SetupReqType, USBFS_SetupReqCode, USBFS_SetupReqLen, USBFS_SetupReqIndex, USBFS_IndexValue );
+#endif
 			len = 0;
 
 			if( ( USBFS_SetupReqType & USB_REQ_TYP_MASK ) != USB_REQ_TYP_STANDARD )
 			{
 #if FUSB_HID_INTERFACES > 0  || FUSB_USER_HANDLERS
-				if( ( USBFS_SetupReqType & USB_REQ_TYP_MASK ) == USB_REQ_TYP_CLASS )
+				if( ( USBFS_SetupReqType & USB_REQ_TYP_MASK ) == USB_REQ_TYP_CLASS || ( USBFS_SetupReqType & USB_REQ_TYP_MASK ) == USB_REQ_TYP_VENDOR )
 				{
 					/* Class Request */
 					switch( USBFS_SetupReqCode )
@@ -288,9 +302,12 @@ void USBFS_IRQHandler()
 							len = HandleHidUserSetReportSetup( ctx, pUSBFS_SetupReqPak );
 							if( len < 0 ) goto sendstall;
 							ctx->USBFS_SetupReqLen = len;
+#else
+							ctx->USBFS_SetupReqLen = 0;
+#endif
 							UEP_CTRL_LEN(0) = 0;
 							// Previously would have been a CTRL_RX = ACK && TOG, but not here on the 203.
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 							UEP_CTRL_TX(0) = USBFS_UEP_R_RES_ACK | USBFS_UEP_R_TOG | CHECK_USBFS_UEP_T_AUTO_TOG | USBFS_UEP_T_TOG;
 							// R8_UEP0_CTRL = (R8_UEP0_CTRL & ~MASK_UEP_R_RES) | UEP_R_RES_ACK ;
 #else
@@ -298,10 +315,15 @@ void USBFS_IRQHandler()
 							UEP_CTRL_TX(0) = CHECK_USBFS_UEP_T_AUTO_TOG | USBFS_UEP_T_TOG;
 #endif
 							goto replycomplete;
-						
+#if FUSB_HID_USER_REPORTS
 						case HID_GET_REPORT:
 							len = HandleHidUserGetReportSetup( ctx, pUSBFS_SetupReqPak );
 							if( len < 0 ) goto sendstall;
+							if( len == 0 )
+							{
+								USBFS_SendNAK( 0, 1 );
+								goto replycomplete;
+							}
 							ctx->USBFS_SetupReqLen = len;
 							len = len >= DEF_USBD_UEP0_SIZE ? DEF_USBD_UEP0_SIZE : len;
 							if( !ctx->pCtrlPayloadPtr )
@@ -358,9 +380,12 @@ void USBFS_IRQHandler()
 								else
 								{
 									ctx->USBFS_SetupReqLen = len;
-									copyBuffer( ctrl0buff, ctx->pCtrlPayloadPtr, len );
-									// printf("%02x-%02x-%02x-%02x-%02x-%02x-%02x\n", ctrl0buff[0], ctrl0buff[1], ctrl0buff[2], ctrl0buff[3], ctrl0buff[4], ctrl0buff[5], ctrl0buff[6]);
-									ctx->pCtrlPayloadPtr += len;
+									if( ctx->pCtrlPayloadPtr )
+									{
+										copyBuffer( ctrl0buff, ctx->pCtrlPayloadPtr, len );
+										// printf("%02x-%02x-%02x-%02x-%02x-%02x-%02x\n", ctrl0buff[0], ctrl0buff[1], ctrl0buff[2], ctrl0buff[3], ctrl0buff[4], ctrl0buff[5], ctrl0buff[6]);
+										ctx->pCtrlPayloadPtr += len;
+									}
 								}
 								
 								if( ctx->USBFS_SetupReqType & DEF_UEP_IN || ctx->USBFS_SetupReqLen == 0)
@@ -370,11 +395,11 @@ void USBFS_IRQHandler()
 									UEP_CTRL_TX(0) = USBFS_UEP_T_TOG|USBFS_UEP_T_RES_ACK;
 									ctx->USBFS_SetupReqLen -= len;
 								}
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 								else UEP_CTRL_TX(0)= USBFS_UEP_R_TOG|USBFS_UEP_R_RES_ACK;
-#else                
+#else
 								else UEP_CTRL_RX(0)= USBFS_UEP_R_TOG|USBFS_UEP_R_RES_ACK;
-#endif                
+#endif
 								// UEP_CTRL_LEN(0) = len;
 								// UEP_CTRL_TX(0) = CHECK_USBFS_UEP_T_AUTO_TOG | USBFS_UEP_T_RES_ACK | USBFS_UEP_T_TOG;
 								// ctx->USBFS_SetupReqLen -= len;
@@ -383,8 +408,8 @@ void USBFS_IRQHandler()
 							}
 							else
 #endif
-							{                
-								goto sendstall; 
+							{
+								goto sendstall;
 							}
 							break;
 					}
@@ -418,6 +443,13 @@ void USBFS_IRQHandler()
 							len = USBFS_SetupReqLen;
 						ctx->USBFS_SetupReqLen = len;
 
+						// Normally we would send data immediately
+						// but for sake of saving 20 bytes, we will send data on the next IN request.
+						// Leaving this code here in case that we found later that we NEED to send here.
+						// len = ctx->USBFS_SetupReqLen >= DEF_USBD_UEP0_SIZE ? DEF_USBD_UEP0_SIZE : ctx->USBFS_SetupReqLen;
+						// copyBuffer( ctrl0buff, ctx->pCtrlPayloadPtr, len ); // FYI -> Would need to do this if using DMA
+						// ctx->pCtrlPayloadPtr += len;
+
 						break;
 					}
 
@@ -438,6 +470,12 @@ void USBFS_IRQHandler()
 					case USB_SET_CONFIGURATION:
 						ctx->USBFS_DevConfig = (uint8_t)( ctx->USBFS_IndexValue & 0xFF );
 						ctx->USBFS_DevEnumStatus = 0x01;
+						for( int ep = 1; ep < FUSB_MAX_EP_CNT; ep++ )
+						{
+							// reset all DATAx
+							UEP_CTRL_RX(ep) &= ~USBFS_UEP_R_TOG;
+							UEP_CTRL_TX(ep) &= ~USBFS_UEP_T_TOG;
+						}
 						break;
 
 					/* Clear or disable one usb feature */
@@ -463,14 +501,14 @@ void USBFS_IRQHandler()
 							if( (uint8_t)( USBFS_IndexValue & 0xFF ) == USB_REQ_FEAT_ENDP_HALT )
 							{
 								/* Clear End-point Feature */
-								if( ep < FUSB_CONFIG_EPS ) 
+								if( ctx->endpoints[ep].mode ) 
 								{
 									// UEP_CTRL_TX(ep) = USBFS_UEP_T_RES_STALL | CHECK_USBFS_UEP_T_AUTO_TOG;
-									if( USBFS_SetupReqIndex & DEF_UEP_IN  && ctx->endpoint_mode[ep] == -1 ) UEP_CTRL_TX(ep) = USBFS_UEP_T_RES_NAK;
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
-									else if( USBFS_SetupReqIndex & DEF_UEP_OUT && ctx->endpoint_mode[ep] == 1 ) UEP_CTRL_TX(ep) = USBFS_UEP_R_RES_ACK;
+									if( USBFS_SetupReqIndex & DEF_UEP_IN  && (ctx->endpoints[ep].mode & USBFS_EP_MODE_TX) ) UEP_CTRL_TX(ep) = USBFS_UEP_T_RES_NAK;
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
+									else if( USBFS_SetupReqIndex & DEF_UEP_OUT && (ctx->endpoints[ep].mode & USBFS_EP_MODE_RX) ) UEP_CTRL_TX(ep) = USBFS_UEP_R_RES_ACK;
 #else
-									else if( USBFS_SetupReqIndex & DEF_UEP_OUT && ctx->endpoint_mode[ep] == 1 ) UEP_CTRL_RX(ep) = USBFS_UEP_R_RES_ACK;
+									else if( USBFS_SetupReqIndex & DEF_UEP_OUT && (ctx->endpoints[ep].mode & USBFS_EP_MODE_RX) ) UEP_CTRL_RX(ep) = USBFS_UEP_R_RES_ACK;
 #endif
 									else goto sendstall;
 								}
@@ -512,13 +550,13 @@ void USBFS_IRQHandler()
 							/* Set Endpoint Feature */
 							if( (uint8_t)( USBFS_IndexValue & 0xFF ) == USB_REQ_FEAT_ENDP_HALT )
 							{
-								if( ep < FUSB_CONFIG_EPS )
+								if( ctx->endpoints[ep].mode )
 								{
-									if( (USBFS_SetupReqIndex & DEF_UEP_IN) && ctx->endpoint_mode[ep] == -1 )UEP_CTRL_TX(ep) = ( UEP_CTRL_TX(ep) & ~USBFS_UEP_T_RES_MASK ) | USBFS_UEP_T_RES_STALL;
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
-									else if( (USBFS_SetupReqIndex & DEF_UEP_OUT) && ctx->endpoint_mode[ep] == 1 )UEP_CTRL_TX(ep) = ( UEP_CTRL_TX(ep) & ~USBFS_UEP_R_RES_MASK ) | USBFS_UEP_R_RES_STALL;
+									if( (USBFS_SetupReqIndex & DEF_UEP_IN) && (ctx->endpoints[ep].mode & USBFS_EP_MODE_TX) )UEP_CTRL_TX(ep) = ( UEP_CTRL_TX(ep) & ~USBFS_UEP_T_RES_MASK ) | USBFS_UEP_T_RES_STALL;
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
+									else if( (USBFS_SetupReqIndex & DEF_UEP_OUT) && (ctx->endpoints[ep].mode & USBFS_EP_MODE_RX) )UEP_CTRL_TX(ep) = ( UEP_CTRL_TX(ep) & ~USBFS_UEP_R_RES_MASK ) | USBFS_UEP_R_RES_STALL;
 #else
-									else if( (USBFS_SetupReqIndex & DEF_UEP_OUT) && ctx->endpoint_mode[ep] == 1 )UEP_CTRL_RX(ep) = ( UEP_CTRL_RX(ep) & ~USBFS_UEP_R_RES_MASK ) | USBFS_UEP_R_RES_STALL;
+									else if( (USBFS_SetupReqIndex & DEF_UEP_OUT) && (ctx->endpoints[ep].mode & USBFS_EP_MODE_RX) )UEP_CTRL_RX(ep) = ( UEP_CTRL_RX(ep) & ~USBFS_UEP_R_RES_MASK ) | USBFS_UEP_R_RES_STALL;
 #endif
 									else goto sendstall;
 								}
@@ -553,13 +591,13 @@ void USBFS_IRQHandler()
 						}
 						else if( ( USBFS_SetupReqType & USB_REQ_RECIP_MASK ) == USB_REQ_RECIP_ENDP )
 						{
-							if( ep < FUSB_CONFIG_EPS )
+							if( ctx->endpoints[ep].mode )
 							{
-								if( USBFS_SetupReqIndex & DEF_UEP_IN && ctx->endpoint_mode[ep] == -1 ) ctrl0buff[0] = ( UEP_CTRL_TX(ep) & USBFS_UEP_T_RES_MASK ) == USBFS_UEP_T_RES_STALL;
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
-								else if( USBFS_SetupReqIndex & DEF_UEP_OUT && ctx->endpoint_mode[ep] == 1 ) ctrl0buff[0] = ( UEP_CTRL_TX(ep) & USBFS_UEP_R_RES_MASK ) == USBFS_UEP_R_RES_STALL;
+								if( USBFS_SetupReqIndex & DEF_UEP_IN && (ctx->endpoints[ep].mode & USBFS_EP_MODE_TX) ) ctrl0buff[0] = ( UEP_CTRL_TX(ep) & USBFS_UEP_T_RES_MASK ) == USBFS_UEP_T_RES_STALL;
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
+								else if( USBFS_SetupReqIndex & DEF_UEP_OUT && (ctx->endpoints[ep].mode & USBFS_EP_MODE_RX) ) ctrl0buff[0] = ( UEP_CTRL_TX(ep) & USBFS_UEP_R_RES_MASK ) == USBFS_UEP_R_RES_STALL;
 #else
-								else if( USBFS_SetupReqIndex & DEF_UEP_OUT && ctx->endpoint_mode[ep] == 1 ) ctrl0buff[0] = ( UEP_CTRL_TX(ep) & USBFS_UEP_R_RES_MASK ) == USBFS_UEP_R_RES_STALL;
+								else if( USBFS_SetupReqIndex & DEF_UEP_OUT && (ctx->endpoints[ep].mode & USBFS_EP_MODE_RX) ) ctrl0buff[0] = ( UEP_CTRL_RX(ep) & USBFS_UEP_R_RES_MASK ) == USBFS_UEP_R_RES_STALL;
 #endif
 								else goto sendstall;
 							}
@@ -584,10 +622,18 @@ void USBFS_IRQHandler()
 				if( USBFS_SetupReqType & DEF_UEP_IN )
 				{
 					len = ( USBFS_SetupReqLen > DEF_USBD_UEP0_SIZE )? DEF_USBD_UEP0_SIZE : USBFS_SetupReqLen;
-					USBFS_SetupReqLen -= len;
 					UEP_CTRL_LEN(0) = len;
-					UEP_CTRL_TX(0) = USBFS_UEP_T_RES_ACK;
-					// R8_UEP0_CTRL = (R8_UEP0_CTRL & ~MASK_UEP_T_RES) | UEP_T_RES_ACK;
+
+					if (USBFS_SetupReqCode != USB_GET_DESCRIPTOR)
+					{
+						ctx->USBFS_SetupReqLen -= len;
+						UEP_CTRL_TX(0) = USBFS_UEP_T_RES_ACK | USBFS_UEP_T_TOG;
+					}
+					else
+					{
+						UEP_CTRL_LEN(0) = 0;
+						UEP_CTRL_TX(0) = USBFS_UEP_T_RES_ACK;
+					}
 				}
 				else
 				{
@@ -599,8 +645,8 @@ void USBFS_IRQHandler()
 					}
 					else
 					{
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
-						UEP_CTRL_TX(0) = CHECK_USBFS_UEP_T_AUTO_TOG | USBFS_UEP_R_RES_ACK | USBFS_UEP_R_TOG;
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
+						UEP_CTRL_TX(0) = USBFS_UEP_R_RES_ACK | USBFS_UEP_R_TOG;
 						// R8_UEP0_CTRL = (R8_UEP0_CTRL & ~MASK_UEP_R_RES) | UEP_R_RES_ACK ;
 #else
 						UEP_CTRL_RX(0) = CHECK_USBFS_UEP_R_AUTO_TOG | USBFS_UEP_R_RES_ACK | USBFS_UEP_R_TOG;
@@ -614,7 +660,7 @@ void USBFS_IRQHandler()
 			// control flow to hard-abort here.
 		sendstall:
 			// if one request not support, return stall.  Stall means permanent error.
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 			UEP_CTRL_TX(0) = USBFS_UEP_T_TOG | USBFS_UEP_T_RES_STALL | USBFS_UEP_R_TOG | USBFS_UEP_R_RES_STALL;
 #else
 			UEP_CTRL_TX(0) = USBFS_UEP_T_TOG | USBFS_UEP_T_RES_STALL;
@@ -635,7 +681,9 @@ void USBFS_IRQHandler()
 	}
 	else if( intfgst & CRB_UIF_BUS_RST )
 	{
+#if ANYPRINTF
 		if( usb_debug ) printf("[USB] RESET\n");
+#endif
 		/* usb reset interrupt processing */
 		ctx->USBFS_DevConfig = 0;
 		ctx->USBFS_DevAddr = 0;
@@ -648,7 +696,9 @@ void USBFS_IRQHandler()
 	}
 	else if( intfgst & CRB_UIF_SUSPEND )
 	{
+#if ANYPRINTF
 		if( usb_debug ) printf("[USB] SUSPEND\n");
+#endif
 		USBFS->INT_FG = USBFS_UMS_SUSPEND;
 		Delay_Us(10);
 		/* usb suspend interrupt processing */
@@ -671,7 +721,9 @@ void USBFS_IRQHandler()
 	{
 			/* other interrupts */
 			USBFS->INT_FG = intfgst & 0xff;
+#if ANYPRINTF
 			if( usb_debug) printf("[USB] intfgst = %04x\n",intfgst);
+#endif
 	}
 
 #if FUSB_IO_PROFILE
@@ -683,124 +735,132 @@ void USBFS_IRQHandler()
 #endif
 }
 
-void USBFS_InternalFinishSetup()
+WEAK void USBFS_InternalFinishSetup()
 {
+	USBFS->UEP4_1_MOD = 0;
+	USBFS->UEP2_3_MOD = 0;
+#if defined(CH5xx) || defined(CH32X03x)
+	USBFS->UEP567_MOD = 0;
+#else
+	USBFS->UEP5_6_MOD = 0;
+	USBFS->UEP7_MOD = 0;
+#endif
+
+	for( int i = 0; i < FUSB_BUFFERS_NUMBER; i++ )
+	{
+		UEP_CTRL_LEN(i) = 0;
+		memset(USBFSCTX.ep_buffers[i], 0, 64);
+	}
+
+int buffer_counter = 1;
+
+	USBFSCTX.endpoints[0].mode = USBFS_EP_MODE_RX | USBFS_EP_MODE_TX;
 
 #if FUSB_EP1_MODE
-	USBFSCTX.endpoint_mode[1] = FUSB_EP1_MODE;
-#if FUSB_EP1_MODE > 0 
-	USBFS->UEP4_1_MOD = USBFS_UEP1_TX_EN;
-#else
-	USBFS->UEP4_1_MOD = USBFS_UEP1_RX_EN;
-#endif
-#endif
-#if FUSB_EP4_MODE
-	USBFSCTX.endpoint_mode[4] = FUSB_EP4_MODE;
-#if FUSB_EP4_MODE > 0 
-	USBFS->UEP4_1_MOD |= USBFS_UEP4_TX_EN;
-#else
-	USBFS->UEP4_1_MOD |= USBFS_UEP4_RX_EN;
-#endif
+	USBFSCTX.endpoints[1].mode = FUSB_EP1_MODE;
+	USBFS->UEP4_1_MOD |= FUSB_EP1_MODE << 4;
 #endif
 
 #if FUSB_EP2_MODE
-	USBFSCTX.endpoint_mode[2] = FUSB_EP2_MODE;
-#if FUSB_EP2_MODE > 0 
-	USBFS->UEP2_3_MOD = USBFS_UEP2_TX_EN;
-#else
-	USBFS->UEP2_3_MOD = USBFS_UEP2_RX_EN;
-#endif
+	USBFSCTX.endpoints[2].mode = FUSB_EP2_MODE;
+	USBFS->UEP2_3_MOD |= FUSB_EP2_MODE;
 #endif
+
 #if FUSB_EP3_MODE
-	USBFSCTX.endpoint_mode[3] = FUSB_EP3_MODE;
-#if FUSB_EP3_MODE > 0 
-	USBFS->UEP2_3_MOD |= USBFS_UEP3_TX_EN;
-#else
-	USBFS->UEP2_3_MOD |= USBFS_UEP3_RX_EN;
+	USBFSCTX.endpoints[3].mode = FUSB_EP3_MODE;
+	USBFS->UEP2_3_MOD |= FUSB_EP3_MODE << 4;
 #endif
+
+#if FUSB_EP4_MODE
+#if defined(CH5xx) || defined(CH32X03x)
+	USBFSCTX.endpoints[4].mode = FUSB_EP4_MODE &0xc;
+
+	USBFSCTX.endpoints[4].out = ((USBFSCTX.endpoints[4].mode & USBFS_EP_MODE_RX)?((uintptr_t)USBFSCTX.ep_buffers[buffer_counter]):0);
+	USBFSCTX.endpoints[4].in = ((USBFSCTX.endpoints[4].mode & USBFS_EP_MODE_TX)?((uintptr_t)USBFSCTX.ep_buffers[buffer_counter+64]):0);
+	buffer_counter += (((USBFSCTX.endpoints[4].mode & USBFS_EP_MODE_TX)?1:0) + ((USBFSCTX.endpoints[4].mode & USBFS_EP_MODE_RX)?1:0));
+#else
+	USBFSCTX.endpoints[4].mode = FUSB_EP4_MODE;
+#endif
+	USBFS->UEP4_1_MOD |= USBFSCTX.endpoints[4].mode;
 #endif
 
 #if FUSB_EP5_MODE
-	USBFSCTX.endpoint_mode[5] = FUSB_EP5_MODE;
-#if FUSB_EP5_MODE > 0
-#if defined (CH5xx) || defined (CH32X03x)
-	USBFS->UEP567_MOD = USBFS_UEP5_TX_EN;
+#if defined(CH5xx) || defined(CH32X03x)
+	USBFSCTX.endpoints[5].mode = FUSB_EP5_MODE & 0xc;
+	USBFS->UEP567_MOD |= (FUSB_EP5_MODE & 0xc) >> 2;
 #else
-	USBFS->UEP5_6_MOD = USBFS_UEP5_TX_EN;
-#endif
-#else
-#if defined (CH5xx) || defined (CH32X03x)
-	USBFS->UEP567_MOD = USBFS_UEP5_RX_EN;
-#else
-	USBFS->UEP5_6_MOD = USBFS_UEP5_RX_EN;
-#endif
+	USBFSCTX.endpoints[5].mode = FUSB_EP5_MODE;
+	USBFS->UEP5_6_MOD |= FUSB_EP5_MODE;
 #endif
 #endif
+
 #if FUSB_EP6_MODE
-	USBFSCTX.endpoint_mode[6] = FUSB_EP6_MODE;
-#if FUSB_EP6_MODE > 0 
-#if defined (CH5xx) || defined (CH32X03x)
-	USBFS->UEP567_MOD = USBFS_UEP6_TX_EN;
+#if defined(CH5xx) || defined(CH32X03x)
+	USBFSCTX.endpoints[6].mode = FUSB_EP6_MODE & 0xc;
+	USBFS->UEP567_MOD |= FUSB_EP6_MODE & 0xc;
 #else
-	USBFS->UEP5_6_MOD |= USBFS_UEP6_TX_EN;
-#endif
-#else
-#if defined (CH5xx) || defined (CH32X03x)
-	USBFS->UEP567_MOD |= USBFS_UEP6_RX_EN;
-#else
-	USBFS->UEP5_6_MOD |= USBFS_UEP6_RX_EN;
-#endif
+	USBFSCTX.endpoints[6].mode = FUSB_EP6_MODE;
+	USBFS->UEP5_6_MOD |= (FUSB_EP6_MODE & 0xc) << 4;
 #endif
 #endif
 
 #if FUSB_EP7_MODE
-	USBFSCTX.endpoint_mode[7] = FUSB_EP7_MODE;
-#if FUSB_EP7_MODE > 0
-#if defined (CH5xx) || defined (CH32X03x)
-	USBFS->UEP567_MOD |= USBFS_UEP7_TX_EN;
+#if defined(CH5xx) || defined(CH32X03x)
+	USBFSCTX.endpoints[7].mode = FUSB_EP7_MODE & 0xc;
+	USBFS->UEP567_MOD |= (FUSB_EP7_MODE & 0xc) << 2;
 #else
-	USBFS->UEP7_MOD = USBFS_UEP1_TX_EN;
-#endif
-#else
-#if defined (CH5xx) || defined (CH32X03x)
-	USBFS->UEP567_MOD |= USBFS_UEP7_RX_EN;
-#else
-	USBFS->UEP7_MOD = USBFS_UEP1_RX_EN;
-#endif
+	USBFSCTX.endpoints[7].mode = FUSB_EP7_MODE;
+	USBFS->UEP7_MOD = FUSB_EP7_MODE;
 #endif
 #endif
 
-#if !defined (FUSB_CONFIG_EPS) || !FUSB_CONFIG_EPS
-#error You must have at least EP0!
-#endif
+	UEP_DMA(0) = (uintptr_t)USBFSCTX.ep_buffers[0];
+	USBFSCTX.endpoints[0].in = USBFSCTX.ep_buffers[0];
+	USBFSCTX.endpoints[0].out = USBFSCTX.ep_buffers[0];
 
-	for( int i = 0; i < FUSB_CONFIG_EPS; i++ )
+	for( int i = 1; i < FUSB_MAX_EP_CNT; i++ )
 	{
-		UEP_DMA(i) = (uintptr_t)USBFSCTX.ENDPOINTS[i];
+		#if defined(CH5xx) || defined(CH32X03x)
+		if( i == 4 ) continue;
+		#endif
+		if( buffer_counter >= FUSB_BUFFERS_NUMBER ) break;
+		if( USBFSCTX.endpoints[i].mode )
+		{
+			int tx_buf = ((USBFSCTX.endpoints[i].mode & USBFS_EP_MODE_TX)?1:0) + (USBFSCTX.endpoints[i].mode & 1);
+			int rx_buf = ((USBFSCTX.endpoints[i].mode & USBFS_EP_MODE_RX)?1:0) + (USBFSCTX.endpoints[i].mode & 1);
+
+			UEP_DMA(i) = (uintptr_t)USBFSCTX.ep_buffers[buffer_counter];
+			if( rx_buf ) USBFSCTX.endpoints[i].out = (uint8_t *)USBFSCTX.ep_buffers[buffer_counter];
+			if( tx_buf ) USBFSCTX.endpoints[i].in = (uint8_t *)USBFSCTX.ep_buffers[buffer_counter+rx_buf];
+
+			buffer_counter += (tx_buf + rx_buf);
+			// printf("EP%d mode = %d, out buffer = 0x%08x, in buffer = 0x%08x, DMA = %08x\n", i, USBFSCTX.endpoints[i].mode, USBFSCTX.endpoints[i].out, USBFSCTX.endpoints[i].in, UEP_DMA(i));
+		}
 	}
 	
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 	UEP_CTRL_TX(0) = USBFS_UEP_T_RES_NAK | USBFS_UEP_R_RES_ACK | CHECK_USBFS_UEP_T_AUTO_TOG;
 #else
 	UEP_CTRL_TX(0) = USBFS_UEP_T_RES_NAK | CHECK_USBFS_UEP_T_AUTO_TOG;
 	UEP_CTRL_RX(0) = USBFS_UEP_R_RES_ACK | CHECK_USBFS_UEP_R_AUTO_TOG;
 #endif
 
-	for( int i = 1; i < FUSB_CONFIG_EPS; i++ )
+	for( int i = 1; i < FUSB_MAX_EP_CNT; i++ )
 	{
-		if( USBFSCTX.endpoint_mode[i] > 0 )
+		if( USBFSCTX.endpoints[i].mode & USBFS_EP_MODE_TX )
 		{
 			UEP_CTRL_TX(i) = USBFS_UEP_T_RES_NAK;
 		}
-		else if( USBFSCTX.endpoint_mode[i] < 0 )
+
+		if( USBFSCTX.endpoints[i].mode & USBFS_EP_MODE_RX )
 		{
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 			UEP_CTRL_TX(i) = USBFS_UEP_R_RES_ACK;
 #else
 			UEP_CTRL_RX(i) = USBFS_UEP_R_RES_ACK;
 #endif
 		}
-		USBFSCTX.USBFS_Endp_Busy[i] = 0;
+		USBFSCTX.endpoints[i].busy = 0;
 	}
 }
 
@@ -820,9 +880,19 @@ int USBFSSetup()
 #endif
 
 #if defined (CH32V20x) || defined (CH32V30x) || defined(CH32L103)
-#ifdef CH32V30x_D8C
+#if defined(CH32V30x_D8C)
 	RCC->CFGR2 = RCC_USBHSSRC | RCC_USBHSPLL | 1<< RCC_USBHSCLK_OFFSET | RCC_USBHSPLLSRC | 1 << RCC_USBHSDIV_OFFSET;
 	RCC->AHBPCENR |= RCC_USBHSEN;
+#elif defined(CH32L103)
+#if FUNCONF_SYSTEM_CORE_CLOCK == 96000000
+	RCC->CFGR0 = (RCC->CFGR0 & ~(3<<22)) | (1<<22);
+#elif FUNCONF_SYSTEM_CORE_CLOCK == 72000000
+  RCC->CFGR0 = (RCC->CFGR0 & ~(3<<22)) | (2<<22);
+#elif FUNCONF_SYSTEM_CORE_CLOCK == 48000000
+	RCC->CFGR0 = (RCC->CFGR0 & ~(3<<22));
+#else
+#error CH32L103 needs 96/72/48MHz main clock for USB to work
+#endif
 #else
 	// USBPRE[1:0] = 10: Divided by 3 (when PLLCLK=144MHz);
 	// Must be done before enabling clock to USBFS tree.
@@ -842,11 +912,11 @@ int USBFSSetup()
 
 #if defined (CH32X03x)
 	RCC->APB2PCENR |= RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOC;
-// #ifdef FUSB_VDD_5V
-	// AFIO->CTLR = (AFIO->CTLR & ~(UDP_PUE_MASK | UDM_PUE_MASK | USB_PHY_V33)) | UDP_PUE_10K | USB_IOEN;
-// #else
+#if FUSB_VDD_5V
+	AFIO->CTLR = (AFIO->CTLR & ~(UDP_PUE_MASK | UDM_PUE_MASK | USB_PHY_V33)) | UDP_PUE_10K | USB_IOEN;
+#else
 	AFIO->CTLR = (AFIO->CTLR & ~(UDP_PUE_MASK | UDM_PUE_MASK )) | USB_PHY_V33 | UDP_PUE_1K5 | USB_IOEN;
-// #endif
+#endif
 	// Enable PC16/17 Alternate Function (USB)
 	// According to EVT, GPIO16 = GPIO_Mode_IN_FLOATING, GPIO17 = GPIO_Mode_IPU
 	GPIOC->CFGXR = 	( GPIOC->CFGXR & ~( (0xf<<(4*0)) | (0xf<<(4*1)) ) )  |
@@ -888,7 +958,7 @@ int USBFSSetup()
 	USBFS->BASE_CTRL = USBFS_UC_INT_BUSY | USBFS_UC_DMA_EN | USBFS_UC_DEV_PU_EN;
 	USBFS->INT_FG    = 0xff;
 	USBFS_InternalFinishSetup();
-	USBFS->UDEV_CTRL = USBFS_UD_PD_DIS | USBFS_UD_PORT_EN;
+	USBFS->UDEV_CTRL = USBFS_UD_PORT_EN; // Previously here also was USBFS_UD_PD_DIS, but cnlohr encountered instabilities with it on ch570
 
 	NVIC_EnableIRQ( USB_IRQn );
 
@@ -923,38 +993,43 @@ void USBFSReset()
 	USBFS->BASE_CTRL = USBFS_UC_RESET_SIE | USBFS_UC_CLR_ALL;
 	USBFS->BASE_CTRL = 0x00;
 	Delay_Us(10);
+}
+#else
+void USBFSReset()
+{
+
 }
 #endif
 
-static inline uint8_t * USBFS_GetEPBufferIfAvailable( int endp )
+uint8_t * USBFS_GetEPBufferIfAvailable( int endp )
 {
-	if( USBFSCTX.USBFS_Endp_Busy[ endp ] ) return 0;
-	return USBFSCTX.ENDPOINTS[ endp ];
+	if( USBFSCTX.endpoints[endp].busy ) return 0;
+	return USBFSCTX.endpoints[ endp ].in;
 }
 
-static inline int USBFS_SendEndpoint( int endp, int len )
+int USBFS_SendEndpoint( int endp, int len )
 {
-	if( USBFSCTX.USBFS_errata_dont_send_endpoint_in_window || USBFSCTX.USBFS_Endp_Busy[ endp ] ) return -1;
+	if( USBFSCTX.USBFS_errata_dont_send_endpoint_in_window || USBFSCTX.endpoints[endp].busy ) return -1;
 	// This prevents sending while ep0 is receiving
 	if( USBFSCTX.USBFS_SetupReqLen > 0 ) return -2;
-#if defined (CH5xx) || defined (CH32X03x)
+#if defined(CH5xx) || defined(CH32X03x)
 	// Check RB_UIS_SETUP_ACT
 	if( (USBFS->INT_ST & 0x80) ) return -3;
 #endif
 	NVIC_DisableIRQ( USB_IRQn );
 	UEP_CTRL_LEN( endp ) = len;
 	UEP_CTRL_TX( endp ) = ( UEP_CTRL_TX( endp ) & ~USBFS_UEP_T_RES_MASK ) | USBFS_UEP_T_RES_ACK;
-	USBFSCTX.USBFS_Endp_Busy[ endp ] = 1;
+	USBFSCTX.endpoints[endp].busy = 1;
 	NVIC_EnableIRQ( USB_IRQn );
 	return 0;
 }
 
 int USBFS_SendEndpointNEW( int endp, uint8_t* data, int len, int copy)
 {
-	if( USBFSCTX.USBFS_errata_dont_send_endpoint_in_window || USBFSCTX.USBFS_Endp_Busy[ endp ] ) return -1;
+	if( USBFSCTX.USBFS_errata_dont_send_endpoint_in_window || USBFSCTX.endpoints[endp].busy ) return -1;
 	// This prevents sending while ep0 is receiving
 	if( USBFSCTX.USBFS_SetupReqLen > 0 ) return USBFSCTX.USBFS_SetupReqLen;
-#if defined (CH5xx) || defined (CH32X03x)
+#if defined(CH5xx) || defined(CH32X03x)
 	// Check RB_UIS_SETUP_ACT
 	if( (USBFS->INT_ST & 0x80) ) return -3;
 #endif
@@ -962,24 +1037,36 @@ int USBFS_SendEndpointNEW( int endp, uint8_t* data, int len, int copy)
 	{
 		if( copy )
 		{
-			UEP_DMA( endp ) = (uintptr_t)USBFSCTX.ENDPOINTS[endp];
-			copyBuffer( USBFSCTX.ENDPOINTS[endp], data, len );
+#if defined(CH5xx) || defined(CH32X03x)
+			if ( endp != 4 ) UEP_DMA( endp ) = (uintptr_t)USBFSCTX.ep_buffers[endp];
+#else
+			UEP_DMA( endp ) = (uintptr_t)USBFSCTX.ep_buffers[endp];
+#endif
+			copyBuffer( USBFSCTX.endpoints[endp].in, data, len );
 			copyBufferComplete();
 		}
-		else UEP_DMA( endp ) = (uintptr_t)data;
+		else
+		{
+		// EP in bi-directional mode won't work with direct DMA
+#if defined(CH5xx) || defined(CH32X03x)
+			if ( endp != 4 ) UEP_DMA( endp ) = (uintptr_t)data;
+#else
+			UEP_DMA( endp ) = (uintptr_t)data;
+#endif
+		}
 	}
 	// NVIC_DisableIRQ( USB_IRQn );
 	UEP_CTRL_LEN( endp ) = len;
 	UEP_CTRL_TX( endp ) = ( UEP_CTRL_TX( endp ) & ~USBFS_UEP_T_RES_MASK ) | USBFS_UEP_T_RES_ACK;
-	USBFSCTX.USBFS_Endp_Busy[ endp ] = 1;
+	USBFSCTX.endpoints[endp].busy = 1;
 	// NVIC_EnableIRQ( USB_IRQn );
 	return 0;
 }
 
-static inline int USBFS_SendACK( int endp, int tx )
+int USBFS_SendACK( int endp, int tx )
 {
 	if( tx ) UEP_CTRL_TX( endp ) = ( UEP_CTRL_TX( endp ) & ~USBFS_UEP_T_RES_MASK ) | USBFS_UEP_T_RES_ACK;
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 	else UEP_CTRL_TX(endp) = ( UEP_CTRL_TX(endp) & ~USBFS_UEP_R_RES_MASK ) | USBFS_UEP_R_RES_ACK;
 #else
 	else UEP_CTRL_RX(endp) = ( UEP_CTRL_RX(endp) & ~USBFS_UEP_R_RES_MASK ) | USBFS_UEP_R_RES_ACK;
@@ -987,10 +1074,10 @@ static inline int USBFS_SendACK( int endp, int tx )
 	return 0;
 }
 
-static inline int USBFS_SendNAK( int endp, int tx )
+int USBFS_SendNAK( int endp, int tx )
 {
 	if( tx ) UEP_CTRL_TX( endp ) = ( UEP_CTRL_TX( endp ) & ~USBFS_UEP_T_RES_MASK ) | USBFS_UEP_T_RES_NAK;
-#if defined (CH5xx) || defined (CH32X03x) || defined (CH32V10x)
+#if defined(CH5xx) || defined(CH32X03x) || defined (CH32V10x)
 	else UEP_CTRL_TX(endp) = ( UEP_CTRL_TX(endp) & ~USBFS_UEP_R_RES_MASK ) | USBFS_UEP_R_RES_NAK;
 #else
 	else UEP_CTRL_RX(endp) = ( UEP_CTRL_RX(endp) & ~USBFS_UEP_R_RES_MASK ) | USBFS_UEP_R_RES_NAK;
@@ -999,20 +1086,46 @@ static inline int USBFS_SendNAK( int endp, int tx )
 }
 
 #if defined( FUNCONF_USE_USBPRINTF ) && FUNCONF_USE_USBPRINTF
-int HandleInRequest( struct _USBState *ctx, int endp, uint8_t *data, int len )
+__attribute__((weak)) int HandleInRequest( struct _USBState *ctx, int endp, uint8_t *data, int len )
 {
 	return 0;
 }
 
-void HandleDataOut( struct _USBState *ctx, int endp, uint8_t *data, int len )
+// static uint8_t usb_inputbuffer[USBFS_PACKET_SIZE]; // this can be extended if polling rate is low
+// static int usb_inbuf_idx;
+WEAK void HandleDataOut( struct _USBState *ctx, int endp, uint8_t *data, int len )
 {
 	if ( endp == 0 )
 	{
 		ctx->USBFS_SetupReqLen = 0; // To ACK
 	}
+	else if( endp == 2 )
+	{
+		// discard oldest if polling is too slow
+		int headroom = (sizeof(usb_inputbuffer) - usb_inbuf_idx) - len;
+		if(headroom < 0) {
+			// not enough space left, free up some
+			int offset = -headroom;
+			for(int i = offset; i < sizeof(usb_inputbuffer); i++) {
+				usb_inputbuffer[i -offset] = usb_inputbuffer[i];
+			}
+			usb_inbuf_idx -= offset;
+		}
+
+		for(int i = 0; i < len; i++) {
+			usb_inputbuffer[usb_inbuf_idx++] = data[i];
+		}
+	}
 }
 
-int HandleSetupCustom( struct _USBState *ctx, int setup_code )
+void poll_input() {
+	if(usb_inbuf_idx) {
+		HandleUSBInput(usb_inbuf_idx, usb_inputbuffer);
+		usb_inbuf_idx = 0;
+	}
+}
+
+WEAK int HandleSetupCustom( struct _USBState *ctx, int setup_code )
 {
 	int ret = -1;
 	if ( ctx->USBFS_SetupReqType & USB_REQ_TYP_CLASS )
diff --git a/inc/extralibs/fsusb.h b/inc/extralibs/fsusb.h
index 616c67f..a3f7ece 100644
--- a/inc/extralibs/fsusb.h
+++ b/inc/extralibs/fsusb.h
@@ -29,9 +29,9 @@ typedef struct
 	__IO uint8_t  MIS_ST;
 	__IO uint8_t  INT_FG; // "Combined" register in some situations. (ST_FG)
 	__IO uint8_t  INT_ST;
-	__IO uint8_t RX_LEN;
+	__IO uint8_t  RX_LEN;
 	__IO uint8_t  Reserved0;
-	__IO uint16_t  Reserved1;
+	__IO uint16_t Reserved1;
 	__IO uint8_t  UEP4_1_MOD;
 	__IO uint8_t  UEP2_3_MOD;
 	__IO uint8_t  UEP567_MOD;
@@ -44,61 +44,66 @@ typedef struct
 	__IO uint16_t Reserved5;
 	__IO uint16_t UEP3_DMA;
 	__IO uint16_t Reserved6;
-	__IO uint8_t UEP0_TX_LEN;
-	__IO uint8_t Reserved7;
+	__IO uint8_t  UEP0_TX_LEN;
+	__IO uint8_t  Reserved7;
 	__IO uint8_t  UEP0_TX_CTRL;
-	__IO uint8_t Reserved8;
-	__IO uint8_t UEP1_TX_LEN;
-	__IO uint8_t Reserved9;
+	__IO uint8_t  Reserved8;
+	__IO uint8_t  UEP1_TX_LEN;
+	__IO uint8_t  Reserved9;
 	__IO uint8_t  UEP1_TX_CTRL;
-	__IO uint8_t Reserved10;
-	__IO uint8_t UEP2_TX_LEN;
-	__IO uint8_t Reserved11;
+	__IO uint8_t  Reserved10;
+	__IO uint8_t  UEP2_TX_LEN;
+	__IO uint8_t  Reserved11;
 	__IO uint8_t  UEP2_TX_CTRL;
-	__IO uint8_t Reserved12;
-	__IO uint8_t UEP3_TX_LEN;
-	__IO uint8_t Reserved13;
+	__IO uint8_t  Reserved12;
+	__IO uint8_t  UEP3_TX_LEN;
+	__IO uint8_t  Reserved13;
 	__IO uint8_t  UEP3_TX_CTRL;
-	__IO uint8_t Reserved14;
-	__IO uint8_t UEP4_TX_LEN;
-	__IO uint8_t Reserved15;
+	__IO uint8_t  Reserved14;
+	__IO uint8_t  UEP4_TX_LEN;
+	__IO uint8_t  Reserved15;
 	__IO uint8_t  UEP4_TX_CTRL;
-	__IO uint8_t Reserved16;
+	__IO uint8_t  Reserved16;
 	__IO uint32_t Reserved17;
 	__IO uint32_t Reserved18;
 	__IO uint32_t Reserved19;
-	__IO uint32_t Reserved20;
+	__IO uint32_t EP_JMPR;        // Nothing here, but used for UEP 5,6,7 jump in UEP_(n) macros
 	__IO uint32_t Reserved21;
 	__IO uint32_t Reserved22;
 	__IO uint32_t Reserved23;
-	__IO uint32_t Reserved24;
-	__IO uint16_t UEP5_DMA;
-	__IO uint16_t UEP6_DMA;
-	__IO uint16_t UEP7_DMA;
-	__IO uint32_t Reserved25;
-	__IO uint8_t UEP5_TX_LEN;
-	__IO uint8_t Reserved26;
-	__IO uint8_t  UEP5_TX_CTRL;
-	__IO uint8_t Reserved27;
-	__IO uint8_t UEP6_TX_LEN;
-	__IO uint8_t Reserved28;
-	__IO uint8_t  UEP6_TX_CTRL;
-	__IO uint8_t Reserved29;
-	__IO uint8_t UEP7_TX_LEN;
-	__IO uint8_t  UEP7_TX_CTRL;
-	__IO uint8_t Reserved30;
-	__IO uint32_t EPX_MODE;
+	__IO uint32_t Reserved24;     // 0x50
+	__IO uint16_t UEP5_DMA;       // 0x54
+	__IO uint16_t Reserved25;     // 0x56
+	__IO uint16_t UEP6_DMA;       // 0x58
+	__IO uint16_t Reserved26;     // 0x5A
+	__IO uint16_t UEP7_DMA;       // 0x5C
+	__IO uint16_t Reserved27;     // 0x5E
+	__IO uint32_t Reserved28;     // 0x60
+	__IO uint8_t  UEP5_TX_LEN;    // 0x64
+	__IO uint8_t  Reserved29;     // 0x65
+	__IO uint8_t  UEP5_TX_CTRL;   // 0x66
+	__IO uint8_t  Reserved30;     // 0x67
+	__IO uint8_t  UEP6_TX_LEN;    // 0x68
+	__IO uint8_t  Reserved31;     // 0x69
+	__IO uint8_t  UEP6_TX_CTRL;   // 0x6A
+	__IO uint8_t  Reserved32;     // 0x6B
+	__IO uint8_t  UEP7_TX_LEN;    // 0x6C
+	__IO uint8_t  Reserved33;     // 0x6D
+	__IO uint8_t  UEP7_TX_CTRL;   // 0x6E
+	__IO uint8_t  Reserved34;     // 0x6F
+	__IO uint32_t EPX_MODE;       // 0x70
 } USBFS_TypeDef;
 
-#define UEP_CTRL_LEN(n) (((uint8_t*)&USBFS->UEP0_TX_LEN)[n*4])
-#define UEP_CTRL_TX(n)  (((uint8_t*)&USBFS->UEP0_TX_CTRL)[n*4])
-#define UEP_CTRL_RX(n)  (((uint8_t*)&USBFS->UEP0_TX_CTRL)[n*4])
-#define UEP_DMA(n)      (((uint16_t*)&USBFS->UEP0_DMA)[n*2])
+#define UEP_JMP         ((void*)&(USBFS->EP_JMPR) - (void*)&(USBFS->UEP0_DMA))
+#define UEP_CTRL_LEN(n) (((volatile uint8_t*)&USBFS->UEP0_TX_LEN)[n*4 +((n>4) ? UEP_JMP : 0)])
+#define UEP_CTRL_TX(n)  (((volatile uint8_t*)&USBFS->UEP0_TX_CTRL)[n*4 +((n>4) ? UEP_JMP : 0)])
+#define UEP_CTRL_RX(n)  (((volatile uint8_t*)&USBFS->UEP0_TX_CTRL)[n*4 +((n>4) ? UEP_JMP : 0)])
+#define UEP_DMA(n)      (((volatile uint16_t*)&USBFS->UEP0_DMA)[n*2 +((n>4) ? (UEP_JMP /2) : 0)]) // BREAKS FOR EP4!!!
 #endif
 
 #if defined(CH32V10x)
-#define USBFS_BASE 0x40023400
-#define USB_IRQn USBFS_IRQn
+#define USBFS_BASE      0x40023400
+#define USB_IRQn        USBFS_IRQn
 typedef struct
 {
 	__IO uint8_t  BASE_CTRL;
@@ -109,9 +114,9 @@ typedef struct
 	__IO uint8_t  MIS_ST;
 	__IO uint8_t  INT_FG; // "Combined" register in some situations. (ST_FG)
 	__IO uint8_t  INT_ST;
-	__IO uint8_t RX_LEN;
+	__IO uint8_t  RX_LEN;
 	__IO uint8_t  Reserved0;
-	__IO uint16_t  Reserved1;
+	__IO uint16_t Reserved1;
 	__IO uint8_t  UEP4_1_MOD;
 	__IO uint8_t  UEP2_3_MOD;
 	__IO uint8_t  UEP5_6_MOD;
@@ -133,29 +138,29 @@ typedef struct
 	__IO uint16_t UEP7_DMA;
 	__IO uint16_t Reserved10;
 	__IO uint16_t UEP0_TX_LEN;
-	__IO uint8_t UEP0_TX_CTRL;
-	__IO uint8_t Reserved11;
+	__IO uint8_t  UEP0_TX_CTRL;
+	__IO uint8_t  Reserved11;
 	__IO uint16_t UEP1_TX_LEN;
-	__IO uint8_t UEP1_TX_CTRL;
-	__IO uint8_t Reserved12;
+	__IO uint8_t  UEP1_TX_CTRL;
+	__IO uint8_t  Reserved12;
 	__IO uint16_t UEP2_TX_LEN;
-	__IO uint8_t UEP2_TX_CTRL;
-	__IO uint8_t Reserved13;
+	__IO uint8_t  UEP2_TX_CTRL;
+	__IO uint8_t  Reserved13;
 	__IO uint16_t UEP3_TX_LEN;
-	__IO uint8_t UEP3_TX_CTRL;
-	__IO uint8_t Reserved14;
+	__IO uint8_t  UEP3_TX_CTRL;
+	__IO uint8_t  Reserved14;
 	__IO uint16_t UEP4_TX_LEN;
-	__IO uint8_t UEP4_TX_CTRL;
-	__IO uint8_t Reserved15;
+	__IO uint8_t  UEP4_TX_CTRL;
+	__IO uint8_t  Reserved15;
 	__IO uint16_t UEP5_TX_LEN;
-	__IO uint8_t UEP5_TX_CTRL;
-	__IO uint8_t Reserved16;
+	__IO uint8_t  UEP5_TX_CTRL;
+	__IO uint8_t  Reserved16;
 	__IO uint16_t UEP6_TX_LEN;
-	__IO uint8_t UEP6_TX_CTRL;
-	__IO uint8_t Reserved17;
+	__IO uint8_t  UEP6_TX_CTRL;
+	__IO uint8_t  Reserved17;
 	__IO uint16_t UEP7_TX_LEN;
-	__IO uint8_t UEP7_TX_CTRL;
-	__IO uint8_t Reserved18;
+	__IO uint8_t  UEP7_TX_CTRL;
+	__IO uint8_t  Reserved18;
 } USBFS_TypeDef;
 
 #define UEP_CTRL_LEN(n) (((volatile uint16_t*)&USBFS->UEP0_TX_LEN)[n*2])
@@ -165,133 +170,155 @@ typedef struct
 #endif
 
 #if !defined (CH32X03x)
-#define DEBUG_PIN PA8
+#define DEBUG_PIN             PA8
 
-#define DEF_USBD_UEP0_SIZE 64	 /* usb hs/fs device end-point 0 size */
-#define UEP_SIZE 64
+#define DEF_USBD_UEP0_SIZE    64 // USB FS device end-point 0 size
+#define UEP_SIZE              64
 
-#define DEF_UEP_IN   0x80
-#define DEF_UEP_OUT  0x00
-#define DEF_UEP_BUSY 0x01
-#define DEF_UEP_FREE 0x00
+#define DEF_UEP_IN            0x80
+#define DEF_UEP_OUT           0x00
+#define DEF_UEP_BUSY          0x01
+#define DEF_UEP_FREE          0x00
 
-#define DEF_UEP0 0
-#define DEF_UEP1 1
-#define DEF_UEP2 2
-#define DEF_UEP3 3
-#define DEF_UEP4 4
-#define DEF_UEP5 5
-#define DEF_UEP6 6
-#define DEF_UEP7 7
-#define UNUM_EP 8
+#define DEF_UEP0              0
+#define DEF_UEP1              1
+#define DEF_UEP2              2
+#define DEF_UEP3              3
+#define DEF_UEP4              4
+#define DEF_UEP5              5
+#define DEF_UEP6              6
+#define DEF_UEP7              7
+#define UNUM_EP               8
 
-#define USBFS_UEP_T_RES_MASK MASK_UEP_T_RES
-#define USBFS_UEP_T_RES_ACK UEP_T_RES_ACK
-#define USBFS_UEP_T_RES_NAK UEP_T_RES_NAK
-#define USBFS_UEP_T_TOG RB_UEP_T_TOG
+#define USBFS_UEP_T_RES_MASK  MASK_UEP_T_RES
+#define USBFS_UEP_T_RES_ACK   UEP_T_RES_ACK
+#define USBFS_UEP_T_RES_NAK   UEP_T_RES_NAK
+#define USBFS_UEP_T_TOG       RB_UEP_T_TOG
 #define USBFS_UEP_T_RES_STALL UEP_T_RES_STALL
 
-#define USBFS_UEP_R_RES_MASK MASK_UEP_R_RES
-#define USBFS_UEP_R_RES_ACK UEP_R_RES_ACK
-#define USBFS_UEP_R_RES_NAK UEP_R_RES_NAK
-#define USBFS_UEP_R_TOG RB_UEP_R_TOG
+#define USBFS_UEP_R_RES_MASK  MASK_UEP_R_RES
+#define USBFS_UEP_R_RES_ACK   UEP_R_RES_ACK
+#define USBFS_UEP_R_RES_NAK   UEP_R_RES_NAK
+#define USBFS_UEP_R_TOG       RB_UEP_R_TOG
 #define USBFS_UEP_R_RES_STALL UEP_R_RES_STALL
 
-#define USBFS_UDA_GP_BIT RB_UDA_GP_BIT
-#define USBFS_UMS_SUSPEND RB_UMS_SUSPEND
+#define USBFS_UDA_GP_BIT      RB_UDA_GP_BIT
+#define USBFS_UMS_SUSPEND     RB_UMS_SUSPEND
 
-#define USBFS_UC_RESET_SIE RB_UC_RESET_SIE
-#define USBFS_UC_CLR_ALL RB_UC_CLR_ALL
-#define USBFS_UIE_SUSPEND RB_UIE_SUSPEND
-#define USBFS_UIE_TRANSFER RB_UIE_TRANSFER
-#define USBFS_UIE_BUS_RST RB_UIE_BUS_RST
-#define USBFS_UC_INT_BUSY RB_UC_INT_BUSY
-#define USBFS_UC_DMA_EN RB_UC_DMA_EN
-#define USBFS_UC_DEV_PU_EN RB_UC_DEV_PU_EN
+#define USBFS_UC_RESET_SIE    RB_UC_RESET_SIE
+#define USBFS_UC_CLR_ALL      RB_UC_CLR_ALL
+#define USBFS_UIE_SUSPEND     RB_UIE_SUSPEND
+#define USBFS_UIE_TRANSFER    RB_UIE_TRANSFER
+#define USBFS_UIE_BUS_RST     RB_UIE_BUS_RST
+#define USBFS_UC_INT_BUSY     RB_UC_INT_BUSY
+#define USBFS_UC_DMA_EN       RB_UC_DMA_EN
+#define USBFS_UC_DEV_PU_EN    RB_UC_DEV_PU_EN
 
 #endif
 
-#define USBFS_UD_PORT_EN RB_UD_PORT_EN
-#define USBFS_UD_PD_DIS RB_UD_PD_DIS
+#define USBFS_UD_PORT_EN      RB_UD_PORT_EN
+#define USBFS_UD_PD_DIS       RB_UD_PD_DIS
 
-#define USBFS_UEP1_RX_EN RB_UEP1_RX_EN
-#define USBFS_UEP2_RX_EN RB_UEP2_RX_EN
-#define USBFS_UEP3_RX_EN RB_UEP3_RX_EN
-#define USBFS_UEP4_RX_EN RB_UEP4_RX_EN
+#define USBFS_UEP1_RX_EN      RB_UEP1_RX_EN
+#define USBFS_UEP2_RX_EN      RB_UEP2_RX_EN
+#define USBFS_UEP3_RX_EN      RB_UEP3_RX_EN
+#define USBFS_UEP4_RX_EN      RB_UEP4_RX_EN
+#define USBFS_UEP5_RX_EN      RB_UEP5_RX_EN
+#define USBFS_UEP6_RX_EN      RB_UEP6_RX_EN
+#define USBFS_UEP7_RX_EN      RB_UEP7_RX_EN
 
-#define USBFS_UEP1_TX_EN RB_UEP1_TX_EN
-#define USBFS_UEP2_TX_EN RB_UEP2_TX_EN
-#define USBFS_UEP3_TX_EN RB_UEP3_TX_EN
-#define USBFS_UEP4_TX_EN RB_UEP4_TX_EN
+#define USBFS_UEP1_TX_EN      RB_UEP1_TX_EN
+#define USBFS_UEP2_TX_EN      RB_UEP2_TX_EN
+#define USBFS_UEP3_TX_EN      RB_UEP3_TX_EN
+#define USBFS_UEP4_TX_EN      RB_UEP4_TX_EN
+#define USBFS_UEP5_TX_EN      RB_UEP5_TX_EN
+#define USBFS_UEP6_TX_EN      RB_UEP6_TX_EN
+#define USBFS_UEP7_TX_EN      RB_UEP7_TX_EN
 
-#define CHECK_USBFS_UEP_AUTO_TOG RB_UEP_AUTO_TOG
+#define CHECK_USBFS_UEP_R_AUTO_TOG RB_UEP_AUTO_TOG
 #define CHECK_USBFS_UEP_T_AUTO_TOG RB_UEP_AUTO_TOG
-// #define CHECK_USBFS_UEP_R_AUTO_TOG USBOTG_UEP_R_AUTO_TOG
 
 #define USBFS           ((USBFS_TypeDef *)USBFS_BASE)
 
 #ifdef CH32X03x
-#define UEP_CTRL_LEN(n) (((volatile uint16_t*)&USBFS->UEP0_TX_LEN)[n*2])
-#define UEP_CTRL_TX(n)  (((volatile uint16_t*)&USBFS->UEP0_CTRL_H)[n*2])
-#define UEP_CTRL_RX(n)  (((volatile uint16_t*)&USBFS->UEP0_CTRL_H)[n*2])
-#define UEP_DMA(n)      (((volatile uint32_t*)&USBFS->UEP0_DMA)[n])
-#define DEBUG_PIN PB12
-#define USB_IRQn USBFS_IRQn
+#define EP_JMPR         (USBFS_BASE + 64)
+#define UEP_JMP         ((void*)(EP_JMPR) - (void*)&(USBFS->UEP0_DMA))
+#define UEP_CTRL_LEN(n) (((volatile uint8_t*)&USBFS->UEP0_TX_LEN)[n*4 +((n>4) ? UEP_JMP : 0)])
+#define UEP_CTRL_TX(n)  (((volatile uint8_t*)&USBFS->UEP0_CTRL_H)[n*4 +((n>4) ? UEP_JMP : 0)])
+#define UEP_CTRL_RX(n)  (((volatile uint8_t*)&USBFS->UEP0_CTRL_H)[n*4 +((n>4) ? UEP_JMP : 0)])
+#define UEP_DMA(n)      (((volatile uint16_t*)&USBFS->UEP0_DMA)[n*2 +((n>4) ? (UEP_JMP /2) : 0)]) // DMA for EP4 is tied to EP0 DMA on these chips, so this maceo won't work for it
+#define DEBUG_PIN       PB12
+#define USB_IRQn        USBFS_IRQn
 #endif
 
 #else
 
-#define DEBUG_PIN PB0
+#define DEBUG_PIN       PB0
 
+#if defined(CH32L103)
+#define USBFS           ((USBFSD_TypeDef *)USBFS_BASE)
+#define CHECK_USBFS_UEP_R_AUTO_TOG USBFS_UEP_T_AUTO_TOG
+#define CHECK_USBFS_UEP_T_AUTO_TOG USBFS_UEP_T_AUTO_TOG
+#else
 #define USBFS           ((USBOTG_FS_TypeDef *)USBFS_BASE)
+#define CHECK_USBFS_UEP_T_AUTO_TOG USBOTG_UEP_T_AUTO_TOG
+#define CHECK_USBFS_UEP_R_AUTO_TOG USBOTG_UEP_R_AUTO_TOG
+#endif
 
 #define UEP_CTRL_LEN(n) (((volatile uint16_t*)&USBFS->UEP0_TX_LEN)[n*2])
 #define UEP_CTRL_TX(n)  (((volatile uint8_t*)&USBFS->UEP0_TX_CTRL)[n*4])
 #define UEP_CTRL_RX(n)  (((volatile uint8_t*)&USBFS->UEP0_RX_CTRL)[n*4])
 #define UEP_DMA(n)      (((volatile uint32_t*)&USBFS->UEP0_DMA)[n])
 
-#define CHECK_USBFS_UEP_T_AUTO_TOG USBOTG_UEP_T_AUTO_TOG
-#define CHECK_USBFS_UEP_R_AUTO_TOG USBOTG_UEP_R_AUTO_TOG
+#if defined(CH32V20x)
+#define USBFS_UEP_T_RES_MASK       USBOTG_UEP_T_RES_MASK
+#define USBFS_UEP_T_RES_ACK        USBOTG_UEP_T_RES_ACK
+#define USBFS_UEP_T_RES_NAK        USBOTG_UEP_T_RES_NAK
+#define USBFS_UEP_T_TOG            USBOTG_UEP_T_TOG
+#define USBFS_UEP_T_RES_STALL      USBOTG_UEP_T_RES_STALL
 
-#define USBFS_UEP_T_RES_MASK USBOTG_UEP_T_RES_MASK
-#define USBFS_UEP_T_RES_ACK USBOTG_UEP_T_RES_ACK
-#define USBFS_UEP_T_RES_NAK USBOTG_UEP_T_RES_NAK
-#define USBFS_UEP_T_TOG USBOTG_UEP_T_TOG
-#define USBFS_UEP_T_RES_STALL USBOTG_UEP_T_RES_STALL
-
-#define USBFS_UEP_R_RES_MASK USBOTG_UEP_R_RES_MASK
-#define USBFS_UEP_R_RES_ACK USBOTG_UEP_R_RES_ACK
-#define USBFS_UEP_R_RES_NAK USBOTG_UEP_R_RES_NAK
-#define USBFS_UEP_R_TOG USBOTG_UEP_R_TOG
-#define USBFS_UEP_R_RES_STALL USBOTG_UEP_R_RES_STALL
+#define USBFS_UEP_R_RES_MASK       USBOTG_UEP_R_RES_MASK
+#define USBFS_UEP_R_RES_ACK        USBOTG_UEP_R_RES_ACK
+#define USBFS_UEP_R_RES_NAK        USBOTG_UEP_R_RES_NAK
+#define USBFS_UEP_R_TOG            USBOTG_UEP_R_TOG
+#define USBFS_UEP_R_RES_STALL      USBOTG_UEP_R_RES_STALL
 
 // #define USBFS_UDA_GP_BIT USBOTG_UDA_GP_BIT
-#define USBFS_UMS_SUSPEND USBOTG_UMS_SUSPEND
-#define USBFS_UEP1_RX_EN USBOTG_UEP1_RX_EN
-#define USBFS_UEP2_RX_EN USBOTG_UEP2_RX_EN
-#define USBFS_UEP3_RX_EN USBOTG_UEP3_RX_EN
-#define USBFS_UEP4_RX_EN USBOTG_UEP4_RX_EN
+#define USBFS_UMS_SUSPEND          USBOTG_UMS_SUSPEND
+#define USBFS_UEP1_RX_EN           USBOTG_UEP1_RX_EN
+#define USBFS_UEP2_RX_EN           USBOTG_UEP2_RX_EN
+#define USBFS_UEP3_RX_EN           USBOTG_UEP3_RX_EN
+#define USBFS_UEP4_RX_EN           USBOTG_UEP4_RX_EN
+#define USBFS_UEP5_RX_EN           USBOTG_UEP5_RX_EN
+#define USBFS_UEP6_RX_EN           USBOTG_UEP6_RX_EN
+#define USBFS_UEP7_RX_EN           USBOTG_UEP7_RX_EN
 
-#define USBFS_UEP1_TX_EN USBOTG_UEP1_TX_EN
-#define USBFS_UEP2_TX_EN USBOTG_UEP2_TX_EN
-#define USBFS_UEP3_TX_EN USBOTG_UEP3_TX_EN
-#define USBFS_UEP4_TX_EN USBOTG_UEP4_TX_EN
+#define USBFS_UEP1_TX_EN           USBOTG_UEP1_TX_EN
+#define USBFS_UEP2_TX_EN           USBOTG_UEP2_TX_EN
+#define USBFS_UEP3_TX_EN           USBOTG_UEP3_TX_EN
+#define USBFS_UEP4_TX_EN           USBOTG_UEP4_TX_EN
+#define USBFS_UEP5_TX_EN           USBOTG_UEP5_TX_EN
+#define USBFS_UEP6_TX_EN           USBOTG_UEP6_TX_EN
+#define USBFS_UEP7_TX_EN           USBOTG_UEP7_TX_EN
 
-#define USBFS_UC_RESET_SIE USBOTG_UC_RESET_SIE
-#define USBFS_UC_CLR_ALL USBOTG_UC_CLR_ALL
-#define USBFS_UIE_SUSPEND USBOTG_UIE_SUSPEND
-#define USBFS_UIE_TRANSFER USBOTG_UIE_TRANSFER
-#define USBFS_UIE_BUS_RST USBOTG_UIE_BUS_RST
-#define USBFS_UC_INT_BUSY USBOTG_UC_INT_BUSY
-#define USBFS_UC_DMA_EN USBOTG_UC_DMA_EN
-#define USBFS_UC_DEV_PU_EN USBOTG_UC_DEV_PU_EN
-#define USBFS_UD_PD_DIS USBOTG_UD_PD_DIS
-#define USBFS_UD_PORT_EN USBOTG_UD_PORT_EN
+#define USBFS_UC_RESET_SIE         USBOTG_UC_RESET_SIE
+#define USBFS_UC_CLR_ALL           USBOTG_UC_CLR_ALL
+#define USBFS_UIE_SUSPEND          USBOTG_UIE_SUSPEND
+#define USBFS_UIE_TRANSFER         USBOTG_UIE_TRANSFER
+#define USBFS_UIE_BUS_RST          USBOTG_UIE_BUS_RST
+#define USBFS_UC_INT_BUSY          USBOTG_UC_INT_BUSY
+#define USBFS_UC_DMA_EN            USBOTG_UC_DMA_EN
+#define USBFS_UC_DEV_PU_EN         USBOTG_UC_DEV_PU_EN
+#define USBFS_UD_PD_DIS            USBOTG_UD_PD_DIS
+#define USBFS_UD_PORT_EN           USBOTG_UD_PORT_EN
+#endif
 
-#ifdef CH32V30x_D8C
-#define USB_IRQn OTG_FS_IRQn
+#if defined(CH32L103)
+#define USB_IRQn                   USBFS_IRQn
+#elif defined(CH32V30x_D8C)
+#define USB_IRQn                   OTG_FS_IRQn
 #else
-#define USB_IRQn USBHD_IRQn
+#define USB_IRQn                   USBHD_IRQn
 #endif
 
 #endif
@@ -313,12 +340,12 @@ typedef struct
 #define CMASK_UIS_TOKEN   (3<<12)
 #define CMASK_UIS_ENDP    (0xf<<8)
 
-#define CUIS_TOKEN_OUT	   0x0
-#define CUIS_TOKEN_SOF     0x1
-#define CUIS_TOKEN_IN      0x2
-#define CUIS_TOKEN_SETUP   0x3
+#define CUIS_TOKEN_OUT	  0x0
+#define CUIS_TOKEN_SOF    0x1
+#define CUIS_TOKEN_IN     0x2
+#define CUIS_TOKEN_SETUP  0x3
 
-#define USBFS_PACKET_SIZE  64
+#define USBFS_PACKET_SIZE 64
 
 extern uint32_t USBDEBUG0, USBDEBUG1, USBDEBUG2;
 
@@ -326,11 +353,13 @@ struct _USBState;
 
 // Provided functions:
 int USBFSSetup();
+void USBFSReset();
 uint8_t USBFS_Endp_DataUp(uint8_t endp, const uint8_t *pbuf, uint16_t len, uint8_t mod);
-static inline uint8_t * USBFS_GetEPBufferIfAvailable( int endp );
-static inline int USBFS_SendEndpoint( int endp, int len );
-static inline int USBFS_SendACK( int endp, int tx );
-static inline int USBFS_SendNAK( int endp, int tx );
+uint8_t * USBFS_GetEPBufferIfAvailable( int endp );
+int USBFS_SendEndpoint( int endp, int len );
+int USBFS_SendACK( int endp, int tx );
+int USBFS_SendNAK( int endp, int tx );
+int USBFS_SendEndpointNEW( int endp, uint8_t* data, int len, int copy);
 
 #if FUSB_USE_DMA7_COPY
 static inline void copyBuffer( uint8_t * dest, const uint8_t * src, int len );
@@ -345,18 +374,64 @@ void HandleHidUserReportDataOut( struct _USBState * ctx, uint8_t * data, int len
 int HandleHidUserReportDataIn( struct _USBState * ctx, uint8_t * data, int len );
 void HandleHidUserReportOutComplete( struct _USBState * ctx );
 #endif
+
 #if FUSB_USER_HANDLERS
 __USBFS_FUN_ATTRIBUTE int HandleInRequest( struct _USBState * ctx, int endp, uint8_t * data, int len );
 __USBFS_FUN_ATTRIBUTE void HandleDataOut( struct _USBState * ctx, int endp, uint8_t * data, int len );
 __USBFS_FUN_ATTRIBUTE int HandleSetupCustom( struct _USBState * ctx, int setup_code);
 #endif
 
-typedef enum
+#if FUNCONF_USE_USBPRINTF
+void HandleUSBInput( int numbytes, uint8_t * data );
+extern uint8_t usb_inputbuffer[USBFS_PACKET_SIZE];
+extern int usb_inbuf_idx;
+#endif
+
+#ifndef FUSB_MAX_EP_CNT
+#define FUSB_MAX_EP_CNT 8
+#endif
+
+#ifndef FUSB_BUFFERS_NUMBER
+#define FUSB_BUFFERS_NUMBER 1
+#undef FUSB_EP1_MODE
+#define FUSB_EP1_MODE       0
+#undef FUSB_EP2_MODE
+#define FUSB_EP2_MODE       0
+#undef FUSB_EP3_MODE
+#define FUSB_EP3_MODE       0
+#undef FUSB_EP4_MODE
+#define FUSB_EP4_MODE       0
+#undef FUSB_EP5_MODE
+#define FUSB_EP5_MODE       0
+#undef FUSB_EP6_MODE
+#define FUSB_EP6_MODE       0
+#undef FUSB_EP7_MODE
+#define FUSB_EP7_MODE       0
+#warning "You may have forgotten to properly define used EPs in usb_config.h"
+#endif
+
+// high-level-ish abstraction to help dealing with endpoints
+typedef struct
 {
-	USBFS_EP_OFF = 0,
-	USBFS_EP_RX  = -1,
-	USBFS_EP_TX = 1,
-} USBFS_EP_mode;
+	union {
+		uint8_t * out;
+		uint8_t * rx;
+	};
+	union {
+		uint8_t * in;
+		uint8_t * tx;
+	};
+	uint8_t mode;
+	volatile uint8_t busy;
+  uint16_t reserved;
+} USBFS_EP_TypeDef;
+
+#define USBFS_EP_MODE_TX          4
+#define USBFS_EP_MODE_TX_DOUBLE   5
+#define USBFS_EP_MODE_RX          8
+#define USBFS_EP_MODE_RX_DOUBLE   9
+#define USBFS_EP_MODE_BDIR        12
+#define USBFS_EP_MODE_BDIR_DOUBLE 13
 
 #ifndef FUSB_EP1_MODE
 #define FUSB_EP1_MODE  0
@@ -380,7 +455,6 @@ typedef enum
 #define FUSB_EP7_MODE  0
 #endif
 
-
 struct _USBState
 {
 	// Setup Request
@@ -397,23 +471,20 @@ struct _USBState
 
 	uint8_t* pCtrlPayloadPtr;
 
-	uint8_t ENDPOINTS[FUSB_CONFIG_EPS][64];
-	USBFS_EP_mode endpoint_mode[FUSB_CONFIG_EPS+1]; // IN -1, OUT 1, OFF 0
+	uint8_t ep_buffers[FUSB_BUFFERS_NUMBER][64];
 
-	#define CTRL0BUFF               (USBFSCTX.ENDPOINTS[0])
+	USBFS_EP_TypeDef endpoints[FUSB_MAX_EP_CNT];
+
+	#define CTRL0BUFF               (USBFSCTX.ep_buffers[0])
 	#define pUSBFS_SetupReqPak      ((tusb_control_request_t*)CTRL0BUFF)
 
 #if FUSB_HID_INTERFACES > 0
 	uint8_t USBFS_HidIdle[FUSB_HID_INTERFACES];
 	uint8_t USBFS_HidProtocol[FUSB_HID_INTERFACES];
 #endif
-	volatile uint8_t USBFS_Endp_Busy[FUSB_CONFIG_EPS];
 	volatile uint8_t USBFS_errata_dont_send_endpoint_in_window;
 };
 
 extern struct _USBState USBFSCTX;
 
-#include "fsusb.c"
-
 #endif
-
diff --git a/inc/extralibs/hsusb.c b/inc/extralibs/hsusb.c
index 2087284..77d9545 100644
--- a/inc/extralibs/hsusb.c
+++ b/inc/extralibs/hsusb.c
@@ -7,7 +7,7 @@ struct _USBState USBHSCTX;
 volatile uint8_t usb_debug = 0;
 
 #ifdef CH584_CH585
-static inline void mcpy_raw( void *dst, void *start, void *end )
+static inline void mcpy_raw( void *dst, const void *start, void *end )
 {
 	__asm__ volatile ( ".insn r 0x0f, 0x7, 0, x0, %3, %0, %1"
 	      : "+r"(start), "+r"(dst)
@@ -15,9 +15,9 @@ static inline void mcpy_raw( void *dst, void *start, void *end )
 	      : "memory" );
 }
 
-void * fast_memcpy( void *dst, void *src, uint32_t size )
+void * fast_memcpy( void *dst, const void *src, uint32_t size )
 {
-	uint32_t * end = src + size;
+	uint32_t * end = (void *)src + size;
 	mcpy_raw( dst, src, (void *)end );
 
 	return dst;
@@ -37,6 +37,10 @@ void * fast_memcpy( void *dst, void *src, uint32_t size )
 #define USBHS_IRQn USB2_DEVICE_IRQn
 #endif
 
+#define ANYPRINTF	(defined( FUNCONF_USE_DEBUGPRINTF ) && FUNCONF_USE_DEBUGPRINTF) || \
+					(defined( FUNCONF_USE_UARTPRINTF ) && FUNCONF_USE_UARTPRINTF) || \
+					(defined( FUNCONF_USE_USBPRINTF ) && FUNCONF_USE_USBPRINTF)
+
 #if FUSB_USE_HPE // Will it ever work?
 // There is an issue with some registers apparently getting lost with HPE, just do it the slow way.
 #if defined(CH584_CH585)
@@ -89,7 +93,9 @@ void USBHS_IRQHandler()
 		int USBHS_SetupReqLen = USBHSCTX.USBHS_SetupReqLen    = pUSBHS_SetupReqPak->wLength;
 		int USBHS_SetupReqIndex = pUSBHS_SetupReqPak->wIndex;
 		int USBHS_IndexValue = USBHSCTX.USBHS_IndexValue = ( pUSBHS_SetupReqPak->wIndex << 16 ) | pUSBHS_SetupReqPak->wValue;
+#if ANYPRINTF
 		if( usb_debug ) printf( "[USB] SETUP: %02x %02x %d %04x %08x\n", USBHS_SetupReqType, USBHS_SetupReqCode, USBHS_SetupReqLen, USBHS_SetupReqIndex, USBHS_IndexValue );
+#endif
 
 		if( ( USBHS_SetupReqType & USB_REQ_TYP_MASK ) != USB_REQ_TYP_STANDARD )
 		{
@@ -256,6 +262,11 @@ void USBHS_IRQHandler()
 				case USB_SET_CONFIGURATION:
 					ctx->USBHS_DevConfig = (uint8_t)( ctx->USBHS_IndexValue & 0xFF );
 					ctx->USBHS_DevEnumStatus = 0x01;
+					for(int ep = 1; ep < FUSB_MAX_EP_CNT; ep++) {
+						// reset all DATAx
+						UEP_CTRL_RX(ep) &= ~USBHS_UEP_R_TOG_DATA1;
+						UEP_CTRL_TX(ep) &= ~USBHS_UEP_T_TOG_DATA1;
+					}
 					break;
 
 				/* Clear or disable one usb feature */
@@ -282,11 +293,11 @@ void USBHS_IRQHandler()
 						{
 							/* Clear End-point Feature */
 							int ep = USBHS_SetupReqIndex & 0xf;
-							if( ep < FUSB_CONFIG_EPS ) 
+							if( ctx->endpoints[ep].mode ) 
 							{
 								// UEP_CTRL_TX(ep) = USBHS_UEP_T_RES_STALL;
-								if( USBHS_SetupReqIndex & USBHS_DEF_UEP_IN  && ctx->endpoint_mode[ep] == -1 ) UEP_CTRL_TX(ep) = USBHS_UEP_T_RES_NAK;
-								else if( USBHS_SetupReqIndex & USBHS_DEF_UEP_OUT && ctx->endpoint_mode[ep] == 1 ) UEP_CTRL_RX(ep) = USBHS_UEP_R_RES_ACK;
+								if( USBHS_SetupReqIndex & USBHS_DEF_UEP_IN  && (ctx->endpoints[ep].mode & USBHS_EP_MODE_TX) ) UEP_CTRL_TX(ep) = USBHS_UEP_T_RES_NAK;
+								else if( USBHS_SetupReqIndex & USBHS_DEF_UEP_OUT && (ctx->endpoints[ep].mode & USBHS_EP_MODE_RX) ) UEP_CTRL_RX(ep) = USBHS_UEP_R_RES_ACK;
 								else 
 								{
 									goto sendstall;
@@ -331,10 +342,10 @@ void USBHS_IRQHandler()
 						if( (uint8_t)( USBHS_IndexValue & 0xFF ) == USB_REQ_FEAT_ENDP_HALT )
 						{
 							int ep = USBHS_SetupReqIndex & 0xf;
-							if( ep < FUSB_CONFIG_EPS )
+							if( ctx->endpoints[ep].mode )
 							{
-								if( (USBHS_SetupReqIndex & USBHS_DEF_UEP_IN) && ctx->endpoint_mode[ep] == -1 ) UEP_CTRL_TX(ep) = ( UEP_CTRL_TX(ep) & ~USBHS_UEP_T_RES_MASK ) | USBHS_UEP_T_RES_STALL;
-								else if( (USBHS_SetupReqIndex & USBHS_DEF_UEP_OUT) && ctx->endpoint_mode[ep] == 1 ) UEP_CTRL_RX(ep) = ( UEP_CTRL_RX(ep) & ~USBHS_UEP_R_RES_MASK ) | USBHS_UEP_R_RES_STALL;
+								if( (USBHS_SetupReqIndex & USBHS_DEF_UEP_IN) && (ctx->endpoints[ep].mode & USBHS_EP_MODE_TX) ) UEP_CTRL_TX(ep) = ( UEP_CTRL_TX(ep) & ~USBHS_UEP_T_RES_MASK ) | USBHS_UEP_T_RES_STALL;
+								else if( (USBHS_SetupReqIndex & USBHS_DEF_UEP_OUT) && (ctx->endpoints[ep].mode & USBHS_EP_MODE_RX) ) UEP_CTRL_RX(ep) = ( UEP_CTRL_RX(ep) & ~USBHS_UEP_R_RES_MASK ) | USBHS_UEP_R_RES_STALL;
 								else 
 								{
 									goto sendstall;
@@ -376,10 +387,10 @@ void USBHS_IRQHandler()
 					else if( ( USBHS_SetupReqType & USB_REQ_RECIP_MASK ) == USB_REQ_RECIP_ENDP )
 					{
 						int ep = USBHS_SetupReqIndex & 0xf;
-						if( ep < FUSB_CONFIG_EPS )
+						if( ctx->endpoints[ep].mode )
 						{
-							if( USBHS_SetupReqIndex & USBHS_DEF_UEP_IN && ctx->endpoint_mode[ep] == -1 ) ctrl0buff[0] = ( UEP_CTRL_TX(ep) & USBHS_UEP_T_RES_MASK ) == USBHS_UEP_T_RES_STALL;
-							else if( USBHS_SetupReqIndex & USBHS_DEF_UEP_OUT && ctx->endpoint_mode[ep] == 1 ) ctrl0buff[0] = ( UEP_CTRL_RX(ep) & USBHS_UEP_R_RES_MASK ) == USBHS_UEP_R_RES_STALL;
+							if( USBHS_SetupReqIndex & USBHS_DEF_UEP_IN && (ctx->endpoints[ep].mode & USBHS_EP_MODE_TX) ) ctrl0buff[0] = ( UEP_CTRL_TX(ep) & USBHS_UEP_T_RES_MASK ) == USBHS_UEP_T_RES_STALL;
+							else if( USBHS_SetupReqIndex & USBHS_DEF_UEP_OUT && (ctx->endpoints[ep].mode & USBHS_EP_MODE_RX) ) ctrl0buff[0] = ( UEP_CTRL_RX(ep) & USBHS_UEP_R_RES_MASK ) == USBHS_UEP_R_RES_STALL;
 							else goto sendstall;
 						}
 						else
@@ -443,16 +454,18 @@ replycomplete:
 		int token = ( intfgst & CMASK_UIS_TOKEN ) >> 12;
 		int ep = ( intfgst & CMASK_UIS_ENDP ) >> 8;
 		
+#if ANYPRINTF
 		if( usb_debug ) printf( "[USB] TRANSFER, token = %02x, ep = %d, bmRequestType = %02x, bRequest = %02x\n", token, ep, pUSBHS_SetupReqPak->bmRequestType, pUSBHS_SetupReqPak->bRequest );
+#endif
 		switch ( token )
 		{
 		case CUIS_TOKEN_IN:
 			if( ep )
 			{
-				if( ep < FUSB_CONFIG_EPS )
+				if( ctx->endpoints[ep].mode )
 				{
 #if FUSB_USER_HANDLERS
-					len = HandleInRequest( ctx, ep, ctx->ENDPOINTS[ ep-1 ], 0 );
+					len = HandleInRequest( ctx, ep, ctx->endpoints[ep].in, 0 );
 #endif
 					UEP_CTRL_TX(ep) ^= USBHS_UEP_T_TOG_DATA1;
 					if( len )
@@ -465,7 +478,7 @@ replycomplete:
 					{
 						UEP_CTRL_TX(ep) = ( UEP_CTRL_TX(ep) & ~USBHS_UEP_T_RES_MASK ) | USBHS_UEP_T_RES_NAK;
 					}
-					ctx->USBHS_Endp_Busy[ep] = 0;
+					ctx->endpoints[ep].busy = 0;
 				}
 			}
 			else // EP0
@@ -629,7 +642,7 @@ replycomplete:
 #endif
 						UEP_CTRL_RX(ep) = rx_ctrl;
 #if FUSB_USER_HANDLERS
-						HandleDataOut( ctx, ep, ctx->ENDPOINTS[ep-1], len );
+						HandleDataOut( ctx, ep, ctx->endpoints[ep].out, len );
 #endif
 					}
 					USBHS_DONE_RX(ep);
@@ -668,7 +681,9 @@ replycomplete:
 	}
 	else if( intfgst & CRB_UIF_BUS_RST )
 	{
+#if ANYPRINTF
 		if( usb_debug ) printf( "[USB] RESET\n" );
+#endif
 		/* usb reset interrupt processing */
 		ctx->USBHS_DevConfig = 0;
 		ctx->USBHS_DevAddr = 0;
@@ -681,7 +696,9 @@ replycomplete:
 	}
 	else if( intfgst & CRB_UIF_SUSPEND )
 	{
+#if ANYPRINTF
 		if( usb_debug ) printf( "[USB] SUSPEND\n" );
+#endif
 		USBHS->INT_FG = CRB_UIF_SUSPEND;
 		Delay_Us(10);
 		/* usb suspend interrupt processing */
@@ -717,142 +734,168 @@ replycomplete:
 
 void USBHS_InternalFinishSetup()
 {
+#if USBHS_IMPL == 1
+	USBHS->UEP_CONFIG = 0;
+#else
+	USBHS->UEP_TX_EN = 0;
+	USBHS->UEP_RX_EN = 0;
+#endif
 
 #if FUSB_EP1_MODE
-	USBHSCTX.endpoint_mode[1] = FUSB_EP1_MODE;
+#if !(defined FUSB_EP1_SIZE && !FUSB_EP1_SIZE)
+	USBHSCTX.endpoints[1].buffer = USBHSCTX.ep1_buffer;
+#endif
+	USBHSCTX.endpoints[1].mode = FUSB_EP1_MODE;
 #ifndef FUSB_EP1_SIZE
 	USBHS->UEP1_MAX_LEN = FUSB_EP_SIZE;
+	USBHSCTX.endpoints[1].size = FUSB_EP_SIZE;
 #else
 	USBHS->UEP1_MAX_LEN = FUSB_EP1_SIZE;
-#endif
-#if FUSB_EP1_MODE > 0 
-	UEP_TX_EN(1);
-	UEP_DMA_TX(1) = (uintptr_t)USBHSCTX.ENDPOINTS[0];
-#else
-	UEP_RX_EN(1);
-	UEP_DMA_RX(1) = (uintptr_t)USBHSCTX.ENDPOINTS[0];
+	USBHSCTX.endpoints[1].size = FUSB_EP1_SIZE;
 #endif
 #endif
 
 #if FUSB_EP2_MODE
-	USBHSCTX.endpoint_mode[2] = FUSB_EP2_MODE;
+#if !(defined FUSB_EP2_SIZE && !FUSB_EP2_SIZE)
+	USBHSCTX.endpoints[2].buffer = USBHSCTX.ep2_buffer;
+#endif
+	USBHSCTX.endpoints[2].mode = FUSB_EP2_MODE;
 #ifndef FUSB_EP2_SIZE
 	USBHS->UEP2_MAX_LEN = FUSB_EP_SIZE;
+	USBHSCTX.endpoints[2].size = FUSB_EP_SIZE;
 #else
 	USBHS->UEP2_MAX_LEN = FUSB_EP2_SIZE;
-#endif
-#if FUSB_EP2_MODE > 0 
-	UEP_TX_EN(2);
-	UEP_DMA_TX(2) = (uintptr_t)USBHSCTX.ENDPOINTS[1];
-#else
-	UEP_RX_EN(2);
-	UEP_DMA_RX(2) = (uintptr_t)USBHSCTX.ENDPOINTS[1];
+	USBHSCTX.endpoints[2].size = FUSB_EP2_SIZE;
 #endif
 #endif
 
 #if FUSB_EP3_MODE
-	USBHSCTX.endpoint_mode[3] = FUSB_EP3_MODE;
+#if !(defined FUSB_EP3_SIZE && !FUSB_EP3_SIZE)
+	USBHSCTX.endpoints[3].buffer = USBHSCTX.ep3_buffer;
+#endif
+	USBHSCTX.endpoints[3].mode = FUSB_EP3_MODE;
 #ifndef FUSB_EP3_SIZE
 	USBHS->UEP3_MAX_LEN = FUSB_EP_SIZE;
+	USBHSCTX.endpoints[3].size = FUSB_EP_SIZE;
 #else
 	USBHS->UEP3_MAX_LEN = FUSB_EP3_SIZE;
-#endif
-#if FUSB_EP3_MODE > 0 
-	UEP_TX_EN(3);
-	UEP_DMA_TX(3) = (uintptr_t)USBHSCTX.ENDPOINTS[2];
-#else
-	UEP_RX_EN(3);
-	UEP_DMA_RX(3) = (uintptr_t)USBHSCTX.ENDPOINTS[2];
+	USBHSCTX.endpoints[3].size = FUSB_EP3_SIZE;
 #endif
 #endif
 
 #if FUSB_EP4_MODE
-	USBHSCTX.endpoint_mode[4] = FUSB_EP4_MODE;
+#if !(defined FUSB_EP4_SIZE && !FUSB_EP4_SIZE)
+	USBHSCTX.endpoints[4].buffer = USBHSCTX.ep4_buffer;
+#endif
+	USBHSCTX.endpoints[4].mode = FUSB_EP4_MODE;
 #ifndef FUSB_EP4_SIZE
 	USBHS->UEP4_MAX_LEN = FUSB_EP_SIZE;
+	USBHSCTX.endpoints[4].size = FUSB_EP_SIZE;
 #else
 	USBHS->UEP4_MAX_LEN = FUSB_EP4_SIZE;
-#endif
-#if FUSB_EP4_MODE > 0 
-	UEP_TX_EN(4);
-	UEP_DMA_TX(4) = (uintptr_t)USBHSCTX.ENDPOINTS[3];
-#else
-	UEP_RX_EN(4);
-	UEP_DMA_RX(4) = (uintptr_t)USBHSCTX.ENDPOINTS[3];
+	USBHSCTX.endpoints[4].size = FUSB_EP4_SIZE;
 #endif
 #endif
 
 #if FUSB_EP5_MODE
-	USBHSCTX.endpoint_mode[5] = FUSB_EP5_MODE;
+#if !(defined FUSB_EP5_SIZE && !FUSB_EP5_SIZE)
+	USBHSCTX.endpoints[5].buffer = USBHSCTX.ep5_buffer;
+#endif
+	USBHSCTX.endpoints[5].mode = FUSB_EP5_MODE;
 #ifndef FUSB_EP5_SIZE
 	USBHS->UEP5_MAX_LEN = FUSB_EP_SIZE;
+	USBHSCTX.endpoints[5].size = FUSB_EP_SIZE;
 #else
 	USBHS->UEP5_MAX_LEN = FUSB_EP5_SIZE;
-#endif
-#if FUSB_EP5_MODE > 0 
-	UEP_TX_EN(5);
-	UEP_DMA_TX(5) = (uintptr_t)USBHSCTX.ENDPOINTS[4];
-#else
-	UEP_RX_EN(5);
-	UEP_DMA_RX(5) = (uintptr_t)USBHSCTX.ENDPOINTS[4];
+	USBHSCTX.endpoints[5].size = FUSB_EP5_SIZE;
 #endif
 #endif
 
 #if FUSB_EP6_MODE
-	USBHSCTX.endpoint_mode[6] = FUSB_EP6_MODE;
+#if !(defined FUSB_EP6_SIZE && !FUSB_EP6_SIZE)
+	USBHSCTX.endpoints[6].buffer = USBHSCTX.ep6_buffer;
+#endif
+	USBHSCTX.endpoints[6].mode = FUSB_EP6_MODE;
 #ifndef FUSB_EP6_SIZE
 	USBHS->UEP6_MAX_LEN = FUSB_EP_SIZE;
+	USBHSCTX.endpoints[6].size = FUSB_EP_SIZE;
 #else
 	USBHS->UEP6_MAX_LEN = FUSB_EP6_SIZE;
-#endif
-#if FUSB_EP6_MODE > 0 
-	UEP_TX_EN(6);
-	UEP_DMA_TX(6) = (uintptr_t)USBHSCTX.ENDPOINTS[5];
-#else
-	UEP_RX_EN(6);
-	UEP_DMA_RX(6) = (uintptr_t)USBHSCTX.ENDPOINTS[5];
+	USBHSCTX.endpoints[6].size = FUSB_EP6_SIZE;
 #endif
 #endif
 
 #if FUSB_EP7_MODE
-	USBHSCTX.endpoint_mode[7] = FUSB_EP7_MODE;
+#if !(defined FUSB_EP7_SIZE && !FUSB_EP7_SIZE)
+	USBHSCTX.endpoints[7].buffer = USBHSCTX.ep7_buffer;
+#endif
+	USBHSCTX.endpoints[7].mode = FUSB_EP7_MODE;
 #ifndef FUSB_EP7_SIZE
 	USBHS->UEP7_MAX_LEN = FUSB_EP_SIZE;
+	USBHSCTX.endpoints[7].size = FUSB_EP_SIZE;
 #else
 	USBHS->UEP7_MAX_LEN = FUSB_EP7_SIZE;
-#endif
-#if FUSB_EP7_MODE > 0 
-	UEP_TX_EN(7);
-	UEP_DMA_TX(7) = (uintptr_t)USBHSCTX.ENDPOINTS[6];
-#else
-	UEP_RX_EN(7);
-	UEP_DMA_RX(7) = (uintptr_t)USBHSCTX.ENDPOINTS[6];
+	USBHSCTX.endpoints[7].size = FUSB_EP7_SIZE;
 #endif
 #endif
 
-#if !defined (FUSB_CONFIG_EPS) || !FUSB_CONFIG_EPS
-#error You must have at least EP0!
-#endif
-	
 	USBHS->UEP0_DMA = (uintptr_t)USBHSCTX.CTRL0BUFF;
 	USBHS->UEP0_MAX_LEN = USBHS_DEF_UEP0_SIZE;
-	USBHS->UEP0_T_LEN = 0;
+	UEP_CTRL_LEN(0) = 0;
 	UEP_TX_EN(0);
 	UEP_RX_EN(0);
 	UEP_CTRL_TX(0) = USBHS_UEP_T_RES_NAK;
 	UEP_CTRL_RX(0) = USBHS_UEP_R_RES_ACK;
+	USBHSCTX.endpoints[0].buffer = USBHSCTX.CTRL0BUFF;
+	USBHSCTX.endpoints[0].mode = USBHS_EP_MODE_TX | USBHS_EP_MODE_RX;
 
-	for( int i = 1; i < FUSB_CONFIG_EPS; i++ )
+	for( int i = 1; i < FUSB_MAX_EP_CNT; i++ )
 	{
-		if( USBHSCTX.endpoint_mode[i] > 0 )
+		USBHS_EP_TypeDef * endpoint = &USBHSCTX.endpoints[i];
+		UEP_CTRL_LEN(i) = 0;
+		if( endpoint->mode & USBHS_EP_MODE_TX )
 		{
 			UEP_CTRL_TX(i) = USBHS_UEP_T_RES_NAK;
+			UEP_TX_EN(i);
+			UEP_DMA_TX(i) = (uintptr_t)endpoint->buffer;
 		}
-		else if( USBHSCTX.endpoint_mode[i] < 0 )
+		else if( USBHSCTX.endpoints[i].mode & USBHS_EP_MODE_RX )
 		{
 			UEP_CTRL_RX(i) = USBHS_UEP_R_RES_ACK;
+			UEP_RX_EN(i);
+			UEP_DMA_RX(i) = (uintptr_t)endpoint->buffer;
+
 		}
-		USBHSCTX.USBHS_Endp_Busy[i] = 0;
+
+#if USBHS_IMPL == 1
+		if( endpoint->mode & USBHS_EP_MODE_DOUBLE )
+		{
+			UEP_MODE_DOUBLE(i);
+			if( endpoint->mode & USBHS_EP_MODE_TX )
+			{
+				UEP_DMA_RX(i) = ((uintptr_t)endpoint->buffer) + endpoint->size;
+			}
+			else
+			{
+				UEP_DMA_TX(i) = ((uintptr_t)endpoint->buffer) + endpoint->size;
+			}
+		}
+		else if( USBHSCTX.endpoints[i].mode & USBHS_EP_MODE_ISO )
+		{
+			UEP_MODE_ISO(i);
+		}
+#else
+		if( USBHSCTX.endpoints[i].mode & USBHS_EP_MODE_TX_BURST )
+		{
+			UEP_MODE_TX_BURST(i);
+		}
+		else if( USBHSCTX.endpoints[i].mode & USBHS_EP_MODE_RX_BURST )
+		{
+			UEP_MODE_RX_BURST(i);
+		}
+#endif
+
+		USBHSCTX.endpoints[i].busy = 0;
 	}
 }
 
@@ -922,15 +965,30 @@ int USBHSSetup()
 	return 0;
 }
 
+void USBHSReset()
+{
+	NVIC_DisableIRQ( USBHS_IRQn );
+#if (USBHS_IMPL == 1) // CH32V30x CH565_CH569
+	USBHS->BASE_CTRL = USBHS_UC_CLR_ALL | USBHS_UC_RESET_SIE;
+	Delay_Us(10);
+	USBHS->BASE_CTRL = 0;
+	USBHS->HOST_CTRL = USBHS_UH_PHY_SUSPENDM;
+#else // CH584_CH585
+	USBHS->BASE_CTRL = USBHS_UD_RST_LINK | USBHS_UD_PHY_SUSPENDM;
+	Delay_Us(10);
+	USBHS->BASE_CTRL = 0;
+#endif
+}
+
 static inline uint8_t * USBHS_GetEPBufferIfAvailable( int endp )
 {
-	if( USBHSCTX.USBHS_Endp_Busy[endp] ) return 0;
-	return USBHSCTX.ENDPOINTS[endp-1];
+	if( USBHSCTX.endpoints[endp].busy ) return 0;
+	return USBHSCTX.endpoints[endp].in;
 }
 
 static inline int USBHS_SendEndpoint( int endp, int len )
 {
-	if( USBHSCTX.USBHS_errata_dont_send_endpoint_in_window || USBHSCTX.USBHS_Endp_Busy[endp] ) return -1;
+	if( USBHSCTX.USBHS_errata_dont_send_endpoint_in_window || USBHSCTX.endpoints[endp].busy ) return -1;
 	// This prevents sending while ep0 is receiving
 	if( USBHSCTX.USBHS_SetupReqLen > 0 ) return -2;
 	// Check if in SETUP request
@@ -938,14 +996,15 @@ static inline int USBHS_SendEndpoint( int endp, int len )
 	NVIC_DisableIRQ( USBHS_IRQn );
 	UEP_CTRL_LEN( endp ) = len;
 	UEP_CTRL_TX( endp ) = ( UEP_CTRL_TX( endp ) & ~USBHS_UEP_T_RES_MASK ) | USBHS_UEP_T_RES_ACK;
-	USBHSCTX.USBHS_Endp_Busy[endp] = 1;
+	USBHSCTX.endpoints[endp].busy = 1;
 	NVIC_EnableIRQ( USBHS_IRQn );
 	return 0;
 }
 
-static inline int USBHS_SendEndpointNEW( int endp, const uint8_t* data, int len, int copy)
+// If youre sending without copy, make sure data buffer is 4 bytes aligned
+static inline int USBHS_SendEndpointNEW( int endp, const uint8_t* data, int len, int copy )
 {
-	if( USBHSCTX.USBHS_errata_dont_send_endpoint_in_window || USBHSCTX.USBHS_Endp_Busy[endp] ) return -1;
+	if( USBHSCTX.USBHS_errata_dont_send_endpoint_in_window || USBHSCTX.endpoints[endp].busy ) return -1;
 	// This prevents sending while ep0 is receiving
 	if( USBHSCTX.USBHS_SetupReqLen > 0 ) return USBHSCTX.USBHS_SetupReqLen;
 	
@@ -953,7 +1012,10 @@ static inline int USBHS_SendEndpointNEW( int endp, const uint8_t* data, int len,
 	{
 		if( copy )
 		{
-			uint8_t* dest = (endp?USBHSCTX.ENDPOINTS[endp-1]:USBHSCTX.CTRL0BUFF);
+			if( !endp ) USBHS->UEP0_DMA = (uintptr_t)USBHSCTX.CTRL0BUFF;
+			else UEP_DMA_TX( endp ) = (uintptr_t)USBHSCTX.endpoints[endp].in;
+
+			uint8_t* dest = (endp?USBHSCTX.endpoints[endp].in:USBHSCTX.CTRL0BUFF);
 			copyBuffer( dest, data, len );
 		}
 		else
@@ -965,7 +1027,7 @@ static inline int USBHS_SendEndpointNEW( int endp, const uint8_t* data, int len,
 	// NVIC_DisableIRQ( USBHS_IRQn );
 	UEP_CTRL_LEN( endp ) = len;
 	UEP_CTRL_TX( endp ) = ( UEP_CTRL_TX( endp ) & ~USBHS_UEP_T_RES_MASK ) | USBHS_UEP_T_RES_ACK;
-	USBHSCTX.USBHS_Endp_Busy[endp] = 1;
+	USBHSCTX.endpoints[endp].busy = 1;
 	// NVIC_EnableIRQ( USBHS_IRQn );
 	return 0;
 }
@@ -1019,4 +1081,3 @@ WEAK int HandleSetupCustom( struct _USBState *ctx, int setup_code )
 	return ret;
 }
 #endif // FUNCONF_USE_USBPRINTF
-
diff --git a/inc/extralibs/hsusb.h b/inc/extralibs/hsusb.h
index 6c533be..28f095d 100644
--- a/inc/extralibs/hsusb.h
+++ b/inc/extralibs/hsusb.h
@@ -187,8 +187,10 @@ typedef struct
 #define CUIS_TOKEN_IN      0x2
 #define CUIS_TOKEN_SETUP   0x3
 
-#define UEP_TX_EN(n)    USBHS->UEP_CONFIG |= ((uint32_t)(1<<n))
-#define UEP_RX_EN(n)    USBHS->UEP_CONFIG |= ((uint32_t)(1<<(n+16)))
+#define UEP_TX_EN(n)       USBHS->UEP_CONFIG |= ((uint32_t)(1<<n))
+#define UEP_RX_EN(n)       USBHS->UEP_CONFIG |= ((uint32_t)(1<<(n+16)))
+#define UEP_MODE_DOUBLE(n) USBHS->UEP_BUF_MODE |= ((uint32_t)(1<<(n)))
+#define UEP_MODE_ISO(n)    USBHS->UEP_BUF_MODE |= ((uint32_t)(1<<(n+16)))
 
 #define USBHS_DONE_TX(n)
 #define USBHS_DONE_RX(n)
@@ -286,9 +288,11 @@ typedef struct
 
 #define USBHS_CHECK_NAK_RX (USBHS->UEP0_RX_CTRL & (1<<6))
 
-#define UEP_RX_LEN(n)   (((volatile uint16_t*)&USBHS->UEP1_RX_LEN)[(n-1)*2])
-#define UEP_TX_EN(n)    USBHS->UEP_TX_EN |= ((uint16_t)(1<<n))
-#define UEP_RX_EN(n)    USBHS->UEP_RX_EN |= ((uint16_t)(1<<n))
+#define UEP_RX_LEN(n)        (((volatile uint16_t*)&USBHS->UEP1_RX_LEN)[(n-1)*2])
+#define UEP_TX_EN(n)         USBHS->UEP_TX_EN |= ((uint16_t)(1<<n))
+#define UEP_RX_EN(n)         USBHS->UEP_RX_EN |= ((uint16_t)(1<<n))
+#define UEP_MODE_TX_BURST(n) USBHS->UEP_T_BURST |= ((uint16_t)(1<<n))
+#define UEP_MODE_RX_BURST(n) USBHS->UEP_R_BURST |= ((uint16_t)(1<<n))
 
 #define DEBUG_PIN PB17
 
@@ -399,6 +403,10 @@ __HIGH_CODE int HandleSetupCustom( struct _USBState * ctx, int setup_code);
 void USBHS_RxReady(int endp);
 #endif
 
+#ifndef FUSB_MAX_EP_CNT
+#define FUSB_MAX_EP_CNT 8
+#endif
+
 typedef enum
 {
 	USB_SPEED_FULL = 0,
@@ -413,6 +421,40 @@ typedef enum
 	USBHS_EP_TX = 1,
 } USBHS_EP_mode;
 
+// high-level-ish abstraction to help dealing with endpoints
+typedef struct
+{
+	union {
+		uint8_t * out;
+		uint8_t * rx;
+		uint8_t * in;
+		uint8_t * tx;
+		uint8_t * buffer;
+	};
+	uint16_t size;
+	uint8_t mode;
+	volatile uint8_t busy;
+} USBHS_EP_TypeDef;
+
+#define USBHS_EP_MODE_TX          4
+#define USBHS_EP_MODE_RX          8
+
+#if USBHS_IMPL == 1
+#define USBHS_EP_MODE_TX_DOUBLE   5
+#define USBHS_EP_MODE_TX_ISO      6
+#define USBHS_EP_MODE_RX_DOUBLE   9
+#define USBHS_EP_MODE_RX_ISO      10
+#define USBHS_EP_MODE_DOUBLE      1
+#define USBHS_EP_MODE_ISO         2
+#endif
+
+#if USBHS_IMPL == 2
+// https://www.wch.cn/bbs/thread-150494-1.html
+#define USBHS_EP_MODE_BURST       1
+#define USBHS_EP_MODE_TX_BURST    5
+#define USBHS_EP_MODE_RX_BURST    9
+#endif
+
 #ifndef FUSB_EP1_MODE
 #define FUSB_EP1_MODE  0
 #endif
@@ -438,7 +480,120 @@ typedef enum
 struct _USBState
 {
 	__attribute__ ((aligned(4))) uint8_t CTRL0BUFF[64];
-	__attribute__ ((aligned(4))) uint8_t ENDPOINTS[FUSB_CONFIG_EPS-1][FUSB_EP_SIZE];
+
+#if FUSB_EP1_MODE
+#if FUSB_EP1_SIZE
+#if FUSB_EP1_MODE & 1
+	__attribute__ ((aligned(4))) uint8_t ep1_buffer[FUSB_EP1_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep1_buffer[FUSB_EP1_SIZE];
+#endif
+#elif !(defined(FUSB_EP1_SIZE) && !FUSB_EP1_SIZE)
+#if (FUSB_EP1_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep1_buffer[FUSB_EP_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep1_buffer[FUSB_EP_SIZE];
+#endif
+#endif
+#endif
+
+#if FUSB_EP2_MODE
+#if FUSB_EP2_SIZE
+#if (FUSB_EP2_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep2_buffer[FUSB_EP2_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep2_buffer[FUSB_EP2_SIZE];
+#endif
+#elif !(defined(FUSB_EP2_SIZE) && !FUSB_EP2_SIZE)
+#if (FUSB_EP2_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep2_buffer[FUSB_EP_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep2_buffer[FUSB_EP_SIZE];
+#endif
+#endif
+#endif
+
+#if FUSB_EP3_MODE
+#if FUSB_EP3_SIZE
+#if (FUSB_EP3_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep3_buffer[FUSB_EP3_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep3_buffer[FUSB_EP3_SIZE];
+#endif
+#elif !(defined(FUSB_EP3_SIZE) && !FUSB_EP3_SIZE)
+#if (FUSB_EP3_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep3_buffer[FUSB_EP_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep3_buffer[FUSB_EP_SIZE];
+#endif
+#endif
+#endif
+
+#if FUSB_EP4_MODE
+#if FUSB_EP4_SIZE
+#if (FUSB_EP4_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep4_buffer[FUSB_EP4_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep4_buffer[FUSB_EP4_SIZE];
+#endif
+#elif !(defined(FUSB_EP4_SIZE) && !FUSB_EP4_SIZE)
+#if (FUSB_EP4_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep4_buffer[FUSB_EP_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep4_buffer[FUSB_EP_SIZE];
+#endif
+#endif
+#endif
+
+#if FUSB_EP5_MODE
+#if FUSB_EP5_SIZE
+#if (FUSB_EP5_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep5_buffer[FUSB_EP5_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep5_buffer[FUSB_EP5_SIZE];
+#endif
+#elif !(defined(FUSB_EP5_SIZE) && !FUSB_EP5_SIZE)
+#if (FUSB_EP5_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep5_buffer[FUSB_EP_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep5_buffer[FUSB_EP_SIZE];
+#endif
+#endif
+#endif
+
+#if FUSB_EP6_MODE
+#if FUSB_EP6_SIZE
+#if (FUSB_EP6_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep6_buffer[FUSB_EP6_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep6_buffer[FUSB_EP6_SIZE];
+#endif
+#elif !(defined(FUSB_EP6_SIZE) && !FUSB_EP6_SIZE)
+#if (FUSB_EP6_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep6_buffer[FUSB_EP_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep6_buffer[FUSB_EP_SIZE];
+#endif
+#endif
+#endif
+
+#if FUSB_EP7_MODE
+#if FUSB_EP7_SIZE
+#if (FUSB_EP7_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep7_buffer[FUSB_EP7_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep7_buffer[FUSB_EP7_SIZE];
+#endif
+#elif !(defined(FUSB_EP7_SIZE) && !FUSB_EP7_SIZE)
+#if (FUSB_EP7_MODE & 1)
+	__attribute__ ((aligned(4))) uint8_t ep7_buffer[FUSB_EP_SIZE*2];
+#else
+	__attribute__ ((aligned(4))) uint8_t ep7_buffer[FUSB_EP_SIZE];
+#endif
+#endif
+#endif
+
+	USBHS_EP_TypeDef endpoints[FUSB_MAX_EP_CNT];
 	// Setup Request
 	uint8_t  USBHS_SetupReqCode;
 	uint8_t  USBHS_SetupReqType;
@@ -452,17 +607,13 @@ struct _USBState
 	uint8_t  USBHS_DevEnumStatus;
 
 	uint8_t* pCtrlPayloadPtr;
-
 	
-	USBHS_EP_mode endpoint_mode[FUSB_CONFIG_EPS+1]; // IN -1, OUT 1, OFF 0
-
 	#define pUSBHS_SetupReqPak      ((tusb_control_request_t*)USBHSCTX.CTRL0BUFF)
 
 #if FUSB_HID_INTERFACES > 0
 	uint8_t USBHS_HidIdle[FUSB_HID_INTERFACES];
 	uint8_t USBHS_HidProtocol[FUSB_HID_INTERFACES];
 #endif
-	volatile uint8_t USBHS_Endp_Busy[FUSB_CONFIG_EPS];
 	volatile uint8_t USBHS_errata_dont_send_endpoint_in_window;
 	volatile uint64_t USBHS_sof_timestamp;
 };
@@ -472,4 +623,3 @@ extern struct _USBState USBHSCTX;
 #include "hsusb.c"
 
 #endif
-
diff --git a/inc/extralibs/iSLER.h b/inc/extralibs/iSLER.h
index ddee7cc..1d742d0 100644
--- a/inc/extralibs/iSLER.h
+++ b/inc/extralibs/iSLER.h
@@ -1,25 +1,147 @@
+/* 
+	iSLER header for the CH570, 571, 572, 573, 582, 583, 584, 585, 591, 592
+
+	For basic RF functionality on supported WCH chips.
+
+		#define ACCESS_ADDRESS 0x8E89BED6 // the "BED6" address for BLE advertisements
+
+		// Initialize the iSLER engine, with the TX power.
+		iSLERInit(LL_TX_POWER_0_DBM);
+
+		// TX packets
+		iSLERTX(ACCESS_ADDRESS, txbuf, sizeof(txbuf), adv_channels[c], PHY_1M);
+
+	To receive packets, you can either:
+
+		#define ISLER_CALLBACK_RX iSLERRXCallback
+		#define ISLER_CALLBACK_TX iSLERTXCallback
+
+		void iSLERRXCallback()
+		{
+			if( !iSLERCRCOK() ) return;
+
+			uint8_t *frame = (uint8_t*)LLE_BUF;
+			int rssi = iSLERRSSI();
+
+			// Do stuff
+		}
+
+		void iSLERTXCallback()
+		{
+			// TX is done, continue using the radio
+		}
+
+	Or, just poll on rx_ready, whenever it's set you can read the packet.
+
+		iSLERRX(ACCESS_ADDRESS, 37, PHY_1M);
+		while(!rx_ready);
+		// Receive packet code.
+
+*/
+
+// 2025-12-25 function rename.
+#define RFCoreInit iSLERInit
+#define Frame_TX   iSLERTX
+#define Frame_RX   iSLERRX
+#define ReadRSSI   iSLERRSSI
+
+#ifdef ISLER_CALLBACK
+#warning "ISLER_CALLBACK is deprecated, use ISLER_CALLBACK_RX"
+#define ISLER_CALLBACK_RX ISLER_CALLBACK
+#endif
+
 #include <stdint.h>
 #include <stddef.h>
 #include <stdio.h>
 #include <string.h>
 
+// define the linker section where the "highcode" functions live
+// on ch5xx this is a section of RAM for quicker execution,
+// but for example in funcongif.h
+//  #define FUNCONF_ISLER_FUNCTION_SECTION ".manual_ram"
+// you can put it where you want
+#if defined(FUNCONF_ISLER_FUNCTION_SECTION)
+#ifdef __HIGH_CODE
+#undef __HIGH_CODE
+#endif
+#define __HIGH_CODE __attribute__( ( section( FUNCONF_ISLER_FUNCTION_SECTION ) ) )
+#elif defined(CH571_CH573)
+// HIGH_CODE doesn't work for CH571/3
+#ifdef __HIGH_CODE
+#undef __HIGH_CODE
+#endif
+#define __HIGH_CODE
+#elif !defined(__HIGH_CODE)
+#define __HIGH_CODE
+#endif
+
+// Define the linker attributes for buffers
+// These need to be 4-byte aligned
+// For some chips, notably the CH571/3,
+// these also need to be in a DMA safe section of RAM
+// Note: the stack is *usually* in the DMA safe region (above 0x20004000)
+
+#ifdef FUNCONF_ISLER_BUFFER_SECTION
+#define ISLER_BUFFER_SECTION FUNCONF_ISLER_BUFFER_SECTION
+#elif defined(CH571_CH573)
+#define ISLER_BUFFER_SECTION ".dma_safe"
+#endif
+
+#if defined(ISLER_BUFFER_SECTION)
+#define ISLER_BUF_ATTR __attribute__((aligned(4))) __attribute__((section(ISLER_BUFFER_SECTION)))
+#else
+#define ISLER_BUF_ATTR __attribute__((aligned(4)))
+#endif
+
+#ifndef __INTERRUPT // for v208
+#define __INTERRUPT __attribute__((interrupt))
+#endif
+
+#define LL_RX              0x01
+#define LL_TX              0x02
+#define LL_STOP            0x08
+#define LL_AUTO            0x01
+
+// common fields
+#define CTRL_CFG           BB0
+#define CRCINIT1           BB1
+#define LLSTATUS           LL2
+#define LLINT_EN           LL3
+#define CTRL_MOD           LL20
+#define TXTUNE_CTRL        RF14
+#define TXCTUNE_CO_CTRL    RF36
+#define TXCTUNE_GA_CTRL    RF37
+#define RXTUNE             RF39
+#define TXCTUNE_CO         RF40
+#define TXCTUNE_GA         RF50
+#define BBSTATUS           BB14
+
+// chip specific fields
 #ifdef CH570_CH572
 #define CRCPOLY1           BB2
 #define ACCESSADDRESS1     BB3
-#define RSSI               BB12 // ? couldn't find it, not sure
 #define CTRL_TX            BB13
 #define CRCINIT2           BB22
 #define CRCPOLY2           BB23
 #define ACCESSADDRESS2     BB24
-#define TMR                LL25
+#define TMR0               LL25 // 5 count down timers, at 2MHz.
+#define INT_ISLER_TMR0     (1 << 17)
+#define TMR1               LL26
+#define INT_ISLER_TMR1     (1 << 18)
+#define TMR2               LL27
+#define INT_ISLER_TMR2     (1 << 19)
+#define TMR3               LL28
+#define INT_ISLER_TMR3     (1 << 20)
+#define TMR4               LL29
+#define INT_ISLER_TMR4     (1 << 21)
 #define TXBUF              LL30
 #define RXBUF              LL31
 #define CTRL_MOD_RFSTOP    0xfffff8ff
 #define DEVSETMODE_ON      ((BB->CTRL_CFG & 0xfffcffff) | 0x20000)
 #define DEVSETMODE_OFF     ((BB->CTRL_CFG & 0xfffcffff) | 0x10000)
-#define DEVSETMODE_TUNE    0x0558
-#define DEVSETMODE_TX      0x0258
-#define DEVSETMODE_RX      0x0158
+#define DEVMODE_TUNE       0x0558
+#define DEVMODE_TX         0x0258
+#define DEVMODE_RX         0x0158
 #define CTRL_CFG_PHY_1M    ((BB->CTRL_CFG & 0xfffffcff) | 0x100)
 #define CTRL_CFG_PHY_2M    (BB->CTRL_CFG & 0xfffffcff)
 #define LL_STATUS_TX       0x20000
@@ -27,16 +149,29 @@
 #elif defined(CH571_CH573)
 #define TXBUF 		       DMA4
 #define ACCESSADDRESS1     BB2
+// BB->CTRL_TX:
+// [26:21] = TX postfix dead time (us)
+// [20:16] = TX postfix 0 carrier time (us)
+// [11:9] = TX prefix 0 carrier time (us)
+// [8:4] = TX dead carrier time (us)
+// [3] not sure, maybe also dead carrier +1 us??  
+// [2] = no apparent effect
 #define CTRL_TX            BB11
 #define RSSI               BB12 // ? couldn't find it, not sure
-#define TMR                LL24
-#define TXBUF              LL28
+// This may be the same on all chips, but only confirmed on ch571/3 so far
+#define BBINT_EN           BB13
+#define TMR0               LL24
+#define INT_ISLER_TMR0     (1 << 14)
+#define TMR1               LL25
+#define INT_ISLER_TMR1     (1 << 15)
+// Registers LL26 - LL31 appear to be hardwired zero
+// Writing seems harmless and simplifies the code flow
 #define RXBUF              LL29
 #define RFEND_TXCTUNE_INIT 0x180000
 #define CTRL_MOD_RFSTOP    0xfffffff8
-#define DEVSETMODE_TUNE    0x5d
-#define DEVSETMODE_TX      0x5a
-#define DEVSETMODE_RX      0x59
+#define DEVMODE_TUNE       0x5d
+#define DEVMODE_TX         0x5a
+#define DEVMODE_RX         0x59
 #define CTRL_CFG_PHY_1M    (BB->CTRL_CFG | 0x10000000)
 #define LL_STATUS_TX       0x20000
 #define CTRL_CFG_START_TX  (BB->CTRL_CFG & 0xefffffff)
@@ -44,7 +179,12 @@
 #define ACCESSADDRESS1     BB2
 #define CTRL_TX            BB11
 #define RSSI               BB12
-#define TMR                LL25
+#define TMR0               LL25 // 3 count down timers, at 2MHz.
+#define INT_ISLER_TMR0     (1 << 13)
+#define TMR1               LL26
+#define INT_ISLER_TMR1     (1 << 14)
+#define TMR2               LL27
+#define INT_ISLER_TMR2     (1 << 15)
 #define TXBUF              LL28
 #define RXBUF              LL29
 #define RFEND_TXCTUNE_INIT 0x880000
@@ -52,9 +192,9 @@
 #define CTRL_MOD_RFSTOP    0xfffffff8
 #define DEVSETMODE_ON      ((BB->CTRL_CFG & 0xfffffe7f) | 0x100)
 #define DEVSETMODE_OFF     ((BB->CTRL_CFG & 0xfffffe7f) | 0x80)
-#define DEVSETMODE_TUNE    0x00dd
-#define DEVSETMODE_TX      0x00da
-#define DEVSETMODE_RX      0x00d9
+#define DEVMODE_TUNE       0x00dd
+#define DEVMODE_TX         0x00da
+#define DEVMODE_RX         0x00d9
 #define CTRL_CFG_PHY_1M    ((BB->CTRL_CFG & 0xffff0fff) | 0x1000)
 #define CTRL_CFG_PHY_2M    (BB->CTRL_CFG & 0xffff0fff)
 #define CTRL_CFG_PHY_CODED ((BB->CTRL_CFG & 0xffff0fff) | 0x2000)
@@ -64,15 +204,24 @@
 #define ACCESSADDRESS1     BB2
 #define CTRL_TX            BB11
 #define RSSI               BB12
-#define TMR                LL25
+#define TMR0               LL25
+#define INT_ISLER_TMR0     (1 << 17)
+#define TMR1               LL26
+#define INT_ISLER_TMR1     (1 << 18)
+#define TMR2               LL27
+#define INT_ISLER_TMR2     (1 << 19)
+#define TMR3               LL28
+#define INT_ISLER_TMR3     (1 << 20)
+#define TMR4               LL29
+#define INT_ISLER_TMR4     (1 << 21)
 #define TXBUF              LL30
 #define RXBUF              LL31
 #define CTRL_MOD_RFSTOP    0xfffff8ff
 #define DEVSETMODE_ON      ((BB->CTRL_CFG & 0xfffffcff) | 0x280)
 #define DEVSETMODE_OFF     ((BB->CTRL_CFG & 0xfffffcff) | 0x100)
-#define DEVSETMODE_TUNE    0x0558
-#define DEVSETMODE_TX      0x0258
-#define DEVSETMODE_RX      0x0158
+#define DEVMODE_TUNE       0x0558
+#define DEVMODE_TX         0x0258
+#define DEVMODE_RX         0x0158
 #define CTRL_CFG_PHY_1M    (BB->CTRL_CFG & 0xffffff7f)
 #define CTRL_CFG_PHY_2M    (BB->CTRL_CFG | 0x80)
 #define LL_STATUS_TX       0x20000
@@ -82,7 +231,12 @@
 #define ACCESSADDRESS1     BB2
 #define CTRL_TX            BB11
 #define RSSI               BB12
-#define TMR                LL25
+#define TMR0               LL25 // 3 count down timers, at 2MHz.
+#define INT_ISLER_TMR0     (1 << 13)
+#define TMR1               LL26
+#define INT_ISLER_TMR1     (1 << 14)
+#define TMR2               LL27
+#define INT_ISLER_TMR2     (1 << 15)
 #define TXBUF              LL28
 #define RXBUF              LL29
 #define RFEND_TXCTUNE_INIT 0x100000
@@ -90,9 +244,9 @@
 #define CTRL_MOD_RFSTOP    0xfffffff8
 #define DEVSETMODE_ON      ((BB->CTRL_CFG & 0xfffffe7f) | 0x100)
 #define DEVSETMODE_OFF     ((BB->CTRL_CFG & 0xfffffe7f) | 0x80)
-#define DEVSETMODE_TUNE    0x5d
-#define DEVSETMODE_TX      0x5a
-#define DEVSETMODE_RX      0x59
+#define DEVMODE_TUNE       0x5d
+#define DEVMODE_TX         0x5a
+#define DEVMODE_RX         0x59
 #define CTRL_CFG_PHY_1M    ((BB->CTRL_CFG & 0xffff0fff) | 0x1000)
 #define CTRL_CFG_PHY_2M    (BB->CTRL_CFG & 0xffff0fff)
 #define CTRL_CFG_PHY_CODED ((BB->CTRL_CFG & 0xffff0fff) | 0x2000)
@@ -142,9 +296,8 @@ typedef struct {
 	// bit 20 = settable, but unknown effect.
 	// bit 24 = set at end of tx routine
 	// bit 29-31 = settable, but unknown effect.
-	volatile uint32_t CTRL_CFG;
-
-	volatile uint32_t CRCINIT1;
+	volatile uint32_t BB0; // CTRL_CFG
+	volatile uint32_t BB1; // CRCINIT1
 	volatile uint32_t BB2; // ch570/2: CRCPOLY1, [ch582/3 ch591/2]: ACCESSADDRESS1
 	volatile uint32_t BB3; // ch570/2 ACCESSADDRESS1
 	volatile uint32_t BB4;
@@ -183,8 +336,8 @@ typedef struct {
 typedef struct {
 	volatile uint32_t LL0;
 	volatile uint32_t LL1;
-	volatile uint32_t STATUS;
-	volatile uint32_t INT_EN;
+	volatile uint32_t LL2; // STATUS
+	volatile uint32_t LL3; // INT_EN
 	volatile uint32_t LL4;
 	volatile uint32_t LL5;
 	volatile uint32_t LL6;
@@ -212,18 +365,18 @@ typedef struct {
 	// Bit 9: If 0, no output.
 	// Bit 10: Somehow required for TX?
 	// Bit 16-17: Normally 1, unknown effect. Seems to suppress odd carrier burst after message.
-	volatile uint32_t CTRL_MOD;
+	volatile uint32_t LL20; // CTRL_MOD
 	volatile uint32_t LL21;
 	volatile uint32_t LL22;
 	volatile uint32_t LL23;
-	volatile uint32_t LL24; // ch571/3: TMR
-	volatile uint32_t LL25; // ch570/2, ch582/3, ch591/2: TMR
-	volatile uint32_t LL26;
-	volatile uint32_t LL27;
-	volatile uint32_t LL28; // ch582/3: TXBUF
-	volatile uint32_t LL29; // ch582/3: RXBUF
-	volatile uint32_t LL30; // ch570/2, ch591/2: TXBUF
-	volatile uint32_t LL31; // ch570/2, ch591/2: RXBUF
+	volatile uint32_t LL24; // ch571/3: TMR0
+	volatile uint32_t LL25; // ch570/2, ch582/3, ch584/5, ch591/2: TMR0, ch571/3: TMR1
+	volatile uint32_t LL26; // ch570/2, ch582/3, ch584/5, ch591/2: TMR1
+	volatile uint32_t LL27; // ch570/2, ch582/3, ch584/5, ch591/2: TMR2
+	volatile uint32_t LL28; // ch582/3, v208: TXBUF, ch570/2, ch582/3, ch584/5, ch591/2: TMR3
+	volatile uint32_t LL29; // ch582/3, v208: RXBUF, ch570/2, ch582/3, ch584/5, ch591/2: TMR4
+	volatile uint32_t LL30; // ch570/2, ch584/5, ch591/2: TXBUF
+	volatile uint32_t LL31; // ch570/2, ch584/5, ch591/2: RXBUF
 } LL_Type;
 
 typedef struct {
@@ -241,7 +394,7 @@ typedef struct {
 	volatile uint32_t RF11;
 	volatile uint32_t RF12;
 	volatile uint32_t RF13;
-	volatile uint32_t TXTUNE_CTRL;
+	volatile uint32_t RF14; // TXTUNE_CTRL
 	volatile uint32_t RF15;
 	volatile uint32_t RF16;
 	volatile uint32_t RF17;
@@ -263,12 +416,12 @@ typedef struct {
 	volatile uint32_t RF33;
 	volatile uint32_t RF34;
 	volatile uint32_t RF35;
-	volatile uint32_t TXCTUNE_CO_CTRL;
-	volatile uint32_t TXCTUNE_GA_CTRL;
+	volatile uint32_t RF36; // TXCTUNE_CO_CTRL
+	volatile uint32_t RF37; // TXCTUNE_GA_CTRL
 	volatile uint32_t RF38;
-	volatile uint32_t RXTUNE;
-	volatile uint32_t TXCTUNE_CO[10];
-	volatile uint32_t TXCTUNE_GA[3];
+	volatile uint32_t RF39; // RXTUNE
+	volatile uint32_t RF40[10]; // TXCTUNE_CO[10]
+	volatile uint32_t RF50[3]; // TXCTUNE_GA[3]
 } RF_Type;
 
 uint8_t channel_map[] = {1,2,3,4,5,6,7,8,9,10,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,0,11,39};
@@ -280,212 +433,160 @@ uint8_t channel_map[] = {1,2,3,4,5,6,7,8,9,10,12,13,14,15,16,17,18,19,20,21,22,2
 #define PHY_S2 4
 #define PHY_S8 8
 
-void DevSetMode(uint16_t mode);
-__attribute__((aligned(4))) uint32_t LLE_BUF[0x110];
+void iSLERSetMode(uint16_t mode);
+void iSLERStop();
+
+ISLER_BUF_ATTR uint32_t LLE_BUF[0x110];
 #ifdef CH571_CH573
-__attribute__((aligned(4))) uint32_t LLE_BUF2[0x110];
+ISLER_BUF_ATTR uint32_t LLE_BUF2[0x110];
 #endif
+
 volatile uint32_t tuneFilter;
 volatile uint32_t tuneFilter2M;
-volatile uint32_t rx_ready;
+volatile uint32_t tx_done;
 
+#ifdef ISLER_CALLBACK_RX
+void ISLER_CALLBACK_RX();
+#else
+volatile uint32_t rx_ready;
+#endif
+#ifdef ISLER_CALLBACK_TX
+void ISLER_CALLBACK_TX();
+#endif
+#ifdef ISLER_CALLBACK_TMR0
+void ISLER_CALLBACK_TMR0();
+#endif
+#ifdef ISLER_CALLBACK_TMR1
+void ISLER_CALLBACK_TMR1();
+#endif
+#ifdef ISLER_CALLBACK_TMR2
+void ISLER_CALLBACK_TMR2();
+#endif
+#ifdef ISLER_CALLBACK_TMR3
+void ISLER_CALLBACK_TMR3();
+#endif
+#ifdef ISLER_CALLBACK_TMR4
+void ISLER_CALLBACK_TMR4();
+#endif
+
+typedef struct {
+	volatile uint32_t access_address;
+	volatile uint32_t txbuf;
+	volatile uint8_t channel;
+	volatile uint8_t phy_mode;
+	volatile uint8_t is_open;
+} LinkConfig_t;
+volatile LinkConfig_t gs_iSLERLink;
 
 
 #ifdef CH571_CH573
-__attribute__((interrupt))
+__INTERRUPT
 void BB_IRQHandler() {
-	// printf("BB\n");
-	if(BB->BB14 & (1<<6)) {
-		BB->BB14 &= 0xffffff9f;
+	if(BB->BBSTATUS & (1<<6)) {
+		BB->BBSTATUS &= 0xffffff9f;
 	}
-	if(BB->BB14 & (1<<1)) {
-		BB->BB14 = 0xfffffffd;
+	if(BB->BBSTATUS & (1<<1)) {
+		BB->BBSTATUS = 0xfffffffd;
 		BB->BB20 = 0x45;
 	}
-	if(BB->BB14 & (1<<4)) {
-		BB->BB14 = 0xffffffef;
+	if(BB->BBSTATUS & (1<<4)) {
+		BB->BBSTATUS = 0xffffffef;
 		BB->BB20 = 0;
 	}
 }
 #endif
 
-__attribute__((interrupt))
+__HIGH_CODE
+__INTERRUPT
 void LLE_IRQHandler() {
-	// printf("LL\n");
+	uint32_t status = LL->LLSTATUS;
 #ifdef CH571_CH573
-	if(LL->STATUS & (1<<9)) {
-		LL->TMR = 400;
+	if(status & (1<<9)) {
+		LL->TMR0 = 400;
 		BB->CTRL_TX = (BB->CTRL_TX & 0xfffffffc) | 2;
 		BB->CTRL_CFG |= 0x10000000;
+		status |= 1; // XXX TODO: Figure out which bit is the RX bit.
 	}
-	LL->STATUS = 0;
-#elif defined(CH582_CH583)
-	if((LL->STATUS & (1<<14)) && (LL->INT_EN & (1<<14))) {
-		LL->LL26 = 0xffffffff;
-		LL->STATUS = 0x4000;
-	}
-	else
-#endif
-	{
-		LL->STATUS &= LL->INT_EN;
-		BB->CTRL_TX = (BB->CTRL_TX & 0xfffffffc) | 1;
-	}
-	DevSetMode(0);
-	LL->CTRL_MOD &= CTRL_MOD_RFSTOP;
-	LL->LL0 |= 0x08;
-
-#ifdef ISLER_CALLBACK
-	ISLER_CALLBACK();
+	LL->LLSTATUS = 0;
 #else
-	rx_ready = 1;
+	LL->LLSTATUS = status; // acknowledge
 #endif
+
+	asm volatile("fence" ::: "memory");
+#if defined(INT_ISLER_TMR0) && defined(ISLER_CALLBACK_TMR0)
+	if(status & INT_ISLER_TMR0) ISLER_CALLBACK_TMR0();
+#endif
+#if defined(INT_ISLER_TMR1) && defined(ISLER_CALLBACK_TMR1)
+	if(status & INT_ISLER_TMR1) ISLER_CALLBACK_TMR1();
+#endif
+#if defined(INT_ISLER_TMR2) && defined(ISLER_CALLBACK_TMR2)
+	if(status & INT_ISLER_TMR2) ISLER_CALLBACK_TMR2();
+#endif
+#if defined(INT_ISLER_TMR3) && defined(ISLER_CALLBACK_TMR3)
+	if(status & INT_ISLER_TMR3) ISLER_CALLBACK_TMR3();
+#endif
+#if defined(INT_ISLER_TMR4) && defined(ISLER_CALLBACK_TMR4)
+	if(status & INT_ISLER_TMR4) ISLER_CALLBACK_TMR4();
+#endif
+
+	if(status & LL_TX) {
+		tx_done = status;
+#ifdef ISLER_CALLBACK_TX
+		ISLER_CALLBACK_TX();
+#endif
+	}
+	else if(status & LL_RX) {
+		BB->CTRL_TX |= 1;
+		iSLERStop();
+
+#ifdef ISLER_CALLBACK_RX
+		ISLER_CALLBACK_RX();
+#else
+		rx_ready = status;
+#endif
+	}
 }
 
-void RFEND_Reset() {
+void iSLERDevInit(uint8_t TxPower) {
 #ifdef CH571_CH573
-	RF->RF3 |= 0x1000;
-	ADD_N_NOPS(20);
-	RF->RF3 &= 0xffffefff;
-	ADD_N_NOPS(20);
-	RF->RF3 |= 0x1000;
-	ADD_N_NOPS(20);
-	RF->RF3 |= 1;
-	ADD_N_NOPS(20);
-	RF->RF3 &= 0xfffffffe;
-	ADD_N_NOPS(20);
-	RF->RF3 |= 1;
-	ADD_N_NOPS(20);
-	RF->RF3 |= 0x100;
-	ADD_N_NOPS(20);
-	RF->RF3 &= 0xfffffeff;
-	ADD_N_NOPS(20);
-	RF->RF3 |= 0x100;
-	ADD_N_NOPS(20);
-#elif defined(CH32V208)
-	RF->RF3 = 0x1101;
-	ADD_N_NOPS(20);
-	RF->RF3 = 0;
-	ADD_N_NOPS(20);
-	RF->RF3 = 0x1101;
-#endif
-}
-
-void DevInit(uint8_t TxPower) {
-#ifdef CH571_CH573
-	DMA->DMA4 = (uint32_t)LLE_BUF;
-	DMA->DMA5 = (uint32_t)LLE_BUF;
-	DMA->DMA6 = (uint32_t)LLE_BUF2;
-	DMA->DMA7 = (uint32_t)LLE_BUF2;
+	// Flipped from blob nomenclature so LLE_BUF is RX on all chips
+	DMA->DMA4 = (uint32_t)LLE_BUF2;
+	DMA->DMA5 = (uint32_t)LLE_BUF2;
+	DMA->DMA6 = (uint32_t)LLE_BUF;
+	DMA->DMA7 = (uint32_t)LLE_BUF;
 	DMA->DMA2 |= 0x2000;
 	DMA->DMA3 |= 0x2000;
 	DMA->DMA2 |= 0x1000;
 	DMA->DMA3 |= 0x1000;
 	DMA->DMA0 |= 2;
 	DMA->DMA0 |= 0x20;
-
-	LL->LL5 = 0x50;
-	LL->LL7 = 10;
-	LL->LL9 = 0x8c;
-	LL->LL13 = 0x8c;
-	LL->LL17 = 0x50;
-	LL->LL19 = 10;
-#elif defined(CH570_CH572) || defined(CH582_CH583) || defined(CH584_CH585) || defined(CH591_CH592)
-	LL->LL5 = 0x8c;
-	LL->LL7 = 0x76;
-	LL->LL9 = 0x8c;
-	LL->LL13 = 0x8c;
-	LL->LL17 = 0x8c;
-	LL->LL19 = 0x76;
-#elif defined(CH32V208)
-	LL->LL5 = 0x8c;
-	LL->LL7 = 0x6c;
-	LL->LL9 = 0x8c;
-	LL->LL13 = 0x8c;
-	LL->LL17 = 0x8c;
-	LL->LL19 = 0x6c;
 #endif
 
 #ifdef CH570_CH572
-	LL->LL11 = 0x6c;
-	LL->LL15 = 0x6c;
-	LL->LL1 = 0x78;
 	LL->LL21 = 0;
-	LL->INT_EN = 0x16000f;
+	LL->LLINT_EN = 0x16000f;
 #elif defined(CH571_CH573)
-	LL->LL11 = 0x3c;
-	LL->LL15 = 0x3c;
 	LL->LL22 = 0xf6;
-	LL->INT_EN = 0xc303;
+	LL->LLINT_EN = 0xc303;
 	NVIC->FIBADDRR = 0x20000000;
 	NVIC->VTFADDR[2] = (uint32_t)LLE_IRQHandler -NVIC->FIBADDRR;
 #elif defined(CH582_CH583) || defined(CH32V208)
-	LL->LL11 = 0x3c;
-	LL->LL15 = 0x3c;
-	LL->INT_EN = 0xf00f;
-#elif defined(CH584_CH585)
-	LL->LL11 = 0x6e;
-	LL->LL15 = 0x6e;
-	LL->LL1 &= 0xffffffe1;
-	LL->LL21 = 0;
-	LL->INT_EN = 0x1f000f;
-#elif defined(CH591_CH592)
-	LL->LL6 = 0x78;
-	LL->LL8 = 0xffffffff;
-	LL->LL11 = 0x6e;
-	LL->LL21 = 0x14;
-	LL->INT_EN = 0x1f000f;
+	LL->LLINT_EN = 0xf00f;
+#elif defined(CH584_CH585) || defined(CH591_CH592)
+	LL->LL21 = 0x0; // 0x14 ch591/2
+	LL->LLINT_EN = 0x1f000f;
 #endif
 
 	LL->RXBUF = (uint32_t)LLE_BUF;
-	LL->STATUS = 0xffffffff;
+	LL->LLSTATUS = 0xffffffff;
 	RF->RF10 = 0x480;
 
 #ifdef CH570_CH572
-	RF->RF12 &= 0xfff9ffff;
-	RF->RF12 |= 0x70000000;
-	RF->RF15 = (RF->RF15 & 0xf8ffffff) | 0x2000000;
-	RF->RF15 = (RF->RF15 & 0x1fffffff) | 0x40000000;
-	RF->RF18 &= 0xfff8ffff;
-	RF->RF20 = (RF->RF20 & 0xfffff8ff) | 0x300;
-	RF->RF23 |= 0x70000;
-	RF->RF23 |= 0x700000;
-
-	BB->BB14 = 0x2020c;
+	BB->BBSTATUS = 0x2020c;
 	BB->BB15 = 0x50;
 	BB->CTRL_TX = (BB->CTRL_TX & 0x1ffffff) | (TxPower | 0x40) << 0x19;
 	BB->CTRL_CFG &= 0xfffffcff;
-#elif defined(CH571_CH573) || defined(CH582_CH583) || defined(CH32V208)
-	RFEND_Reset();
-	RF->RF18 = (RF->RF18 & 0x8fffffff) | 0x20000000;
-	RF->RF18 = (RF->RF18 & 0xf8ffffff) | 0x4000000;
-	RF->RF18 = (RF->RF18 & 0xfffffff0) | 9;
-	RF->RF18 &= 0xfff8ffff;
-	RF->RF18 |= 0x80000000;
-	RF->RF19 = (RF->RF19 & 0xfffffff8) | 3;
-	RF->RF19 = (RF->RF19 & 0xffffff8f) | 0x30;
-	RF->RF19 = (RF->RF19 & 0xfffff8ff) | 0x300;
-	RF->RF19 &= 0xfeffffff;
-	RF->RF19 |= 0x2000000;
-	RF->RF20 = (RF->RF20 & 0xffff0fff) | 0x4000;
-	RF->RF21 = (RF->RF21 & 0xfffffff0) | 0xc;
-	RF->RF21 |= 0x80;
-	RF->RF21 &= 0xffffefff;
-	RF->RF15 = (RF->RF15 & 0xffff0fff) | 0x8000;
-	RF->RF15 = (RF->RF15 & 0xf8ffffff) | 0x2000000;
-	RF->RF15 = (RF->RF15 & 0x1fffffff) | 0x40000000;
-	RF->RF11 |= 0x700000;
-	RF->RF11 &= 0xf8ffffff;
-	RF->RF11 = (RF->RF11 & 0xffffcfff) | 0x2000;
-	RF->RF11 = (RF->RF11 & 0xfffcffff) | 0x20000;
-	RF->RF12 &= 0xfffffff0;
-	RF->RF12 &= 0xffffff0f;
-	RF->RF12 &= 0xfffff8ff;
-	RF->RF12 |= 0x700000;
-	RF->RF12 = (RF->RF12 & 0x8fffffff) | 0x50000000;
-	RF->TXTUNE_CTRL = (RF->TXTUNE_CTRL & 0xff07ffff) | RFEND_TXCTUNE_INIT;
-	RF->TXTUNE_CTRL |= 0x80000000;
-
-#ifdef CH571_CH573
+#elif defined(CH571_CH573)
 	BB->CTRL_CFG = (TxPower << 8) | BB->CTRL_CFG | 0x1008000;
 	BB->CTRL_CFG = (BB->CTRL_CFG & 0xffffc0ff) | (TxPower & 0x3f) << 8;
 	SYS_SAFE_ACCESS(
@@ -496,8 +597,6 @@ void DevInit(uint8_t TxPower) {
 	BB->BB6 |= 0x8000;
 	BB->BB6 = (BB->BB6 & 0xffff807f) | 0x3500;
 	BB->BB13 = 0x152;
-
-	// NVIC->VTFADDR[3] = (uint32_t)BB_IRQHandler +0x14000000; // why 14000000?
 #elif defined(CH582_CH583) || defined(CH32V208)
 	BB->CTRL_CFG |= 0x800000;
 	BB->CTRL_CFG |= 0x10000000;
@@ -505,18 +604,9 @@ void DevInit(uint8_t TxPower) {
 	BB->CTRL_TX = TxPower << 0x19 | CTRL_TX_TXPOWER;
 	BB->CTRL_TX = (BB->CTRL_TX & 0x81ffffff) | (TxPower & 0x3f) << 0x19;
 	BB->BB8 = 0x90083;
-
-	// NVIC->VTFADDR[3] = (uint32_t)BB_IRQHandler +0x20000000; // why 20000000?
-#endif
 #elif defined(CH584_CH585) || defined(CH591_CH592)
-	RF->RF12 = (RF->RF12 & 0x8fffffff) | 0x10077700;
-	RF->RF15 = (RF->RF15 & 0x18ff0fff) | 0x42005000;
-	RF->RF19 &= 0xfffcff88;
-	RF->RF21 = (RF->RF21 & 0xfffffff0) | 9;
-	RF->RF23 &= 0xff88ffff;
-
 	BB->CTRL_CFG |= 0x800000;
-	BB->BB14 = 0x3ff; // ch584/5
+	BB->BBSTATUS = 0x3ff; // ch584/5
 	BB->BB13 = 0x50;
 	BB->CTRL_TX = (BB->CTRL_TX & 0x81ffffff) | (TxPower & 0x3f) << 0x19;
 	uint32_t uVar3 = 0x1000000;
@@ -542,7 +632,7 @@ void DevInit(uint8_t TxPower) {
 	NVIC->VTFIDR[3] = 0x14;
 }
 
-void DevSetMode(uint16_t mode) {
+void iSLERSetMode(uint16_t mode) {
 #if !defined(CH571_CH573)
 	if(mode) {
 		BB->CTRL_CFG = DEVSETMODE_ON;
@@ -561,15 +651,15 @@ void DevSetMode(uint16_t mode) {
 	LL->CTRL_MOD = mode;
 }
 
-uint32_t RFEND_TXCTune(uint8_t channel) {
+uint32_t iSLERTXCTune(uint8_t channel) {
 	// 0xbf = 2401 MHz
 	RF->RF1 &= 0xfffffffe;
 	RF->TXTUNE_CTRL = (RF->TXTUNE_CTRL & 0xfffe00ff) | (0xbf00 + (channel_map[channel] << 8));
 	RF->RF1 |= 1;
 
-	LL->TMR = 8000;
+	LL->TMR0 = 8000;
 	while(!(RF->TXCTUNE_CO_CTRL & (1 << 25)) || !(RF->TXCTUNE_CO_CTRL & (1 << 26))) {
-		if(LL->TMR == 0) {
+		if(LL->TMR0 == 0) {
 			break;
 		}
 	}
@@ -577,12 +667,10 @@ uint32_t RFEND_TXCTune(uint8_t channel) {
 	uint8_t nCO = (uint8_t)RF->TXCTUNE_CO_CTRL & 0x3f;
 	uint8_t nGA = (uint8_t)(RF->TXCTUNE_GA_CTRL >> 10) & 0x7f;
 
-	// printf("nCO,nGA ch:%u idx:%u %u,%u\n", channel, channel_map[channel], nCO,nGA);
-
 	return (nGA << 24) | nCO;
 }
 
-void RFEND_TXTune() {
+void iSLERTXTune() {
 	RF->RF1 &= 0xfffffeff;
 	RF->RF10 &= 0xffffefff;
 	RF->RF11 &= 0xffffffef;
@@ -590,17 +678,17 @@ void RFEND_TXTune() {
 	RF->RF1 |= 0x10;
 
 	// 2401 MHz
-	uint32_t tune2401 = RFEND_TXCTune(37);
+	uint32_t tune2401 = iSLERTXCTune(37);
 	uint8_t nCO2401 = (uint8_t)(tune2401 & 0x3f);
 	uint8_t nGA2401 = (uint8_t)(tune2401 >> 24) & 0x7f;
 
 	// 2480 MHz
-	uint32_t tune2480 = RFEND_TXCTune(39);
+	uint32_t tune2480 = iSLERTXCTune(39);
 	uint8_t nCO2480 = (uint8_t)(tune2480 & 0x3f);
 	uint8_t nGA2480 = (uint8_t)(tune2480 >> 24) & 0x7f;
 
 	// 2440 MHz
-	uint32_t tune2440 = RFEND_TXCTune(18);
+	uint32_t tune2440 = iSLERTXCTune(18);
 	uint8_t nCO2440 = (uint8_t)(tune2440 & 0x3f);
 	uint8_t nGA2440 = (uint8_t)(tune2440 >> 24) & 0x7f;
 
@@ -640,17 +728,6 @@ void RFEND_TXTune() {
 		RF->TXCTUNE_GA[i] = ((uint32_t*)txctune_ga)[i];
 	}
 
-
-#if 0
-	printf("2401 2440 2480 CO: %u %u %u, GA: %u %u %u\n", nCO2401, nCO2440, nCO2480, nGA2401, nGA2440, nGA2480);
-	for(int i = 0; i < 10; i++ ) {
-		printf( "%d: %08lx\n", i, RF->TXCTUNE_CO[i] );
-	}
-	for(int i = 0; i < 3; i++ ) {
-		printf( "%d: %08lx\n", i, RF->TXCTUNE_GA[i] );
-	}
-#endif
-
 	RF->RF1 &= 0xffffffef;
 	RF->RF1 &= 0xfffffffe;
 	RF->RF10 |= 0x1000;
@@ -662,14 +739,14 @@ void RFEND_TXTune() {
 	RF->RF1 |= 0x100;
 }
 
-void RFEND_RXTune() {
+void iSLERRXTune() {
 	RF->RF20 &= 0xfffeffff;
 	RF->RF2 |= 0x200000;
 	RF->RF3 = (RF->RF3 & 0xffffffef) | 0x10;
 	RF->RF1 |= 0x1000;
 
-	LL->TMR = 100;
-	while(LL->TMR && ((RF->RXTUNE >> 8) & 1));
+	LL->TMR0 = 100;
+	while(LL->TMR0 && ((RF->RXTUNE >> 8) & 1));
 
 	tuneFilter = RF->RXTUNE & 0x1f;
 	RF->RF20 |= 0x10000;
@@ -684,25 +761,14 @@ void RFEND_RXTune() {
 	RF->RF1 = (RF->RF1 & 0xfffeffff) | 0x100000;
 }
 
-void RegInit() {
-	DevSetMode(DEVSETMODE_TUNE);
-	RFEND_TXTune();
-	RFEND_RXTune();
-	DevSetMode(0);
+void iSLERTune() {
+	iSLERSetMode(DEVMODE_TUNE);
+	iSLERTXTune();
+	iSLERRXTune();
+	iSLERSetMode(0);
 }
 
-void RFCoreInit(uint8_t TxPower) {
-#if defined(CH571_CH573) || defined(CH584_CH585) // maybe all?
-	NVIC->IENR[0] = 0x1000;
-	NVIC->IRER[0] = 0x1000;
-#endif
-	DevInit(TxPower);
-	RegInit();
-	NVIC->IPRIOR[0x15] |= 0x80;
-	NVIC_EnableIRQ(LLE_IRQn);
-}
-
-void DevSetChannel(uint8_t channel) {
+void iSLERSetChannel(uint8_t channel) {
 #ifdef CH571_CH573
 	BB->BB6 = (BB->BB6 & 0xf8ffffff) | 0x4000000;
 	BB->BB6 = (BB->BB6 & 0xffffff83) | 0x1c;
@@ -711,44 +777,83 @@ void DevSetChannel(uint8_t channel) {
 	BB->CTRL_CFG = (BB->CTRL_CFG & 0xffffff80) | (channel & 0x7f);
 }
 
-__HIGH_CODE
-int8_t ReadRSSI() {
-	return (int8_t)(BB->RSSI >> 0xf);
+void iSLERInit(uint8_t TxPower) {
+#if defined(CH571_CH573) || defined(CH584_CH585) // maybe all?
+	NVIC->IENR[0] = 0x1000;
+	NVIC->IRER[0] = 0x1000;
+#endif
+	iSLERDevInit(TxPower);
+	iSLERTune();
+	NVIC->IPRIOR[0x15] |= 0x80;
+	NVIC_EnableIRQ(LLE_IRQn);
 }
 
 __HIGH_CODE
-void Frame_TX(uint32_t access_address, uint8_t adv[], size_t len, uint8_t channel, uint8_t phy_mode) {
-	BB->CTRL_TX = (BB->CTRL_TX & 0xfffffffc) | 1;
+void iSLERStop() {
+	iSLERSetMode(0);
+	if(LL->LL0 & (LL_RX | LL_TX)) {
+		LL->CTRL_MOD &= CTRL_MOD_RFSTOP;
+		LL->LL0 |= LL_STOP;
+	}
+}
 
-	DevSetChannel(channel);
+__HIGH_CODE
+int iSLERRSSI() {
+#ifdef CH570_CH572
+	uint8_t *tx_buf = (uint8_t*)LLE_BUF;
+	int len = tx_buf[1];
+	return (int8_t)tx_buf[len +4];
+#else
+	return (int8_t)(BB->RSSI >> 0xf);
+#endif
+}
 
-	// Uncomment to disable whitening to debug RF.
-	//BB->CTRL_CFG |= (1<<6);
-	DevSetMode(DEVSETMODE_TX);
+__HIGH_CODE
+int iSLERCRCOK() {
+#if defined(CH570_CH572) || defined(CH584_CH585) || defined(CH591_CH592)
+	uint8_t *tx_buf = (uint8_t*)LLE_BUF;
+	int len = tx_buf[1];
+	return (int8_t)!(tx_buf[len + 5] & 0x10);
+#else
+	// Unimplemented!
+	return -1;
+#endif
+}
 
-	BB->ACCESSADDRESS1 = access_address; // access address
-	BB->CRCINIT1 = 0x555555; // crc init
+__HIGH_CODE
+void iSLERLinkConfig(uint32_t access_address, uint8_t channel, uint8_t phy_mode, uint8_t *txbuf, uint8_t auto_mode) {
+	// Set channel and tx buffer
+	gs_iSLERLink.access_address = access_address;
+	gs_iSLERLink.channel = channel;
+	gs_iSLERLink.txbuf = (uint32_t)txbuf;
+	gs_iSLERLink.phy_mode = phy_mode;
+
+	iSLERSetChannel(channel);
+
+	if (txbuf != NULL) {
+#if defined(CH571_CH573)
+		DMA->TXBUF = (uint32_t)txbuf;
+#else
+		LL->TXBUF = (uint32_t)txbuf;
+#endif
+	}
+
+	// Set Access Address & CRC
+	BB->ACCESSADDRESS1 = access_address;
+	BB->CRCINIT1 = 0x555555;
 #ifdef CH570_CH572
 	BB->ACCESSADDRESS2 = access_address;
 	BB->CRCINIT2 = 0x555555;
-	BB->CRCPOLY1 = (BB->CRCPOLY1 & 0xff000000) | 0x80032d; // crc poly
+	BB->CRCPOLY1 = (BB->CRCPOLY1 & 0xff000000) | 0x80032d;
 	BB->CRCPOLY2 = (BB->CRCPOLY2 & 0xff000000) | 0x80032d;
 #endif
 
-#if defined(CH571_CH573)
-	DMA->TXBUF = (uint32_t)adv;
-#else
-	LL->TXBUF = (uint32_t)adv;
-#endif
-
-	// Wait for tuning bit to clear.
-	for( int timeout = 3000; !(RF->RF26 & 0x1000000) && timeout >= 0; timeout-- );
-
+	// Set Global PHY Mode (CTRL_CFG)
 #if defined(CH582_CH583) || defined(CH32V208)
 	BB->CTRL_CFG = (phy_mode == PHY_2M) ? CTRL_CFG_PHY_2M:
 				   (phy_mode == PHY_S2) ? CTRL_CFG_PHY_CODED:
 				   (phy_mode == PHY_S8) ? CTRL_CFG_PHY_CODED:
-										  CTRL_CFG_PHY_1M; // default 1M for now
+										  CTRL_CFG_PHY_1M; 
 	if(phy_mode > PHY_2M) { // coded phy
 		BB->CTRL_CFG = (BB->CTRL_CFG & 0xffff3fff) | ((phy_mode == PHY_S2) ? 0x4000 : 0);
 	}
@@ -756,72 +861,15 @@ void Frame_TX(uint32_t access_address, uint8_t adv[], size_t len, uint8_t channe
 	BB->CTRL_CFG = CTRL_CFG_PHY_1M; // no 2M PHY on ch571/3
 #else
 	BB->CTRL_CFG = (phy_mode == PHY_2M) ? CTRL_CFG_PHY_2M:
-										  CTRL_CFG_PHY_1M; // default 1M for now
-#endif
-
-#if defined(CH570_CH572)
-	BB->BB9 = (BB->BB9 & 0xf9ffffff) | ((phy_mode == PHY_2M) ? 0 : 0x2000000);
-#endif
-
-#if defined(CH571_CH573)
-	BB->BB11 = (BB->BB11 & 0xfffffffc); // |2 for RX
-#endif
-
-	// This clears bit 17 (If set, seems to have no impact.)
-	LL->LL4 &= 0xfffdffff;
-
-#if !defined(CH571_CH573)
-	LL->STATUS = LL_STATUS_TX;
-#endif
-	LL->TMR = (uint32_t)(len *512); // needs optimisation, per phy mode
-
-	BB->CTRL_CFG |= CTRL_CFG_START_TX;
-	BB->CTRL_TX &= 0xfffffffc;
-
-	LL->LL0 = 2; // Not sure what this does, but on RX it's 1
-
-	while(LL->TMR); // wait for tx buffer to empty
-	DevSetMode(0);
-	if(LL->LL0 & 3) {
-		LL->CTRL_MOD &= CTRL_MOD_RFSTOP;
-		LL->LL0 |= 0x08;
-	}
-}
-
-__HIGH_CODE
-void Frame_RX(uint32_t access_address, uint8_t channel, uint8_t phy_mode) {
-	DevSetMode(0);
-	if(LL->LL0 & 3) {
-		LL->CTRL_MOD &= CTRL_MOD_RFSTOP;
-		LL->LL0 |= 0x08;
-	}
-	LL->TMR = 0;
-
-	DevSetChannel(channel);
-	DevSetMode(DEVSETMODE_RX);
-
-#if defined(CH582_CH583) || defined(CH32V208)
-	BB->CTRL_CFG = (phy_mode == PHY_2M) ? CTRL_CFG_PHY_2M:
-				   (phy_mode == PHY_S2) ? CTRL_CFG_PHY_CODED:
-				   (phy_mode == PHY_S8) ? CTRL_CFG_PHY_CODED:
-										  CTRL_CFG_PHY_1M; // default 1M for now
-	if(phy_mode > PHY_2M) { // coded phy
-		BB->CTRL_CFG = (BB->CTRL_CFG & 0xffff3fff) | ((phy_mode == PHY_S2) ? 0x4000 : 0);
-	}
-#elif defined(CH571_CH573)
-	BB->CTRL_CFG = CTRL_CFG_PHY_1M; // no 2M PHY on ch571/3
-#else
-	BB->CTRL_CFG = (phy_mode == PHY_2M) ? CTRL_CFG_PHY_2M:
-										  CTRL_CFG_PHY_1M; // default 1M for now
+										  CTRL_CFG_PHY_1M; 
 #endif
 
+	// Baseband Tuning
 #ifdef CH570_CH572
-	BB->BB9 = (BB->BB9 & 0xf9ffffff) | ((phy_mode == PHY_2M) ? 0 : 0x2000000);
+	BB->BB9 &= (phy_mode == PHY_2M) ? 0xf9ffffff : 0xfbffffff;
 	RF->RF20 = (RF->RF20 & 0xffffffe0) | ((phy_mode == PHY_2M) ? (tuneFilter2M & 0x1f) : (tuneFilter & 0x1f));
 	BB->BB5 = (BB->BB5 & 0xffffffc0) | ((phy_mode == PHY_2M) ? 0xd : 0xb);
 	BB->BB7 = (BB->BB7 & 0xff00fc00) | ((phy_mode == PHY_2M) ? 0x7f00a0 : 0x79009c);
-#elif defined(CH571_CH573)
-	BB->BB11 = (BB->BB11 & 0xfffffffc) | 2; // no |2 for TX
 #elif defined(CH582_CH583) || defined(CH32V208)
 #if defined(CH582_CH583)
 	BB->BB4 = (phy_mode < PHY_S2) ? 0x3722d0 : 0x3722df;
@@ -838,15 +886,75 @@ void Frame_RX(uint32_t access_address, uint8_t channel, uint8_t phy_mode) {
 	BB->BB4 = (BB->BB4 & 0x00ffffff) | ((phy_mode == PHY_2M) ? 0x78000000 : 0x7f000000);
 #endif
 
-	BB->ACCESSADDRESS1 = access_address; // access address
-	BB->CRCINIT1 = 0x555555; // crc init
-#ifdef CH570_CH572
-	BB->ACCESSADDRESS2 = access_address;
-	BB->CRCINIT2 = 0x555555;
-	BB->CRCPOLY1 = (BB->CRCPOLY1 & 0xff000000) | 0x80032d; // crc poly
-	BB->CRCPOLY2 = (BB->CRCPOLY2 & 0xff000000) | 0x80032d;
+	if(auto_mode) {
+		// switch between RX <-> TX automatically
+		LL->LL1 |= LL_AUTO;
+	}
+	else {
+		LL->LL1 &= ~LL_AUTO;
+	}
+
+	gs_iSLERLink.is_open = 1;
+}
+
+__HIGH_CODE
+void iSLERLinkTX() {
+	iSLERStop();
+	iSLERSetMode(DEVMODE_TX);
+
+	// Wait for PLL tuning bit to clear (ensures radio has settled after mode switch)
+	// TODO: RF26 0x1000000 is for CH570/2, figure out this bit on the others
+	for( int timeout = Ticks_from_Us(100); !(RF->RF26 & 0x1000000) && timeout >= 0; timeout-- );
+
+#if defined(CH584_CH585) || defined(CH591_CH592)
+	BB->CTRL_CFG = (gs_iSLERLink.phy_mode == PHY_2M) ? CTRL_CFG_PHY_2M: CTRL_CFG_PHY_1M;
 #endif
 
-	LL->LL0 = 1; // Not sure what this does, but on TX it's 2
-	rx_ready = 0;
+	tx_done = 0;
+	BB->CTRL_CFG |= CTRL_CFG_START_TX;
+	BB->CTRL_TX &= ~((1 << 0) | (1 << 1)); // TX specific: Clear bits 0-1
+	LL->LL0 = LL_TX;
+}
+
+__HIGH_CODE
+void iSLERLinkRX(void) {
+	iSLERStop();
+	iSLERSetMode(DEVMODE_RX);
+
+#ifdef CH571_CH573
+	BB->CTRL_TX |= (1 << 1); // RX specific: Set bit 1
+#elif defined(CH584_CH585) || defined(CH591_CH592)
+	BB->CTRL_CFG = (gs_iSLERLink.phy_mode == PHY_2M) ? CTRL_CFG_PHY_2M: CTRL_CFG_PHY_1M;
+#endif
+
+	LL->LL0 = LL_RX;
+#ifndef ISLER_CALLBACK_RX
+	rx_ready = 0;
+#endif
+}
+
+__HIGH_CODE
+void iSLERTX(uint32_t access_address, uint8_t txbuf[], size_t len, uint8_t channel, uint8_t phy_mode) {
+	txbuf[1] = (len > 257) ? 255 : len -2;
+	iSLERLinkConfig(access_address, channel, phy_mode, txbuf, /*auto_mode*/0);
+	iSLERLinkTX();
+
+	// make it blocking using Idle, for more control over things use iSLERLink[Config,RX,TX]
+#ifdef ISLER_IDLE_WHILE_TX
+	while(!tx_done) {
+		NVIC->SCTLR &= ~(1 << 2); // don't deep sleep
+		NVIC->SCTLR &= ~(1 << 3); // wfi
+		__WFI();
+	}
+#else
+	for( int timeout = Ticks_from_Ms(5); !tx_done && timeout >= 0; timeout-- );
+#endif
+	gs_iSLERLink.is_open = 0;
+}
+
+__HIGH_CODE
+void iSLERRX(uint32_t access_address, uint8_t channel, uint8_t phy_mode) {
+	iSLERLinkConfig(access_address, channel, phy_mode, NULL, /*auto_mode*/0);
+	iSLERLinkRX();
+	gs_iSLERLink.is_open = 0;
 }
diff --git a/inc/extralibs/rtc.h b/inc/extralibs/rtc.h
new file mode 100644
index 0000000..2b90ac7
--- /dev/null
+++ b/inc/extralibs/rtc.h
@@ -0,0 +1,415 @@
+// MIT License
+// Copyright (c) 2025 UniTheCat
+// This library supports two WCH hardware implementations for an RTC
+// and distinguishes between them using compile-time flags.
+#include <stdio.h>
+#if defined(CH32V10x) 
+#include "ch32v103hw.h"
+#elif defined (CH32L103)
+#include "ch32l103hw.h"
+#elif defined (CH32V20x)
+#include "ch32v20xhw.h"
+#elif defined (CH32V30x)
+#include "ch32v30xhw.h"
+#elif defined (CH5xx)
+#include "ch5xxhw.h"
+#else
+#error "Currently unsupported RTC"
+#endif
+
+#if !defined(RTC_CLOCK_SOURCE_LSI) && !defined(RTC_CLOCK_SOURCE_LSE)
+#define RTC_CLOCK_SOURCE_LSI
+#endif
+
+#define RTC_CLOCK_TICKS_PER_SECOND 32768
+
+#ifndef CH5xx // No custom RTC divider support on CH5xx
+#ifndef RTC_CLOCK_TICKS_PER_INCREMENT
+#define RTC_CLOCK_TICKS_PER_INCREMENT RTC_CLOCK_TICKS_PER_SECOND
+#endif
+#define RTC_INCREMENTS_PER_SECOND (RTC_CLOCK_TICKS_PER_SECOND / RTC_CLOCK_TICKS_PER_INCREMENT)
+#endif // !CH5xx
+
+#define SECONDS_PER_MINUTE 60
+#define SECONDS_PER_HOUR 3600
+#define SECONDS_PER_DAY 86400
+
+#define IS_LEAP_YEAR(year) ((((year) % 4 == 0) && ((year) % 100 != 0)) || ((year) % 400 == 0))
+
+// Array of days in each month (non-leap year)
+const u8 DAYS_IN_MONTH[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
+
+
+typedef struct {
+	u16 year;
+	u8 month;
+	u8 day;
+} rtc_date_t;
+
+typedef struct {
+	u8 hr;
+	u8 min;
+	u8 sec;
+	u16 ms;
+} rtc_time_t;
+
+typedef struct {
+	rtc_date_t date;
+	rtc_time_t time;
+} rtc_datetime_t;
+
+const rtc_date_t date_reference = {2020, 1, 1};
+
+// calculate days of the current year. eg. 2020-02-05 is 36 day
+u32 RTC_days_of_year(u16 year, u8 month, u8 day) {
+	u32 day_of_year = 0;
+	
+	// Add days from January to month-1
+	for (u8 m = 0; m < month - 1; m++) {
+		day_of_year += DAYS_IN_MONTH[m];
+	}
+	
+	// Add extra day for February (full month) if it's a leap year
+	if (month > 2 && IS_LEAP_YEAR(year)) {
+		day_of_year += 1;
+	}
+	
+	// Add days in the current month
+	day_of_year += day;
+
+	return day_of_year;
+}
+
+u32 RTC_get_seconds(u16 year, u8 month, u8 day, u8 hr, u8 min, u8 sec) {
+	//# Validate input
+	if (month < 1 || month > 12 || day < 1 || day > 31 || 
+		hr > 23 || min > 59 || sec > 59 || year < 1970) { return 0; }
+
+	// calculate days of the current year, -1 for 0-based days
+	u32 days = RTC_days_of_year(year, month, day) - 1;
+
+	// add the days count excluding the current year
+	// (start from epoch time 1970-01-01 00:00:00)
+	for (int y=1970; y < year; y++) {
+		days += IS_LEAP_YEAR(y) ? 366 : 365;
+	}
+
+	// calculate total seconds
+	return days * SECONDS_PER_DAY + 
+			hr * SECONDS_PER_HOUR +
+			min * SECONDS_PER_MINUTE + sec;
+}
+
+rtc_time_t RTC_get_time(u32 total_seconds, u16 ms) {
+	u32 minutes = total_seconds / 60;
+
+	return (rtc_time_t) {
+		.sec = total_seconds % 60,
+		.min = minutes % 60,
+		.hr = (minutes / 60) % 24,
+		.ms = ms
+	};
+}
+
+rtc_date_t RTC_get_date(u32 total_seconds) {
+	rtc_date_t output = {
+		.year = date_reference.year,
+		.month = date_reference.month,
+		.day = date_reference.day
+	};
+
+	// Days since epoch
+	u32 days_remaining = total_seconds / SECONDS_PER_DAY;
+
+	// Find the year
+	while(1) {
+		u16 days_in_year = IS_LEAP_YEAR(output.year) ? 366 : 365;
+		if (days_remaining < days_in_year) break;
+		days_remaining -= days_in_year;
+		output.year++;
+	}
+
+	// find the month
+	for (u8 m = 0; m < 12; m++) {
+		u8 days_in_month = DAYS_IN_MONTH[m];
+		if (m == 1 && IS_LEAP_YEAR(output.year)) days_in_month = 29;
+		if (days_remaining < days_in_month) break;
+
+		days_remaining -= days_in_month;
+		output.month++;
+	}
+
+	// add 1 because days_remaining is 0-based
+	output.day = days_remaining + 1;
+
+	return output;	
+}
+
+void RTC_print_date(rtc_date_t date, char *delimiter) {
+	printf("%04d", date.year);
+	printf("%s%02d", delimiter, date.month);
+	printf("%s%02d", delimiter, date.day);
+}
+
+void RTC_print_time(rtc_time_t time) {
+	printf("%02d:%02d:%02d.%03d",
+		time.hr, time.min, time.sec, time.ms);
+}
+
+
+//! ####################################
+//! CORE FUNCTION
+//! ####################################
+
+// There are potential speed improvements to this function, such as not resetting the
+// backup domain or the LSI/LSE clock if they are already enabled. However, after
+// testing with standby resets, it either didn't work at all or stopped working
+// after a few minutes. Tread carefully!
+void RTC_init() {
+#ifdef CH5xx
+	SYS_SAFE_ACCESS(
+		R32_RTC_TRIG = 0;
+		R32_RTC_CTRL |= RB_RTC_LOAD_HI;
+		R32_RTC_CTRL |= RB_RTC_LOAD_LO;
+		R8_RTC_MODE_CTRL |= RB_RTC_TRIG_EN;  //enable RTC trigger
+	);
+#else
+	RCC->APB1PCENR |= RCC_APB1Periph_PWR | RCC_APB1Periph_BKP;
+
+	// backup domain enable
+	PWR->CTLR |= PWR_CTLR_DBP;
+
+	// prepare to configure clock source
+	RCC->BDCTLR |= RCC_BDRST;
+	RCC->BDCTLR &=~ RCC_BDRST;
+
+	#ifdef RTC_CLOCK_SOURCE_LSI
+	// Takes 300us, even if already enabled
+	RCC->RSTSCKR |= RCC_LSION;
+	while ( !(RCC->RSTSCKR & RCC_LSIRDY) );
+
+	// Set clock source for RTC
+	RCC->BDCTLR &=~ RCC_RTCSEL;
+	RCC->BDCTLR |= RCC_RTCSEL_LSI;
+	#elif defined(RTC_CLOCK_SOURCE_LSE)
+	// Enable LSE
+	RCC->BDCTLR |= RCC_LSEON;
+	uint16_t timeout = 0;
+	while ( !(RCC->BDCTLR & RCC_LSERDY) ) {
+		Delay_Ms(1);
+		timeout++;
+		if (timeout > 1000) {
+			printf("Could not start LSE.\r\n");
+			break;
+		}
+	}
+	// Set clock source for RTC
+	RCC->BDCTLR &=~ RCC_RTCSEL;
+	RCC->BDCTLR |= RCC_RTCSEL_LSE;
+	#else 
+	#error "No RTC clock source defined"
+	#endif
+
+	RCC->BDCTLR |= RCC_RTCEN;
+
+	// Wait until we can safely communicate with RTC registers
+	RTC->CTLRL &= ~RTC_FLAG_RSF;
+	while (!(RTC->CTLRL & RTC_FLAG_RSF));
+
+	// Reset alarm, second, and overflow flags
+	RTC->CTLRL &= ~(RTC_CTLRL_ALRF | RTC_CTLRL_SECF | RTC_CTLRL_OWF);
+	
+	RTC_CONFIG_CHANGE(
+		// Set prescaler value
+		RTC->PSCRH = ((RTC_CLOCK_TICKS_PER_INCREMENT-1) & PRLH_MSB_MASK) >> 16;
+		RTC->PSCRL = (RTC_CLOCK_TICKS_PER_INCREMENT-1) & RTC_LSB_MASK;
+	);
+#endif	
+}
+
+#ifdef CH5xx
+void RTC_getCounter(u16 *days, u16 *seconds_2s, u16 *ticks_32k) {
+	u16 days_1, days_2, seconds_2s_1, seconds_2s_2, ticks;
+
+	// properly deal with asynchronous read of multiple registers in
+	// rollover case
+	days_1 = R32_RTC_CNT_DAY;
+	seconds_2s_1 = R16_RTC_CNT_2S;
+	ticks = R16_RTC_CNT_32K;
+	days_2 = R32_RTC_CNT_DAY;
+	seconds_2s_2 = R16_RTC_CNT_2S;
+
+	if (days_1 != days_2 || seconds_2s_1 != seconds_2s_2) {
+		// Rollover occurred, re-read all values
+		*days = R32_RTC_CNT_DAY;
+		*seconds_2s = R16_RTC_CNT_2S;
+		*ticks_32k = R16_RTC_CNT_32K;
+	} else {
+		// No rollover, use the captured values
+		*days = days_1;
+		*seconds_2s = seconds_2s_1;
+		*ticks_32k = ticks;
+	}
+}
+
+#else
+uint32_t RTC_getCounter(void) {
+	uint16_t high1, high2, low;
+	uint32_t s;
+   
+	// Wait for registers to be synchronized
+	RTC->CTLRL &= ~RTC_FLAG_RSF;
+	while (!(RTC->CTLRL & RTC_FLAG_RSF));
+
+	high1 = RTC->CNTH;
+	low = RTC->CNTL;
+	high2 = RTC->CNTH;
+
+	// properly deal with asynchronous read of multiple registers in
+	// rollover case
+	if (high1 != high2) {
+		s = (high2 << 16) | RTC->CNTL;
+	} else {
+		s = (high1 << 16) | low;
+	}
+	
+	return s;
+}
+#endif
+
+rtc_datetime_t RTC_getDateTime() {
+#ifdef CH5xx
+	u16 days, seconds_2s, ticks_32k;
+	RTC_getCounter(&days, &seconds_2s, &ticks_32k);
+	u32 total_seconds = (days * SECONDS_PER_DAY) + (seconds_2s * 2) + (ticks_32k / RTC_CLOCK_TICKS_PER_SECOND);
+	u16 ms = ((ticks_32k % RTC_CLOCK_TICKS_PER_SECOND) * 1000UL) / RTC_CLOCK_TICKS_PER_SECOND;
+#else
+	u32 count = RTC_getCounter();
+	u32 total_seconds = count / RTC_INCREMENTS_PER_SECOND;
+	u16 ms = ((count % RTC_INCREMENTS_PER_SECOND) * 1000UL) / RTC_INCREMENTS_PER_SECOND;
+#endif
+	
+	rtc_date_t date = RTC_get_date(total_seconds);
+	rtc_time_t time = RTC_get_time(total_seconds, ms);
+	return (rtc_datetime_t) {date, time};
+}
+
+#ifdef CH5xx
+void RTC_setDayCounter(u32 day_counter) {
+	SYS_SAFE_ACCESS(
+		R32_RTC_TRIG = day_counter - 1;
+		R8_RTC_MODE_CTRL |= RB_RTC_LOAD_HI;
+	);
+}
+
+void RTC_setTimeCounter(u32 time_counter) {
+	SYS_SAFE_ACCESS(
+		R32_RTC_TRIG = time_counter;
+		R8_RTC_MODE_CTRL |= RB_RTC_LOAD_LO;
+	);
+}
+#endif // CH5xx
+
+void RTC_setDateTime(rtc_datetime_t *datetime) {
+	// Clip year
+	u16 year_min = date_reference.year;
+#ifdef CH5xx
+	u16 year_max = date_reference.year + 0x3FFF / 365; // 14 bits = ~44 years
+#else
+	u16 year_max = date_reference.year + 0xFFFFFFFF / 60 / 60 / 24 / 365 / RTC_INCREMENTS_PER_SECOND;
+#endif
+
+	if (datetime->date.year < year_min) datetime->date.year = year_min;
+	if (datetime->date.year > year_max) datetime->date.year = year_max;
+
+#ifdef CH5xx
+	u32 days_counter = 0;
+	// Add days for complete years from year_min to year-1
+	for (u16 y = year_min; y < datetime->date.year; y++) {
+		days_counter += IS_LEAP_YEAR(y) ? 366 : 365;
+	}
+
+	// Add days in the current year
+	days_counter += RTC_days_of_year(datetime->date.year, datetime->date.month, datetime->date.day);
+	RTC_setDayCounter(days_counter);
+
+	u16 seconds = datetime->time.hr * 3600 + datetime->time.min * 60 + datetime->time.sec;
+	u16 time_2s = seconds / 2;
+	u32 time_32k = (seconds % 2) * RTC_CLOCK_TICKS_PER_SECOND + datetime->time.ms * RTC_CLOCK_TICKS_PER_SECOND / 1000;
+	u32 time_counter = (time_2s << 16) | (time_32k & 0xFFFF);
+	RTC_setTimeCounter(time_counter);
+	
+#else
+	u32 total_seconds = RTC_get_seconds(datetime->date.year, datetime->date.month, datetime->date.day,
+										datetime->time.hr, datetime->time.min, datetime->time.sec);
+	u32 increments = total_seconds * RTC_INCREMENTS_PER_SECOND +
+						(datetime->time.ms * RTC_INCREMENTS_PER_SECOND) / 1000;
+	RTC_CONFIG_CHANGE(
+		RTC->CNTH = increments >> 16;
+		RTC->CNTL = increments & RTC_LSB_MASK;	
+	);
+#endif
+}
+
+#ifdef CH5xx
+// Set RTC to generate an interrupt after cyc ticks.
+// Keep RTCTrigger function name for legacy reasons
+void RTCTrigger(uint32_t cyc) 
+{
+	//get the rtc current time
+	uint32_t alarm = (uint32_t) R16_RTC_CNT_LSI | ( (uint32_t) R16_RTC_CNT_DIV1 << 16 );
+	alarm += cyc;
+
+	if( alarm > RTC_MAX_COUNT ) {
+		alarm -= RTC_MAX_COUNT;
+	}
+
+	SYS_SAFE_ACCESS(
+		R32_RTC_TRIG = alarm;
+	);
+}
+void RTC_setAlarm(u32 increments) { RTCTrigger(increments); }
+#else
+void RTC_setAlarm(u32 increments) {
+	// Make sure AFIO power is on for EXTI interrupts
+	RCC->APB2PCENR |= RCC_AFIOEN;
+
+	// Clear any pending alarm interrupt before reconfiguring
+	NVIC_ClearPendingIRQ(RTCAlarm_IRQn);
+	EXTI->INTFR = EXTI_Line17;	
+	RTC->CTLRL &= ~RTC_CTLRL_ALRF;
+	
+	RTC_CONFIG_CHANGE(
+		RTC->ALRMH = increments >> 16;
+		RTC->ALRML = increments & RTC_LSB_MASK;	
+	);
+
+	// A few RTC ticks are necessary before enabling EXTI, as they are on two
+	// different clock domains and <my current working theory> EXTI fires prematurely
+	// on stale alarm register values?
+	// What is weird is that waiting for the register synchronization flag (RSF) to be
+	// set is not sufficient and doesn't appear to make any difference for the timing!
+	Delay_Us(100);
+
+	EXTI->INTENR   |= EXTI_Line17;
+	EXTI->RTENR    |= EXTI_Line17;   // RTC alarm is rising-edge
+	NVIC_EnableIRQ(RTCAlarm_IRQn);
+	RTC->CTLRH |= RTC_CTLRH_ALRIE;
+}
+
+void RTC_setAlarmAbsolute(rtc_datetime_t *datetime) {
+	u32 total_seconds = RTC_get_seconds(datetime->date.year, datetime->date.month, datetime->date.day,
+									datetime->time.hr, datetime->time.min, datetime->time.sec);
+	u32 increments = total_seconds * RTC_INCREMENTS_PER_SECOND +
+						(datetime->time.ms * RTC_INCREMENTS_PER_SECOND) / 1000;
+	RTC_setAlarm(increments);
+}
+
+void RTC_setAlarmRelative(u16 seconds_from_now, u16 milliseconds_from_now) {
+	u32 current = RTC_getCounter();
+	u32 alarm_value = current + (seconds_from_now * RTC_INCREMENTS_PER_SECOND) + 
+		(milliseconds_from_now * RTC_INCREMENTS_PER_SECOND) / 1000;
+	RTC_setAlarm(alarm_value);
+}
+
+#endif // !CH5xx
diff --git a/inc/extralibs/ssd1306.h b/inc/extralibs/ssd1306.h
index 97edc67..81fe798 100644
--- a/inc/extralibs/ssd1306.h
+++ b/inc/extralibs/ssd1306.h
@@ -11,13 +11,17 @@
 #include "font_8x8.h"
 
 // comfortable packet size for this OLED
+#if SSD1306_3WIRE_SPI && !SSD1306_SOFT_SPI
+#define SSD1306_PSZ 8
+#else
 #define SSD1306_PSZ 32
+#endif
 
 #if defined (SSD1306_CUSTOM)
 // Let the caller configure the OLED.
 #else
 // characteristics of each type
-#if !defined (SSD1306_64X32) && !defined (SSD1306_72X40) && !defined (SSD1306_128X32) && !defined (SSD1306_128X64) && !defined (SH1107_128x128) && !(defined(SSD1306_W) && defined(SSD1306_H) && defined(SSD1306_OFFSET) )
+#if !defined (SSD1306_64X32) && !defined (SSD1306_72X40) && !defined (SSD1306_128X32) && !defined (SSD1306_128X64) && !defined (SH1106_128x64) && !defined (SH1107_128x128) && !(defined(SSD1306_W) && defined(SSD1306_H) && defined(SSD1306_OFFSET) )
 	#error "Please define the SSD1306_WXH resolution used in your application"
 #endif
 
@@ -48,6 +52,15 @@
 #define SSD1306_OFFSET 0
 #endif
 
+#ifdef SH1106_128x64
+#define SH1107
+#define SSD1306_FULLUSE
+#define SSD1306_W 128
+#define SSD1306_H 64
+#define SSD1306_FULLUSE
+#define SSD1306_OFFSET 2
+#endif
+
 #ifdef SH1107_128x128
 #define SH1107
 #define SSD1306_FULLUSE
@@ -112,6 +125,7 @@ uint8_t ssd1306_data(uint8_t *data, int sz)
 // OLED initialization commands for 128x32
 const uint8_t ssd1306_init_array[] =
 {
+
 #ifdef SH1107
 	SSD1306_DISPLAYOFF,               // Turn OLED off
 	0x00,                             // Low column
@@ -206,8 +220,8 @@ void ssd1306_refresh(void)
 	for(i=0;i<SSD1306_H/8;i++)
 	{
 		ssd1306_cmd(0xb0 | i);
-		ssd1306_cmd( 0x00 | (0&0xf) ); 
-		ssd1306_cmd( 0x10 | (0>>4) );
+		ssd1306_cmd( 0x00 | (SSD1306_OFFSET&0xf) );
+		ssd1306_cmd( 0x10 | (SSD1306_OFFSET>>4) );
 		ssd1306_data(&ssd1306_buffer[i*4*SSD1306_PSZ+0*SSD1306_PSZ], SSD1306_PSZ);
 		ssd1306_data(&ssd1306_buffer[i*4*SSD1306_PSZ+1*SSD1306_PSZ], SSD1306_PSZ);
 		ssd1306_data(&ssd1306_buffer[i*4*SSD1306_PSZ+2*SSD1306_PSZ], SSD1306_PSZ);
@@ -298,15 +312,18 @@ void ssd1306_drawImage(uint32_t x, uint32_t y, const unsigned char* input, uint3
 			uint32_t input_byte = input[byte + line * bytes_to_draw];
 
 			for (pixel = 0; pixel < 8; pixel++) {
-				x_absolute = x + 8 * (bytes_to_draw - byte) + pixel;
+				x_absolute = x + 8 * byte + pixel;
 				if (x_absolute >= SSD1306_W) {
 					break;
 				}
 				// looking at the horizontal display, we're drawing bytes bottom to top, not left to right, hence y / 8
 				buffer_addr = x_absolute + SSD1306_W * (y_absolute / 8);
 				// state of current pixel
-				uint8_t input_pixel = input_byte & (1 << pixel);
-
+#ifdef SSD1306_BMP_LSB_FIRST
+				uint8_t input_pixel = input_byte & (1 << pixel);  // LSB first
+#else
+				uint8_t input_pixel = input_byte & (0x80 >> pixel); // MSB first (default)
+#endif
 				switch (color_mode) {
 					case 0:
 						// write pixels as they are
@@ -592,7 +609,7 @@ void ssd1306_drawchar(uint8_t x, uint8_t y, uint8_t chr, uint8_t color)
 /*
  * draw a string to the display
  */
-void ssd1306_drawstr(uint8_t x, uint8_t y, char *str, uint8_t color)
+void ssd1306_drawstr(int x, int y, const char *str, uint8_t color)
 {
 	uint8_t c;
 	
@@ -618,7 +635,7 @@ typedef enum {
 /*
  * Draw character to the display buffer, scaled to size
  */
-void ssd1306_drawchar_sz(uint8_t x, uint8_t y, uint8_t chr, uint8_t color, font_size_t font_size)
+void ssd1306_drawchar_sz(int x, int y, uint8_t chr, int color, font_size_t font_size)
 {
     uint16_t i, j, col;
     uint8_t d;
@@ -657,7 +674,7 @@ void ssd1306_drawchar_sz(uint8_t x, uint8_t y, uint8_t chr, uint8_t color, font_
 /*
  * draw a string to the display buffer, scaled to size
  */
-void ssd1306_drawstr_sz(uint8_t x, uint8_t y, char *str, uint8_t color, font_size_t font_size)
+void ssd1306_drawstr_sz(int x, int y, const char *str, uint8_t color, font_size_t font_size)
 {
 	uint8_t c;
 	
diff --git a/inc/extralibs/ssd1306_spi.h b/inc/extralibs/ssd1306_spi.h
index 60c6ed0..676696f 100644
--- a/inc/extralibs/ssd1306_spi.h
+++ b/inc/extralibs/ssd1306_spi.h
@@ -35,6 +35,10 @@
 #define SSD1306_SOFT_SPI 0
 #endif
 
+#ifndef SSD1306_3WIRE_SPI
+#define SSD1306_3WIRE_SPI 0
+#endif
+
 /*
  * init SPI and GPIO for SSD1306 OLED
  */
@@ -101,6 +105,10 @@ void ssd1306_rst(void)
  */
 uint8_t ssd1306_pkt_send(const uint8_t *data, int sz, uint8_t cmd)
 {
+#if SSD1306_3WIRE_SPI
+	uint8_t leftover = 0;
+	int8_t dc_pos = 7;
+#else
 	if(cmd)
 	{
 		funDigitalWrite( SSD1306_DC_PIN, FUN_LOW );
@@ -109,7 +117,7 @@ uint8_t ssd1306_pkt_send(const uint8_t *data, int sz, uint8_t cmd)
 	{
 		funDigitalWrite( SSD1306_DC_PIN, FUN_HIGH );
 	}
-
+#endif
 	funDigitalWrite( SSD1306_CS_PIN, FUN_LOW );
 	
 	// send data
@@ -118,6 +126,11 @@ uint8_t ssd1306_pkt_send(const uint8_t *data, int sz, uint8_t cmd)
 #if SSD1306_SOFT_SPI
 		uint8_t c = *data++;
 		int i = 8;
+#if SSD1306_3WIRE_SPI
+			funDigitalWrite( SSD1306_SCK_PIN, FUN_LOW );       ADD_N_NOPS(1)
+			funDigitalWrite( SSD1306_MOSI_PIN, !cmd );         ADD_N_NOPS(1)
+			funDigitalWrite( SSD1306_SCK_PIN, FUN_HIGH );      ADD_N_NOPS(1)
+#endif
 		do
 		{
 			funDigitalWrite( SSD1306_SCK_PIN, FUN_LOW );       ADD_N_NOPS(1)
@@ -128,15 +141,41 @@ uint8_t ssd1306_pkt_send(const uint8_t *data, int sz, uint8_t cmd)
 
 #elif defined( CH5xx )
 		while(! (R8_SPI0_INT_FLAG & RB_SPI_FREE) );
+#if SSD1306_3WIRE_SPI
+		uint8_t byte = leftover | (!cmd) << dc_pos | (*data) >> (8-dc_pos);
+		leftover = (*data++) << (dc_pos);
+		if( dc_pos-- < 0 ) dc_pos = 7;
+		R8_SPI0_BUFFER = byte;
+#else
 		R8_SPI0_BUFFER = *data++;
+#endif
 #else
 		// wait for TXE
 		while(!(SPI1->STATR & SPI_STATR_TXE));
 		
 		// Send byte
+#if SSD1306_3WIRE_SPI
+		uint8_t byte = leftover | (!cmd) << dc_pos | (*data) >> (8-dc_pos);
+		leftover = (*data++) << (dc_pos--);
+		if( dc_pos < 0 ) dc_pos = 7;
+		SPI1->DATAR = byte;
+#else
 		SPI1->DATAR = *data++;
+#endif
 #endif
 	}
+#if !SSD1306_SOFT_SPI && SSD1306_3WIRE_SPI
+	if( dc_pos )
+	{
+#if defined( CH5xx )
+		while(! (R8_SPI0_INT_FLAG & RB_SPI_FREE) );
+		R8_SPI0_BUFFER = leftover;
+#else
+		while(!(SPI1->STATR & SPI_STATR_TXE));
+		SPI1->DATAR = leftover;
+#endif
+	}
+#endif
 	
 	// wait for not busy before exiting
 #if SSD1306_SOFT_SPI
diff --git a/inc/extralibs/static_i2c.h b/inc/extralibs/static_i2c.h
index 7dc4b7e..ac12888 100644
--- a/inc/extralibs/static_i2c.h
+++ b/inc/extralibs/static_i2c.h
@@ -16,8 +16,7 @@
 
 	I2CPREFIX   -> #define to be the prefix, i.e. BOB will cause BOBConfigI2C
 		to be generated.
-	I2CNOSTATIC -> #define if you want the functions to be generated as
-		not-static code.
+	I2CCODE -> decorate functions with this, otherwise assume it's static.
 
 	NOTE: You must initially configure the port to be outputs on both DSDA
 		and DSCL and set them both to be driven high.
@@ -46,10 +45,8 @@
     #define I2CPREFIX
 #endif
 
-#ifndef I2CNOSTATIC
-    #define I2CSTATICODE
-#else
-    #define I2CSTATICODE static
+#ifndef I2CCODE
+    #define I2CCODE static
 #endif
 
 #ifndef I2CFNCOLLAPSE
@@ -69,13 +66,13 @@
     #define DSDA_IHIGH DSDA_INPUT
 #endif
 
-I2CSTATICODE void I2CFNCOLLAPSE(I2CPREFIX, ConfigI2C)()
+I2CCODE void I2CFNCOLLAPSE(I2CPREFIX, ConfigI2C)()
 {
 	DSDA_IHIGH
 	DSCL_IHIGH
 }
 
-I2CSTATICODE void I2CFNCOLLAPSE(I2CPREFIX, SendStart)()
+I2CCODE void I2CFNCOLLAPSE(I2CPREFIX, SendStart)()
 {
 	DELAY1
 	DSCL_IHIGH
@@ -86,7 +83,7 @@ I2CSTATICODE void I2CFNCOLLAPSE(I2CPREFIX, SendStart)()
 	DELAY1
 }
 
-I2CSTATICODE void I2CFNCOLLAPSE(I2CPREFIX, SendStop)()
+I2CCODE void I2CFNCOLLAPSE(I2CPREFIX, SendStop)()
 {
 	DELAY1
 	DSDA_OUTPUT
@@ -98,7 +95,7 @@ I2CSTATICODE void I2CFNCOLLAPSE(I2CPREFIX, SendStop)()
 }
 
 // Return nonzero on failure.
-I2CSTATICODE unsigned char I2CFNCOLLAPSE(I2CPREFIX, SendByte)(unsigned char data)
+I2CCODE unsigned char I2CFNCOLLAPSE(I2CPREFIX, SendByte)(unsigned char data)
 {
     unsigned int i;
     // Assume we are in a started state (DSCL = 0 & DSDA = 0)
@@ -136,7 +133,7 @@ I2CSTATICODE unsigned char I2CFNCOLLAPSE(I2CPREFIX, SendByte)(unsigned char data
 
 #if I2CNEEDGETBYTE
 
-I2CSTATICODE unsigned char I2CFNCOLLAPSE(I2CPREFIX, GetByte)(uint8_t send_nak)
+I2CCODE unsigned char I2CFNCOLLAPSE(I2CPREFIX, GetByte)(uint8_t send_nak)
 {
     unsigned char i;
     unsigned char ret = 0;
@@ -179,7 +176,7 @@ I2CSTATICODE unsigned char I2CFNCOLLAPSE(I2CPREFIX, GetByte)(uint8_t send_nak)
 
 #if I2CNEEDSCAN
 
-I2CSTATICODE void I2CFNCOLLAPSE(I2CPREFIX, Scan)()
+I2CCODE void I2CFNCOLLAPSE(I2CPREFIX, Scan)()
 {
 	int i;
 	printf( "  " );
diff --git a/inc/extralibs/static_onewire.h b/inc/extralibs/static_onewire.h
new file mode 100644
index 0000000..d59c1b5
--- /dev/null
+++ b/inc/extralibs/static_onewire.h
@@ -0,0 +1,376 @@
+/*
+	Copyright 2025 monte-monte
+	MIT license
+
+	Generic 1-wire library for any GPIO-based system.
+
+	WiP.
+
+	Include this in your .c file only!!!
+	Include it after the following are defined:
+
+	ONEPREFIX   -> #define to be the prefix, i.e. BOB will cause BOBConfigONE
+		to be generated.
+	ONENOSTATIC -> #define if you want the functions to be generated as
+		not-static code.
+*/
+
+/* Put this in your code:
+
+	#define DELAY(n)   Delay_Us(n)
+	#define ONE_INPUT  { funPinMode(PIN_ONE, GPIO_CFGLR_IN_PUPD); funDigitalWrite(PIN_ONE, 1); } 
+	#define ONE_OUTPUT { funDigitalWrite(PIN_ONE, 0); funPinMode(PIN_ONE, GPIO_CFGLR_OUT_2Mhz_PP); } 
+	#define ONE_SET    { funDigitalWrite(PIN_ONE, 1); }
+	#define ONE_CLEAR  { funDigitalWrite(PIN_ONE, 0); }
+	#define ONE_READ   { funDigitalRead(PIN_ONE); }
+
+	#define ONENEEDSCAN       1
+	#define ONENEEDCRC8_TABLE 1
+	#define __ONE_TABLE_IN_FLASH __attribute__((section(".srodata"), used))
+
+	#include "static_onewire.h"
+*/
+
+typedef union {
+	unsigned char rom[8];
+	struct {
+		unsigned char code;
+		unsigned char serial[6];
+		unsigned char crc;
+	};
+} __attribute__((packed)) OneWire_dev_t;
+
+#define ONEDELAY_A DELAY(6)  // (3)
+#define ONEDELAY_B DELAY(64) // (55) 
+#define ONEDELAY_C DELAY(60) // (65)
+#define ONEDELAY_D DELAY(10) // (5)
+#define ONEDELAY_E DELAY(9)  // (10)
+#define ONEDELAY_F DELAY(55) // (53)
+#define ONEDELAY_G 
+#define ONEDELAY_H DELAY(480)
+#define ONEDELAY_I DELAY(70)
+#define ONEDELAY_J DELAY(410)
+
+#ifndef INTERRUPT_OFF
+#define INTERRUPT_OFF __disable_irq()
+#endif
+#ifndef INTERRUPT_ON
+#define INTERRUPT_ON __enable_irq()
+#endif
+
+#define ONEWIRE_SEARCH_ROM_CMD      0xF0
+#define ONEWIRE_COND_SEARCH_ROM_CMD 0xEC
+#define ONEWIRE_READ_ROM_CMD        0x33
+#define ONEWIRE_SELECT_CMD          0x55
+#define ONEWIRE_SKIP_CMD            0xCC
+
+#ifndef ONEPREFIX
+	#define ONEPREFIX
+#endif
+
+#ifndef ONENOSTATIC
+	#define ONESTATICODE
+#else
+	#define ONESTATICODE static
+#endif
+
+#ifndef ONEFNCOLLAPSE
+	#define INTONEFNCOLLAPSE(PFX, name) PFX##name
+	#define ONEFNCOLLAPSE(PFX, name)    INTONEFNCOLLAPSE(PFX, name)
+#endif
+
+#ifndef ONENEEDGETBYTE
+	#define ONENEEDGETBYTE 1
+#endif
+
+#ifndef __ONE_TABLE_IN_FLASH
+	#define __ONE_TABLE_IN_FLASH
+#endif
+
+#ifndef ONE_CRC_POLYNOMIAL
+	#define ONE_CRC_POLYNOMIAL 0x8C
+#endif
+
+ONESTATICODE unsigned int ONEFNCOLLAPSE(ONEPREFIX, Reset)() {
+	unsigned int state;
+	unsigned char retries = 125;
+	ONE_INPUT;
+
+	do {
+		DELAY(2);
+	} while(!ONE_READ && --retries);
+
+	INTERRUPT_OFF;
+	ONE_OUTPUT;
+	ONE_CLEAR;
+	ONEDELAY_H;
+	// ONE_SET;
+	ONE_INPUT;
+	ONEDELAY_I;
+
+	state = ONE_READ;
+	ONEDELAY_J;
+	INTERRUPT_ON;
+	// ONE_OUTPUT;
+
+	return !!state;
+}
+
+ONESTATICODE void ONEFNCOLLAPSE(ONEPREFIX, SendBit)(const unsigned char bit) {
+	ONE_OUTPUT;
+	ONE_CLEAR;
+
+	INTERRUPT_OFF;
+	if (bit) ONEDELAY_A;
+	else ONEDELAY_C;
+	// if (bit) DELAY(10);
+	// else DELAY(65);
+
+	ONE_SET;
+
+	if (bit) ONEDELAY_B;
+	else ONEDELAY_D;
+	INTERRUPT_ON;
+	// if (bit) DELAY(55);
+	// else DELAY(5);
+}
+
+ONESTATICODE void ONEFNCOLLAPSE(ONEPREFIX, SendByte)(const unsigned char data) {
+	for (unsigned char i = 1; i != 0; i <<= 1) {
+		ONEFNCOLLAPSE(ONEPREFIX, SendBit)(data & i);
+	}
+	// ONE_CLEAR; // This makes reads unreliable on my knock-offs of db18b20
+}
+
+ONESTATICODE unsigned char ONEFNCOLLAPSE(ONEPREFIX, GetBit)() {
+	unsigned char ret = 0;
+
+	INTERRUPT_OFF;
+	ONE_OUTPUT;
+	ONE_CLEAR;
+	ONEDELAY_A;
+	ONE_INPUT;
+	ONEDELAY_E;
+
+	ret = (ONE_READ);
+	ONEDELAY_F;
+	INTERRUPT_ON;
+
+	// ONE_OUTPUT;
+	return ret;
+}
+
+ONESTATICODE unsigned char ONEFNCOLLAPSE(ONEPREFIX, GetByte)() {
+	unsigned char ret = 0;
+
+	for (unsigned char i = 0; i < 8; i++) {
+		ret |= ONEFNCOLLAPSE(ONEPREFIX, GetBit)() << i;
+	}
+
+	return ret;
+}
+
+ONESTATICODE void ONEFNCOLLAPSE(ONEPREFIX, Select)(OneWire_dev_t device) {
+	ONEFNCOLLAPSE(ONEPREFIX, SendByte)(ONEWIRE_SELECT_CMD);
+
+	for (int i = 0; i < 8; i++) {
+		ONEFNCOLLAPSE(ONEPREFIX, SendByte)(device.rom[i]);
+	}
+}
+
+ONESTATICODE void ONEFNCOLLAPSE(ONEPREFIX, PowerOff)() {
+	ONE_INPUT;
+	ONE_CLEAR;
+}
+
+#if ONENEEDREADROM
+
+ONESTATICODE char ONEFNCOLLAPSE(ONEPREFIX, ReadRom)(OneWire_dev_t * dev) {
+
+	if (!dev) return -1;
+
+	uint8_t * ptr = (uint8_t*)dev;
+
+	if (ONEFNCOLLAPSE(ONEPREFIX, Reset)()) return -2;
+	ONEFNCOLLAPSE(ONEPREFIX, SendByte)(ONEWIRE_READ_ROM_CMD);
+
+	for (int i = 0; i < sizeof(OneWire_dev_t); i++) {
+		*(ptr++) = ONEFNCOLLAPSE(ONEPREFIX, GetByte)();
+	}
+
+	return 0;
+}
+#endif
+
+#if ONENEEDCRC8_TABLE
+// Dow-CRC using polynomial X^8 + X^5 + X^4 + X^0
+// Tiny 32 bytes CRC table. Copyright (C) 1992-2017 Arjen Lentz
+// http://lentz.com.au/blog/calculating-crc-with-a-tiny-32-entry-lookup-table
+__ONE_TABLE_IN_FLASH
+const unsigned char crc8_table[] = {
+	0x00, 0x5E, 0xBC, 0xE2, 0x61, 0x3F, 0xDD, 0x83,
+	0xC2, 0x9C, 0x7E, 0x20, 0xA3, 0xFD, 0x1F, 0x41,
+	0x00, 0x9D, 0x23, 0xBE, 0x46, 0xDB, 0x65, 0xF8,
+	0x8C, 0x11, 0xAF, 0x32, 0xCA, 0x57, 0xE9, 0x74
+};
+
+ONESTATICODE unsigned char ONEFNCOLLAPSE(ONEPREFIX, CRC8)(unsigned char crc, unsigned char * bytes, unsigned char size) {
+
+	while (size--) {
+		crc = *bytes++ ^ crc;  // just re-using crc as intermediate
+		crc = crc8_table[crc & 0x0f] ^ crc8_table[16 + ((crc >> 4) & 0x0f)];
+	}
+
+	return crc;
+}
+#elif ONENEEDCRC8
+// Following functions were written by M. P. Hayes for https://github.com/mph-/mmculib/
+
+// Update CRC for a single byte. 
+ONESTATICODE unsigned char ONEFNCOLLAPSE(ONEPREFIX, CRC8_byte)(unsigned char crc, unsigned char val) {
+
+	crc = crc ^ val;
+	for (int i = 0; i < 8; i++) {
+		if (crc & 0x01)
+			crc = (crc >> 1) ^ 0x8C;
+		else
+			crc >>= 1;
+	}
+
+	return crc;
+}
+
+// Update CRC for a number of bytes.
+ONESTATICODE unsigned char ONEFNCOLLAPSE(ONEPREFIX, CRC8)(unsigned char crc, unsigned char * bytes, unsigned char size) {
+
+	for (int i = 0; i < size; i++) {
+		crc = ONEFNCOLLAPSE(ONEPREFIX, CRC8_byte)(crc, bytes[i]);
+	}
+
+	return crc;
+}
+
+#endif
+
+#if ONENEEDCRC16
+// Following function is taken from crcutil.c of Public Domain 1-Wire Net Functions
+// Copyright (C) 2000 Dallas Semiconductor Corporation, All Rights Reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a
+// copy of this software and associated documentation files (the "Software"),
+// to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense,
+// and/or sell copies of the Software, and to permit persons to whom the
+// Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included
+// in all copies or substantial portions of the Software.
+
+__ONE_TABLE_IN_FLASH
+static const char oddparity[16] = { 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0 };
+
+ONESTATICODE unsigned short ONEFNCOLLAPSE(ONEPREFIX, CRC16)(unsigned short crc, unsigned char * bytes, unsigned char size) {
+	for (int i = 0 ; i < len ; i++) {
+		// Even though we're just copying a byte from the input,
+		// we'll be doing 16-bit computation with it.
+		unsigned short cdata = input[i];
+		cdata = (cdata ^ crc) & 0xff;
+		crc >>= 8;
+
+		if (oddparity[cdata & 0x0F] ^ oddparity[cdata >> 4]) crc ^= 0xC001;
+
+		cdata <<= 6;
+		crc ^= cdata;
+		cdata <<= 1;
+		crc ^= cdata;
+	}
+
+	return crc;
+}
+
+#endif
+
+#if ONENEEDSCAN
+
+// As described here https://www.analog.com/en/resources/app-notes/1wire-search-algorithm.html
+ONESTATICODE int ONEFNCOLLAPSE(ONEPREFIX, Scan)(OneWire_dev_t * dev_buffer, unsigned char size) {
+
+	int ret = 0;
+	unsigned char retries = 10;
+
+	unsigned char bit = 0;
+	unsigned char bit_comp = 0;
+	unsigned char direction = 0;
+	unsigned char discrepancy = 0;
+
+	// unsigned char family_discrepancy = 0;
+	unsigned char last_zero = 0;
+
+	unsigned char bit_n = 1;
+	unsigned char byte_mask = 1;
+	unsigned char byte_n = 0;
+
+	unsigned char device_n = 0;
+
+search_start:
+
+	if ((ret = ONEFNCOLLAPSE(ONEPREFIX, Reset)())) goto search_end;
+	ONEFNCOLLAPSE(ONEPREFIX, SendByte)(ONEWIRE_SEARCH_ROM_CMD);
+
+	do {
+		
+		bit = ONEFNCOLLAPSE(ONEPREFIX, GetBit)();
+		bit_comp = ONEFNCOLLAPSE(ONEPREFIX, GetBit)();
+
+		if (bit && bit_comp) break; // 1 1 - No more devices on the bus
+
+		if (bit == bit_comp) { // 0 0 - Discrepancy
+			if (bit_n == discrepancy) {
+				direction = 1;
+			} else {
+				if (bit_n > discrepancy) direction = 0;
+				else direction = !!(dev_buffer[device_n?device_n-1:0].rom[byte_n] & byte_mask);
+			}
+
+			if (!direction) {
+				last_zero = bit_n;
+				// if (last_zero < 9) family_discrepancy = last_zero;
+			}
+			
+		} else {
+			direction = bit;
+		}
+		
+		if (direction) dev_buffer[device_n].rom[byte_n] |= byte_mask;
+		else dev_buffer[device_n].rom[byte_n] &= ~byte_mask;
+
+		bit_n++;
+		if (!(byte_mask <<= 1)) {
+			byte_mask = 1;
+			byte_n++;
+		}
+
+		ONEFNCOLLAPSE(ONEPREFIX, SendBit)(direction);
+	} while(byte_n < 8);
+
+#if (ONENEEDCRC8) || (ONENEEDCRC8_TABLE)
+	if (dev_buffer[device_n].crc != ONEFNCOLLAPSE(ONEPREFIX, CRC8)(0, dev_buffer[device_n].rom, 7)) {
+		if (--retries) goto search_start;
+		else retries = 10;
+	}
+#endif
+
+	if ((++device_n < size) && last_zero) {
+		discrepancy = last_zero;
+		last_zero = 0;
+		bit_n = 1;
+		byte_n = 0;
+		direction = 0;
+		goto search_start;
+	}
+
+search_end:
+	if (device_n) return device_n;
+	else return 0;
+}
+#endif
\ No newline at end of file
diff --git a/inc/extralibs/usbd.c b/inc/extralibs/usbd.c
new file mode 100644
index 0000000..d8a6b8f
--- /dev/null
+++ b/inc/extralibs/usbd.c
@@ -0,0 +1,857 @@
+#include "usbd.h"
+#include "ch32fun.h"
+#include "usb_config.h"
+
+#include <stdio.h>
+
+// clang-format off
+#define ANYPRINTF	(defined( FUNCONF_USE_DEBUGPRINTF ) && FUNCONF_USE_DEBUGPRINTF) || \
+					(defined( FUNCONF_USE_UARTPRINTF ) && FUNCONF_USE_UARTPRINTF) || \
+					(defined( FUNCONF_USE_USBPRINTF ) && FUNCONF_USE_USBPRINTF)
+
+volatile uint8_t usb_debug = 0;
+// Uncomment to enable debugging
+// #define DEBUG
+#define USBD_DEBUGPRINTF( ... ) {if ( usb_debug ) { printf( __VA_ARGS__ ); }}
+#define min(a, b) (((a)<(b))?(a):(b))
+// clang-format on
+
+struct _USBState USBDCTX;
+
+void USBD_InternalFinishSetup();
+
+static inline void SetEPR_Status( const int ep, const uint16_t mask, const uint16_t value )
+{
+	const uint16_t reg = USBD->EPR[ep];
+	const uint16_t current_stat = reg & mask;
+	// Which bits we need to toggle
+	const uint16_t toggle = current_stat ^ value;
+
+	// Conserve EA, TYPE and KIND (Non-toggle)
+	uint16_t write_val = ( reg & ( USBD_EPR_EA | USBD_EPR_EP_TYPE_MASK | USBD_EPR_EP_KIND ) );
+	write_val |= toggle;
+	// Write 1 has no effect for CTR_RX and CTR_TX (0 clears)
+	write_val |= ( USBD_EPR_CTR_RX | USBD_EPR_CTR_TX );
+
+	USBD->EPR[ep] = write_val;
+}
+
+int USBDSetup( void )
+{
+	// Initialize USB pins to pull-down to avoid mis-detection
+	GPIOA->CFGHR &= ~( ( 0xf << ( 4 * 3 ) ) | ( 0xf << ( 4 * 4 ) ) );
+	GPIOA->CFGHR |= ( GPIO_Speed_2MHz | GPIO_CNF_OUT_PP ) << ( 4 * 3 ) | ( GPIO_Speed_2MHz | GPIO_CNF_OUT_PP )
+	                                                                         << ( 4 * 4 );
+	GPIOA->BSHR = ( 1 << ( 16 + 11 ) ) | ( 1 << ( 16 + 12 ) );
+
+	// We need 48MHz clock for USB
+#if FUNCONF_SYSTEM_CORE_CLOCK == 240000000
+	RCC->CFGR0 = ( RCC->CFGR0 & ~RCC_USBPRE ) | RCC_USBPRE_DIV5;
+#elif FUNCONF_SYSTEM_CORE_CLOCK == 144000000
+	RCC->CFGR0 = ( RCC->CFGR0 & ~RCC_USBPRE ) | RCC_USBPRE_DIV3;
+#elif FUNCONF_SYSTEM_CORE_CLOCK == 96000000
+	RCC->CFGR0 = ( RCC->CFGR0 & ~RCC_USBPRE ) | RCC_USBPRE_DIV2;
+#elif FUNCONF_SYSTEM_CORE_CLOCK == 48000000
+	RCC->CFGR0 = ( RCC->CFGR0 & ~RCC_USBPRE ) | RCC_USBPRE_DIV1;
+#else
+#error CH32V20x/30x need 240/144/96/48MHz main clock for USB to work
+#endif
+
+	RCC->APB2PCENR |= RCC_AFIOEN | RCC_IOPAEN;
+	RCC->AHBPCENR = RCC_AHBPeriph_SRAM;
+	RCC->APB1PCENR |= RCC_USBEN;
+
+#if defined( CH32V203F8 ) || defined( CH32V203F8U6 )
+	// Sometimes USB shares pins w/ SWD
+	// SWD must be disabled in such cases
+	Delay_Ms( 100 );
+	AFIO->PCFR1 |= AFIO_PCFR1_SWJ_CFG_DISABLE;
+#endif
+
+	// Suspend & disable all interrupts
+	USBD->CNTR = USBD_FRES;
+	USBD->CNTR = 0;
+
+	// Delay a tad (Slightly more optimized)
+	for ( volatile int i = 0; i < 1000; i++ );
+
+	// Initialize required registers & packet buffer description table
+	USBD_InternalFinishSetup();
+
+	EXTEN->EXTEN_CTR |= EXTEN_USBD_PU_EN;
+	USBD->CNTR = USBD_CTRM | USBD_RESETM | USBD_SUSPM | USBD_WKUPM;
+	USBD->ISTR = 0;
+	NVIC_EnableIRQ( USB_LP_CAN1_RX0_IRQn );
+
+	return 0;
+}
+
+WEAK void USBD_InternalFinishSetup( void )
+{
+	USBD->BTABLE = 0;
+
+	// EP0 has to be RTX for control transfers
+	USBDCTX.endpoints[0].mode = USBD_EP_MODE_RX | USBD_EP_MODE_TX;
+
+#if FUSB_EP1_MODE
+	USBDCTX.endpoints[1].mode = FUSB_EP1_MODE;
+#endif
+
+#if FUSB_EP2_MODE
+	USBDCTX.endpoints[2].mode = FUSB_EP2_MODE;
+#endif
+
+#if FUSB_EP3_MODE
+	USBDCTX.endpoints[3].mode = FUSB_EP3_MODE;
+#endif
+
+#if FUSB_EP4_MODE
+	USBDCTX.endpoints[4].mode = FUSB_EP4_MODE;
+#endif
+
+#if FUSB_EP5_MODE
+	USBDCTX.endpoints[5].mode = FUSB_EP5_MODE;
+#endif
+
+#if FUSB_EP6_MODE
+	USBDCTX.endpoints[6].mode = FUSB_EP6_MODE;
+#endif
+
+#if FUSB_EP7_MODE
+	USBDCTX.endpoints[7].mode = FUSB_EP7_MODE;
+#endif
+
+	// 64 bytes for each buffer
+	uint16_t pma = USBD_PMA_BASE; // running allocation cursor
+	for ( uint8_t ep = 0; ep < FUSB_MAX_EP_CNT; ++ep )
+	{
+		USBDCTX.usbd_ep_data_ptr[ep] = (uint32_t *)( CAN_USBD_SHARED_BASE + pma * 2 );
+
+		if ( USBDCTX.endpoints[ep].mode & USBD_EP_MODE_TX )
+		{
+			USBD_BDT->EP[ep].ADDn_TX = pma;
+			// Configured on the fly
+			USBD_BDT->EP[ep].COUNTn_TX = 0;
+
+			// If we have an RX node, we double-buffer
+			if ( !( USBDCTX.endpoints[ep].mode & USBD_EP_MODE_RX ) )
+			{
+				pma += USBD_PACKET_SIZE;
+			}
+		}
+
+		if ( USBDCTX.endpoints[ep].mode & USBD_EP_MODE_RX )
+		{
+			USBD_BDT->EP[ep].ADDn_RX = pma;
+			// USBD_BLSIZE: Each cnt is 32 bytes
+			USBD_BDT->EP[ep].COUNTn_RX = USBD_BLSIZE | ( ( USBD_PACKET_SIZE / 32 - 1 ) << 10 );
+			pma += USBD_PACKET_SIZE;
+		}
+	}
+}
+
+void USBDReset( void )
+{
+	NVIC_DisableIRQ( USB_LP_CAN1_RX0_IRQn );
+	USBD->CNTR = USBD_FRES;
+	for ( volatile int i = 0; i < 1000; i++ );
+	USBD->CNTR = USBD_CTRM | USBD_RESETM | USBD_SUSPM | USBD_WKUPM;
+	USBD->ISTR = 0;
+	// Delay 100us for the USB to fully reset
+	// Not recognized otherwise
+	Delay_Us( 100 );
+	NVIC_EnableIRQ( USB_LP_CAN1_RX0_IRQn );
+}
+
+// It seems that even WCH doesn't use the high-priority usb interrupt
+// maybe it's only for CAN?
+#if FUSB_USE_HPE
+// There is an issue with some registers apparently getting lost with HPE, just do it the slow way.
+void USB_LP_CAN1_RX0_IRQHandler( void ) __attribute__( ( section( ".text.vector_handler" ) ) )
+__attribute( ( interrupt ) );
+#else
+#if defined( FUSB_FROM_RAM ) && ( FUSB_FROM_RAM )
+void USB_LP_CAN1_RX0_IRQHandler( void ) __USBFS_FUN_ATTRIBUTE __attribute( ( interrupt ) );
+#else
+void USB_LP_CAN1_RX0_IRQHandler( void ) __attribute__( ( section( ".text.vector_handler" ) ) )
+__attribute( ( interrupt ) );
+#endif
+#endif
+
+void USB_LP_CAN1_RX0_IRQHandler( void )
+{
+	static uint8_t zeros[] = { 0, 0 }; // Macro to reply 0s
+	const uint32_t istr = USBD->ISTR;
+	int len = 0;
+
+	// Correct transfer
+	if ( istr & USBD_CTR )
+	{
+		const int ep = istr & USBD_EP_ID;
+		const int epr = USBD->EPR[ep];
+
+#ifdef USB_DEBUG
+		if ( epr & USBD_CTR_RX && epr & USBD_CTR_TX )
+		{
+			printf( "Both RX & TX flags are set! Processing is too slow\n" );
+		}
+#endif
+
+		if ( epr & USBD_CTR_RX )
+		{
+			// EP0 Setup Packet
+			if ( ep == 0 && epr & USBD_SETUP )
+			{
+				// Memory in USBD is stored in ranks of 16 bits (even though they can store
+				// 32 bits)
+				// What a weird design
+				const uint8_t USBD_request_type = USBDCTX.USBD_SetupReqType = USBDCTX.usbd_ep_data_ptr[0][0] & 0xFF;
+				const uint8_t USBD_request = USBDCTX.USBD_SetupReqCode = USBDCTX.usbd_ep_data_ptr[0][0] >> 8;
+				const uint32_t USBD_index = ( (uint32_t)USBDCTX.usbd_ep_data_ptr[0][2] );
+				const uint32_t USBD_indexValue = USBDCTX.USBD_IndexValue =
+					( ( (uint32_t)( USBDCTX.usbd_ep_data_ptr[0][2] ) ) << 16 ) | USBDCTX.usbd_ep_data_ptr[0][1];
+				const uint16_t USBD_length = USBDCTX.USBD_SetupReqLen = USBDCTX.usbd_ep_data_ptr[0][3];
+
+				// Copy to CTRL0BUFF
+				// Can be removed if CTRL0BUFF unused
+				for ( int i = 0; i < min( USBD_length, 64 ); ++i )
+				{
+					CTRL0BUFF[i] = ( USBDCTX.usbd_ep_data_ptr[0][i / 2] >> ( ( i & 1 ) << 3 ) ) & 0xFF;
+				}
+
+				// Request 0b00100001 0x0A = SET IDLE, can be ignored
+				if ( ( USBD_request_type & USBD_REQ_TYP_MASK ) == USBD_REQ_TYP_STANDARD )
+				{
+					// Standard setup
+					switch ( USBD_request )
+					{
+						case USB_GET_STATUS:
+							// Non remote wakeup and non self powered -> 0
+							USBDCTX.pCtrlPayloadPtr = zeros;
+							USBDCTX.USBD_SetupReqLen = 2;
+							break;
+						case USB_SET_ADDRESS:
+							USBDCTX.USBD_DevAddr = USBD_indexValue & 0xFF;
+							USBDCTX.USBD_SetupReqLen = USBD_SEND_ACK;
+							break;
+						case USB_GET_DESCRIPTOR:
+							const struct descriptor_list_struct *e = descriptor_list;
+							const struct descriptor_list_struct *e_end = e + DESCRIPTOR_LIST_ENTRIES;
+							for ( ; e != e_end; e++ )
+							{
+								if ( e->lIndexValue == (uint32_t)USBD_indexValue )
+								{
+									USBDCTX.pCtrlPayloadPtr = (uint8_t *)e->addr;
+									USBDCTX.USBD_SetupReqLen = min( e->length, USBD_length );
+									break;
+								}
+							}
+							if ( e == e_end ) goto stall;
+							break;
+						case USB_GET_CONFIG:
+							USBDCTX.pCtrlPayloadPtr = &USBDCTX.USBD_DevConfig;
+							USBDCTX.USBD_SetupReqLen = 1;
+							break;
+						case USB_SET_CONFIG:
+							USBDCTX.USBD_DevConfig = USBD_indexValue & 0xFF;
+							USBDCTX.USBD_DevEnumStatus = 0x01;
+							USBDCTX.USBD_SetupReqLen = USBD_SEND_ACK;
+
+							// Reset DTOG bits
+							for ( int ep = 1; ep < FUSB_MAX_EP_CNT; ep++ )
+							{
+								if ( USBD->EPR[ep] & USBD_DTOG_RX )
+								{
+									USBD->EPR[ep] = ( USBD->EPR[ep] & ( USBD_EA | USBD_EPKIND | USBD_EPTYPE ) ) |
+									                USBD_CTR_TX | USBD_CTR_RX | USBD_DTOG_RX;
+								}
+								if ( USBD->EPR[ep] & USBD_DTOG_TX )
+								{
+									USBD->EPR[ep] = ( USBD->EPR[ep] & ( USBD_EA | USBD_EPKIND | USBD_EPTYPE ) ) |
+									                USBD_CTR_TX | USBD_CTR_RX | USBD_DTOG_TX;
+								}
+							}
+							break;
+						case USB_GET_INTERFACE:
+							USBDCTX.pCtrlPayloadPtr = zeros;
+							USBDCTX.USBD_SetupReqLen = 1;
+							break;
+						case USB_CLEAR_FEATURE:
+#if FUSB_SUPPORTS_SLEEP
+							if ( ( USBD_request_type & USB_REQ_RECIP_MASK ) == USB_REQ_RECIP_DEVICE )
+							{
+								/* clear one device feature */
+								if ( (uint8_t)( USBD_indexValue & 0xFF ) == USB_REQ_FEAT_REMOTE_WAKEUP )
+								{
+									/* clear usb sleep status, device not prepare to sleep */
+									USBDCTX.USBD_DevSleepStatus &= ~0x01;
+								}
+								else
+								{
+									goto stall;
+								}
+							}
+							else
+#endif
+								if ( ( USBD_request_type & USB_REQ_RECIP_MASK ) == USB_REQ_RECIP_ENDP )
+							{
+								if ( (uint8_t)( USBD_indexValue & 0xFF ) == USB_REQ_FEAT_ENDP_HALT )
+								{
+									/* Clear End-point Feature */
+									if ( USBDCTX.endpoints[ep].mode )
+									{
+										if ( USBD_index & DEF_UEP_IN &&
+											 ( USBDCTX.endpoints[ep].mode & USBD_EP_MODE_TX ) )
+											SetEPR_Status( ep, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_NAK );
+										else if ( USBD_index & DEF_UEP_OUT &&
+												  ( USBDCTX.endpoints[ep].mode & USBD_EP_MODE_RX ) )
+											SetEPR_Status( ep, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_ACK );
+										else
+											goto stall;
+									}
+									else
+									{
+										goto stall;
+									}
+								}
+								else
+								{
+									goto stall;
+								}
+							}
+							else
+							{
+								goto stall;
+							}
+							break;
+						case USB_SET_FEATURE:
+							if ( ( USBD_request_type & USB_REQ_RECIP_MASK ) == USB_REQ_RECIP_DEVICE )
+							{
+#if FUSB_SUPPORTS_SLEEP
+								/* Set Device Feature */
+								if ( (uint8_t)( USBD_indexValue & 0xFF ) == USB_REQ_FEAT_REMOTE_WAKEUP )
+								{
+									/* Set Wake-up flag, device prepare to sleep */
+									USBD_DevSleepStatus |= 0x01;
+								}
+								else
+#endif
+								{
+									goto stall;
+								}
+							}
+							else if ( ( USBD_request_type & USB_REQ_RECIP_MASK ) == USB_REQ_RECIP_ENDP )
+							{
+								/* Set Endpoint Feature */
+								if ( (uint8_t)( USBD_indexValue & 0xFF ) == USB_REQ_FEAT_ENDP_HALT )
+								{
+									if ( USBDCTX.endpoints[ep].mode )
+									{
+										if ( ( USBD_index & DEF_UEP_IN ) &&
+											 ( USBDCTX.endpoints[ep].mode & USBD_EP_MODE_TX ) )
+											SetEPR_Status( ep, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_STALL );
+										else if ( ( USBD_index & DEF_UEP_OUT ) &&
+												  ( USBDCTX.endpoints[ep].mode & USBD_EP_MODE_RX ) )
+											SetEPR_Status( ep, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_STALL );
+										else
+											goto stall;
+									}
+								}
+								else
+									goto stall;
+							}
+							else
+								goto stall;
+							break;
+
+						default: goto stall;
+					}
+				}
+				else
+				{
+					// EP0 Setup non-standard
+#if FUSB_HID_INTERFACES > 0 || FUSB_USER_HANDLERS
+#if FUSB_HID_USER_REPORTS
+					const uint16_t packet_len = USBD_BDT->EP[ep].COUNTn_RX & 0x3FF;
+					uint8_t buff[USBD_PACKET_SIZE];
+
+					for ( int i = 0; i < packet_len; ++i )
+					{
+						buff[i] = ( USBDCTX.usbd_ep_data_ptr[ep][i / 2] >> ( ( i & 1 ) << 3 ) ) & 0xFF;
+					}
+#endif
+
+					len = 0;
+					switch ( USBD_request )
+					{
+#if FUSB_HID_INTERFACES > 0
+						case HID_SET_REPORT:
+#if FUSB_HID_USER_REPORTS
+							len = HandleHidUserSetReportSetup( &USBDCTX, (tusb_control_request_t *)buff );
+							if ( len < 0 ) goto stall;
+							USBDCTX.USBD_SetupReqLen = 0;
+							break;
+
+						case HID_GET_REPORT:
+							len = HandleHidUserGetReportSetup( &USBDCTX, (tusb_control_request_t *)buff );
+							if ( len < 0 ) goto stall;
+							if ( len == 0 )
+							{
+								USBDCTX.USBD_SetupReqLen = USBD_SEND_ACK;
+								break;
+							}
+
+							// len > 0:
+							// We would like to send something it seems
+							USBDCTX.USBD_SetupReqLen = len;
+							len = min( USBD_PACKET_SIZE, len );
+
+							// If HandleHidUserGetReportSetup hasn't provided data to send,
+							// we ask HandleHidUserReportDataIn for data
+							if ( !USBDCTX.pCtrlPayloadPtr )
+							{
+								len = HandleHidUserReportDataIn( &USBDCTX, buff, len );
+								USBDCTX.pCtrlPayloadPtr = buff;
+							}
+
+#endif
+							break;
+						case HID_SET_IDLE:
+							if ( USBD_index < FUSB_HID_INTERFACES )
+								USBDCTX.USBD_HidIdle[USBD_index] = (uint8_t)( USBD_indexValue >> 8 );
+
+							// ACK
+							USBDCTX.USBD_SetupReqLen = USBD_SEND_ACK;
+							break;
+						case HID_SET_PROTOCOL:
+							if ( USBD_index < FUSB_HID_INTERFACES )
+								USBDCTX.USBD_HidProtocol[USBD_index] = (uint8_t)USBD_indexValue;
+							USBDCTX.USBD_SetupReqLen = USBD_SEND_ACK;
+							break;
+						case HID_GET_IDLE:
+							if ( USBD_index < FUSB_HID_INTERFACES )
+							{
+								USBDCTX.pCtrlPayloadPtr = &USBDCTX.USBD_HidIdle[USBD_index];
+								USBDCTX.USBD_SetupReqLen = 1;
+							}
+							break;
+
+						case HID_GET_PROTOCOL:
+							if ( USBD_index < FUSB_HID_INTERFACES )
+							{
+								USBDCTX.pCtrlPayloadPtr = &USBDCTX.USBD_HidProtocol[USBD_index];
+								USBDCTX.USBD_SetupReqLen = 1;
+							}
+							break;
+#endif
+						default:
+#if FUSB_USER_HANDLERS
+							len = HandleSetupCustom( &USBDCTX, USBD_request );
+
+							if ( len )
+							{
+								if ( len < 0 )
+								{
+									// ACK
+									USBDCTX.USBD_SetupReqLen = USBD_SEND_ACK;
+								}
+								else
+								{
+									USBDCTX.USBD_SetupReqLen = min( len, USBD_length );
+								}
+							}
+							else
+#endif
+							{
+								goto stall;
+							}
+							break;
+					}
+#endif
+				}
+			}
+			else
+			{
+				// OUT RX packet
+				const uint16_t packet_len = USBD_BDT->EP[ep].COUNTn_RX & 0x3FF;
+
+#if FUSB_HID_USER_REPORTS || FUSB_USER_HANDLERS
+				uint8_t buff[USBD_PACKET_SIZE];
+
+				for ( int i = 0; i < packet_len; ++i )
+				{
+					buff[i] = ( USBDCTX.usbd_ep_data_ptr[ep][i / 2] >> ( ( i & 1 ) << 3 ) ) & 0xFF;
+				}
+#endif
+#if FUSB_HID_USER_REPORTS
+				if ( USBDCTX.USBD_SetupReqCode == HID_SET_REPORT )
+				{
+					HandleHidUserReportDataOut( &USBDCTX, buff, len );
+				}
+#endif
+#if FUSB_USER_HANDLERS
+				if ( USBDCTX.USBD_SetupReqCode != HID_SET_REPORT )
+				{
+					HandleDataOut( &USBDCTX, ep, buff, packet_len );
+				}
+#endif
+#if FUSB_HID_USER_REPORTS
+				// Transaction finished
+				if ( USBDCTX.USBD_SetupReqLen == 0 )
+				{
+					if ( USBDCTX.USBD_SetupReqCode == HID_SET_REPORT ) HandleHidUserReportOutComplete( &USBDCTX );
+				}
+#endif
+
+				if ( ep == 0 && ( USBDCTX.USBD_SetupReqLen == 0 || packet_len != 0 ) )
+				{
+					USBDCTX.USBD_SetupReqLen = USBD_SEND_ACK;
+				}
+
+				SetEPR_Status( ep, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_ACK );
+			}
+
+			// Clear RX (Toggle, 1 conserves bit)
+			USBD->EPR[ep] = ( USBD->EPR[ep] & ( USBD_EA | USBD_EPKIND | USBD_EPTYPE ) ) | USBD_CTR_TX;
+		}
+
+		// length < sizeof -> Return start
+		// length = sizeof -> Return all
+		// length > sizeof -> Return partial then 0
+
+		// 1st load for RX and auto-reload for TX
+		// We need to send something:
+		// - Set tx_buf and tx_pending to non-null and set TX to ACK
+		// - This piece of code shall handle loading to buffer
+		// - TX part shall start next transaction if we need to
+		// TX detects if the operation should be continued by checking
+		// if the tx_buf is null
+
+		// We are always handling either a TX or a RX transaction
+		// This is only for the control endpoint
+		if ( ep == 0 )
+		{
+			if ( USBDCTX.USBD_SetupReqLen == 0 )
+			{
+				USBDCTX.pCtrlPayloadPtr = NULL;
+				// Enable recieving data
+				SetEPR_Status( 0, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_ACK );
+			}
+			else
+			{
+				// USBD_SEND_ACK is special variable to say 0 byte tx
+				if ( USBDCTX.USBD_SetupReqLen == USBD_SEND_ACK )
+				{
+					USBDCTX.USBD_SetupReqLen = 0;
+				}
+
+				const uint16_t tx_len = min( USBDCTX.USBD_SetupReqLen, USBD_PACKET_SIZE );
+				// Overreads but eh
+				for ( int i = 0; i < tx_len; i += 2 )
+				{
+					USBDCTX.usbd_ep_data_ptr[0][i / 2] = *( (const uint16_t *)( USBDCTX.pCtrlPayloadPtr + i ) );
+				}
+				USBD_BDT->EP[0].COUNTn_TX = tx_len;
+				USBDCTX.pCtrlPayloadPtr += tx_len;
+				USBDCTX.USBD_SetupReqLen -= tx_len;
+				SetEPR_Status( 0, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_ACK );
+			}
+		}
+
+		// IN request
+		if ( epr & USBD_CTR_TX )
+		{
+			if ( ep == 0 )
+			{
+				if ( USBDCTX.USBD_SetupReqCode == USB_SET_ADDRESS )
+				{
+					USBD->DADDR = 0x80 | USBDCTX.USBD_DevAddr;
+					USBDCTX.USBD_DevAddr = 0;
+				}
+			}
+			else
+			{
+				// Send data for non-zero endpoint
+#if FUSB_USER_HANDLERS
+				len = HandleInRequest( &USBDCTX, ep, USBDCTX.endpoints[ep].in, 0 );
+#endif
+
+				if ( len )
+				{
+					if ( len < 0 ) len = 0;
+					SetEPR_Status( ep, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_ACK );
+				}
+				else
+				{
+					SetEPR_Status( ep, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_NAK );
+				}
+			}
+
+			// Sent, no longer busy
+			USBDCTX.endpoints[ep].busy = 0;
+
+			// Clear TX (Toggle)
+			USBD->EPR[ep] = ( USBD->EPR[ep] & ( USBD_EA | USBD_EPKIND | USBD_EPTYPE ) ) | USBD_CTR_RX;
+		}
+
+		USBD->ISTR = ~USBD_CTR;
+		return;
+
+	stall:
+		SetEPR_Status( 0, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_STALL );
+		SetEPR_Status( 0, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_STALL );
+		USBD->EPR[ep] = ( epr & ( USBD_EPR_EA | USBD_EPR_EP_TYPE_MASK | USBD_EPR_EP_KIND ) );
+	}
+
+	if ( istr & USBD_RESET )
+	{
+		USBD->ISTR = ~USBD_RESET;
+		USBD->BTABLE = 0;
+
+		for ( uint8_t ep = 0; ep < FUSB_MAX_EP_CNT; ++ep )
+		{
+			USBD->EPR[ep] = ep;
+
+			// TODO: Add method to set EP type
+			// Currently the user must override the function
+			// It seems that using TYPE_BULK is fine for all types though
+			SetEPR_Status( ep, USBD_EPR_EP_TYPE_MASK, USBD_EPR_EP_TYPE_BULK );
+			SetEPR_Status( ep, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_ACK );
+			SetEPR_Status( ep, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_NAK );
+
+			// Disable unused modes
+			if ( !( USBDCTX.endpoints[ep].mode & USBD_EP_MODE_RX ) )
+			{
+				SetEPR_Status( ep, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_DIS );
+			}
+
+			if ( !( USBDCTX.endpoints[ep].mode & USBD_EP_MODE_TX ) )
+			{
+				SetEPR_Status( ep, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_DIS );
+			}
+			USBDCTX.endpoints[ep].busy = 0;
+		}
+
+		// Override EP0 to control - mandatory
+		SetEPR_Status( 0, USBD_EPR_EP_TYPE_MASK, USBD_EPR_EP_TYPE_CTRL );
+
+		// Reset context
+		USBDCTX.USBD_DevConfig = 0;
+		USBDCTX.USBD_DevAddr = 0;
+		USBDCTX.USBD_DevSleepStatus = 0;
+		USBDCTX.USBD_DevEnumStatus = 0;
+
+		// Enable USB function (we default to address 0)
+		USBD->DADDR = USBD_EF;
+		return;
+	}
+
+	// If size is a concern, merge this with the ISTR set bellow
+	if ( istr & ( USBD_ERR | USBD_PMAOVR ) )
+	{
+#ifdef DEBUG
+		printf( "ERROR! USB error\n" );
+#endif
+		USBD->ISTR = ~( USBD_ERR | USBD_PMAOVR );
+		return;
+	}
+
+	// We don't need the rest of the events, so we just default to resetting them
+	USBD->ISTR = ~( USBD_ESOF | USBD_SOF | USBD_WKUP | USBD_SUSP );
+}
+
+static uint8_t EP_buffer[USBD_PACKET_SIZE];
+
+uint8_t *USBD_GetEPBufferIfAvailable( int endp )
+{
+	if ( USBDCTX.endpoints[endp].busy ) return 0;
+	return EP_buffer;
+}
+
+int USBD_SendEndpoint( int endp, int len )
+{
+	if ( USBDCTX.endpoints[endp].busy ) return -1;
+	if ( USBDCTX.USBD_SetupReqLen > 0 ) return -2;
+
+	for ( int i = 0; i < len; i += 2 )
+	{
+		USBDCTX.usbd_ep_data_ptr[endp][i / 2] = *( (const uint16_t *)( EP_buffer + i ) );
+	}
+
+	USBD_BDT->EP[endp].COUNTn_TX = len;
+	USBDCTX.endpoints[endp].busy = 1;
+	SetEPR_Status( endp, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_ACK );
+
+	return 0;
+}
+
+int USBD_SendEndpointNEW( int endp, uint8_t *data, int len, int copy )
+{
+	if ( USBDCTX.endpoints[endp].busy ) return -1;
+	// This prevents sending while ep0 is receiving
+	if ( USBDCTX.USBD_SetupReqLen > 0 ) return USBDCTX.USBD_SetupReqLen;
+
+	// For FS compatability
+	USBDCTX.endpoints[endp].in = data;
+
+	for ( int i = 0; i + 1 < len; i += 2 )
+	{
+		USBDCTX.usbd_ep_data_ptr[endp][i / 2] = data[i] | ( (uint16_t)data[i + 1] << 8 );
+	}
+	if ( len % 2 != 0 )
+	{
+		USBDCTX.usbd_ep_data_ptr[endp][len / 2] = data[len - 1];
+	}
+
+	USBD_BDT->EP[endp].COUNTn_TX = len;
+	USBDCTX.endpoints[endp].busy = 1;
+	SetEPR_Status( endp, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_ACK );
+
+	return 0;
+}
+
+int USBD_SendACK( int endp, int tx )
+{
+	if ( tx )
+	{
+		SetEPR_Status( endp, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_ACK );
+	}
+	else
+	{
+		SetEPR_Status( endp, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_ACK );
+	}
+
+	return 0;
+}
+
+int USBD_SendNAK( int endp, int tx )
+{
+	if ( tx )
+	{
+		SetEPR_Status( endp, USBD_EPR_STAT_TX_MASK, USBD_EPR_STAT_TX_NAK );
+	}
+	else
+	{
+		SetEPR_Status( endp, USBD_EPR_STAT_RX_MASK, USBD_EPR_STAT_RX_NAK );
+	}
+
+	return 0;
+}
+
+#if defined( FUNCONF_USE_USBPRINTF ) && FUNCONF_USE_USBPRINTF
+uint8_t usb_inputbuffer[USBD_PACKET_SIZE];
+int usb_inbuf_idx;
+
+WEAK int HandleInRequest( struct _USBState *ctx, int endp, uint8_t *data, int len )
+{
+	return 0;
+}
+
+WEAK void HandleDataOut( struct _USBState *ctx, int endp, uint8_t *data, int len )
+{
+	if ( endp == 0 )
+	{
+		ctx->USBD_SetupReqLen = 0; // To ACK
+	}
+	else if ( endp == 2 )
+	{
+		// discard oldest if polling is too slow
+		int headroom = ( sizeof( usb_inputbuffer ) - usb_inbuf_idx ) - len;
+		if ( headroom < 0 )
+		{
+			// not enough space left, free up some
+			int offset = -headroom;
+			for ( int i = offset; i < sizeof( usb_inputbuffer ); i++ )
+			{
+				usb_inputbuffer[i - offset] = usb_inputbuffer[i];
+			}
+			usb_inbuf_idx -= offset;
+		}
+
+		for ( int i = 0; i < len; i++ )
+		{
+			usb_inputbuffer[usb_inbuf_idx++] = data[i];
+		}
+	}
+}
+
+void poll_input()
+{
+	if ( usb_inbuf_idx )
+	{
+		HandleUSBInput( usb_inbuf_idx, usb_inputbuffer );
+		usb_inbuf_idx = 0;
+	}
+}
+
+WEAK int HandleSetupCustom( struct _USBState *ctx, int setup_code )
+{
+	int ret = -1;
+	if ( ctx->USBD_SetupReqType & USB_REQ_TYP_CLASS )
+	{
+		switch ( setup_code )
+		{
+			case CDC_SET_LINE_CODING:
+			case CDC_SET_LINE_CTLSTE:
+			case CDC_SEND_BREAK: ret = ( ctx->USBD_SetupReqLen ) ? ctx->USBD_SetupReqLen : -1; break;
+			case CDC_GET_LINE_CODING: ret = ctx->USBD_SetupReqLen; break;
+			default: ret = 0; break;
+		}
+	}
+	else
+	{
+		ret = 0; // Go to STALL
+	}
+
+	return ret;
+}
+#endif
+
+#if defined( FUNCONF_USE_USBPRINTF ) && FUNCONF_USE_USBPRINTF
+int USBFS_SendEndpointNEW( int endp, uint8_t *data, int len, int copy )
+{
+	return USBD_SendEndpointNEW( endp, data, len, copy );
+}
+#endif
+
+#if __STDC_VERSION__ >= 201112L || defined( __GNUC__ ) || defined( __clang__ )
+
+// Very hacky way to ensure that we don't get a BDT overflow
+// I miss C++...
+// This should more logically be in usbd.h, but we get multiple objects
+// if we do that
+//
+// No checking on compilers w/o static assert :(
+static const char _number_of_endpoints[] = {
+#if FUSB_EP1_MODE
+	FUSB_EP1_MODE,
+#endif
+#if FUSB_EP2_MODE
+	FUSB_EP2_MODE,
+#endif
+#if FUSB_EP3_MODE
+	FUSB_EP3_MODE,
+#endif
+#if FUSB_EP4_MODE
+	FUSB_EP4_MODE,
+#endif
+#if FUSB_EP5_MODE
+	FUSB_EP5_MODE,
+#endif
+#if FUSB_EP6_MODE
+	FUSB_EP6_MODE,
+#endif
+#if FUSB_EP7_MODE
+	FUSB_EP7_MODE,
+#endif
+};
+
+#if __STDC_VERSION__ >= 202311L
+static_assert( ( sizeof( _number_of_endpoints ) * USBD_PACKET_SIZE + USBD_PMA_BASE ) <= 512,
+	"USBD BDT overflow! Please use less endpoints, or make usbd packet size smaller" );
+#elif defined( __GNUC__ ) || defined( __clang__ ) || __STDC_VERSION__ >= 201112L
+_Static_assert( ( sizeof( _number_of_endpoints ) * USBD_PACKET_SIZE + USBD_PMA_BASE ) <= 512,
+	"USBD BDT overflow! Please use less endpoints, or make usbd packet size smaller" );
+#endif
+
+#endif
diff --git a/inc/extralibs/usbd.h b/inc/extralibs/usbd.h
new file mode 100644
index 0000000..f9c19c8
--- /dev/null
+++ b/inc/extralibs/usbd.h
@@ -0,0 +1,187 @@
+#ifndef USBD_H
+#define USBD_H
+
+#include "ch32fun.h"
+#include "usb_config.h"
+#include "usb_defines.h"
+#include <stdint.h>
+
+#if defined(FUSB_FROM_RAM) && (FUSB_FROM_RAM)
+#define __USBD_FUN_ATTRIBUTE __attribute__((section( ".srodata" ), used))
+#else
+#define __USBD_FUN_ATTRIBUTE
+#endif
+
+// 0x400 of shared memory
+// Up to 8 endpoints (0x10 each)
+
+// Buffer Description Table
+typedef struct __attribute__((__packed__))
+{
+	USBD_BTABLE_TypeDef EP[8];
+} USBD_BDT_TypeDef;
+#define USBD_BDT ( (USBD_BDT_TypeDef *)CAN_USBD_SHARED_BASE )
+
+// Packet buffer (0x400 - 0x80 = 0x380) left
+#define USBD_PMA_BASE 0x80 // Offset from USBD_BDT_BASE
+#define USBD_PACKET_SIZE 64
+
+#define USBD_EPR_CTR_RX 0x8000
+#define USBD_EPR_DTOG_RX 0x4000
+#define USBD_EPR_STAT_RX_MASK 0x3000
+#define USBD_EPR_STAT_RX_DIS 0x0000
+#define USBD_EPR_STAT_RX_STALL 0x1000
+#define USBD_EPR_STAT_RX_NAK 0x2000
+#define USBD_EPR_STAT_RX_ACK 0x3000
+#define USBD_EPR_SETUP 0x0800
+#define USBD_EPR_EP_TYPE_MASK 0x0600
+#define USBD_EPR_EP_TYPE_BULK 0x0000
+#define USBD_EPR_EP_TYPE_CTRL 0x0200
+#define USBD_EPR_EP_TYPE_ISO 0x0400
+#define USBD_EPR_EP_TYPE_INT 0x0600
+#define USBD_EPR_EP_KIND 0x0100
+#define USBD_EPR_CTR_TX 0x0080
+#define USBD_EPR_DTOG_TX 0x0040
+#define USBD_EPR_STAT_TX_MASK 0x0030
+#define USBD_EPR_STAT_TX_DIS 0x0000
+#define USBD_EPR_STAT_TX_STALL 0x0010
+#define USBD_EPR_STAT_TX_NAK 0x0020
+#define USBD_EPR_STAT_TX_ACK 0x0030
+#define USBD_EPR_EA 0x000F
+
+#define USB_GET_STATUS 0x00
+#define USB_SET_ADDRESS 0x05
+#define USB_GET_DESCRIPTOR 0x06
+#define USB_GET_CONFIG 0x08
+#define USB_SET_CONFIG 0x09
+#define USB_GET_INTERFACE 0x0A
+
+#define USB_CLEAR_FEATURE 0x01
+#define USB_SET_FEATURE 0x03
+#define USB_SET_DESCRIPTOR 0x07
+#define USB_SET_INTERFACE 0x0B
+
+#define USBD_DEVICE_DESCRIPTOR 0x01
+#define USBD_CONFIG_DESCRIPTOR 0x02
+#define USBD_HID_DESCRIPTOR 0x22
+#define USBD_STRING_DESCRIPTOR 0x03
+
+#define USBD_REQ_TYP_MASK 0b01100000
+#define USBD_REQ_TYP_STANDARD 0b00000000
+#define USBD_REQ_TYP_CLASS 0b00100000
+#define USBD_REQ_TYP_VENDOR 0b01000000
+
+#define USBD_EP_MODE_TX 4
+#define USBD_EP_MODE_RX 8
+#define USBD_EP_MODE_BDIR 12
+
+// Magic value for ack
+#define USBD_SEND_ACK (0xFFFF)
+
+#ifndef USBD_SILENCE_EP_MODE
+#ifdef USBFS_EP_MODE_RX
+#warning USBFS_EP_MODE_RX "USBD should use USBD_EP_MODE_RX instead of USBFS_EP_MODE_RX unless also using USBFS (silence by defining USBD_SILENCE_EP_MODE)"
+#endif
+#ifdef USBFS_EP_MODE_TX
+#warning USBFS_EP_MODE_TX "USBD should use USBD_EP_MODE_TX instead of USBFS_EP_MODE_TX unless also using USBFS (silence by defining USBD_SILENCE_EP_MODE)"
+#endif
+#endif
+
+struct _USBState;
+
+int USBDSetup( void );
+void USBDReset( void );
+uint8_t *USBD_GetEPBufferIfAvailable( int endp ); // Only 1 buffer might be active at a time
+int USBD_SendEndpoint( int endp, int len );
+int USBD_SendEndpointNEW( int endp, uint8_t *data, int len, int copy ); // Copy is ignored
+int USBD_SendACK( int endp, int tx );
+int USBD_SendNAK( int endp, int tx );
+
+#if FUSB_HID_USER_REPORTS
+int HandleHidUserGetReportSetup( struct _USBState *ctx, tusb_control_request_t *req );
+int HandleHidUserSetReportSetup( struct _USBState *ctx, tusb_control_request_t *req );
+void HandleHidUserReportDataOut( struct _USBState *ctx, uint8_t *data, int len );
+int HandleHidUserReportDataIn( struct _USBState *ctx, uint8_t *data, int len );
+void HandleHidUserReportOutComplete( struct _USBState *ctx );
+#endif
+
+#if FUSB_USER_HANDLERS
+int HandleInRequest( struct _USBState *ctx, int endp, uint8_t *data, int len );
+void HandleDataOut( struct _USBState *ctx, int endp, uint8_t *data, int len );
+int HandleSetupCustom( struct _USBState *ctx, int setup_code );
+#endif
+
+#if FUNCONF_USE_USBPRINTF
+void HandleUSBInput( int numbytes, uint8_t *data );
+extern uint8_t usb_inputbuffer[USBD_PACKET_SIZE];
+extern int usb_inbuf_idx;
+#endif
+
+#ifndef FUSB_MAX_EP_CNT
+#define FUSB_MAX_EP_CNT 8
+#endif
+
+#if !defined( FUSB_BUFFERS_NUMBER ) || FUSB_BUFFERS_NUMBER == 0
+#define FUSB_BUFFERS_NUMBER 1
+#undef FUSB_EP1_MODE
+#define FUSB_EP1_MODE 0
+#undef FUSB_EP2_MODE
+#define FUSB_EP2_MODE 0
+#undef FUSB_EP3_MODE
+#define FUSB_EP3_MODE 0
+#undef FUSB_EP4_MODE
+#define FUSB_EP4_MODE 0
+#undef FUSB_EP5_MODE
+#define FUSB_EP5_MODE 0
+#undef FUSB_EP6_MODE
+#define FUSB_EP6_MODE 0
+#undef FUSB_EP7_MODE
+#define FUSB_EP7_MODE 0
+#warning "You may have forgotten to properly define used EPs in usb_config.h"
+#endif
+
+struct _USBState
+{
+	// Setup Request
+	uint8_t USBD_SetupReqCode;
+	uint8_t USBD_SetupReqType;
+	uint16_t USBD_SetupReqLen; // Used for tracking place along send.
+	uint32_t USBD_IndexValue;
+
+	// USB Device Status
+	uint8_t USBD_DevConfig;
+	uint8_t USBD_DevAddr;
+	uint8_t USBD_DevSleepStatus;
+	uint8_t USBD_DevEnumStatus;
+
+	uint8_t *pCtrlPayloadPtr;
+
+	uint32_t *usbd_ep_data_ptr[FUSB_MAX_EP_CNT];
+
+	struct
+	{
+		union
+		{
+			uint8_t *in;
+			uint8_t *tx;
+		};
+		uint8_t mode;
+		uint8_t busy;
+	} endpoints[FUSB_MAX_EP_CNT];
+
+#if FUSB_HID_INTERFACES > 0
+	uint8_t USBD_HidIdle[FUSB_HID_INTERFACES];
+	uint8_t USBD_HidProtocol[FUSB_HID_INTERFACES];
+#endif
+
+	// We don't really need buffers for other endpoints
+	// We waste 64 bytes to prevent overflows (why are overflows happening??)
+	// This was probably fine in other demos
+	// But here we only have 1 buffer
+	uint8_t ep0buf[2][USBD_PACKET_SIZE];
+#define CTRL0BUFF ( USBDCTX.ep0buf[0] )
+};
+
+extern struct _USBState USBDCTX;
+
+#endif // USBD_H