diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2a90985 --- /dev/null +++ b/.gitignore @@ -0,0 +1,14 @@ +*.[oa] +.*.d +*~ +*.ld +*.elf +*.bin +*.hex +*.orig +*.rej +*.rld +*.upd +*.adf +*.scp +Greaseweazle_* diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..182d822 --- /dev/null +++ b/Makefile @@ -0,0 +1,45 @@ + +PROJ = Greaseweazle +VER = v0.0.1a + +SUBDIRS += src + +.PHONY: all clean dist mrproper flash start serial + +ifneq ($(RULES_MK),y) +export ROOT := $(CURDIR) +all: + $(MAKE) -C src -f $(ROOT)/Rules.mk $(PROJ).elf $(PROJ).bin $(PROJ).hex + cp -a src/$(PROJ).hex $(PROJ)-$(VER).hex + +clean: + $(MAKE) -f $(ROOT)/Rules.mk $@ + +dist: + rm -rf $(PROJ)_* + mkdir -p $(PROJ)_$(VER)/reloader + $(MAKE) clean + $(MAKE) all + cp -a $(PROJ)-$(VER).hex $(PROJ)_$(VER)/ + $(MAKE) clean + cp -a COPYING $(PROJ)_$(VER)/ + cp -a README.md $(PROJ)_$(VER)/ +# cp -a RELEASE_NOTES $(PROJ)_$(VER)/ + zip -r $(PROJ)_$(VER).zip $(PROJ)_$(VER) + +mrproper: clean + rm -rf $(PROJ)_* + +endif + +BAUD=115200 +DEV=/dev/ttyUSB0 + +flash: all + sudo stm32flash -b $(BAUD) -w src/$(PROJ).hex $(DEV) + +start: + sudo stm32flash -b $(BAUD) -g 0 $(DEV) + +serial: + sudo miniterm.py $(DEV) 3000000 diff --git a/Rules.mk b/Rules.mk new file mode 100644 index 0000000..0d9cc10 --- /dev/null +++ b/Rules.mk @@ -0,0 +1,91 @@ +TOOL_PREFIX = arm-none-eabi- +CC = $(TOOL_PREFIX)gcc +OBJCOPY = $(TOOL_PREFIX)objcopy +LD = $(TOOL_PREFIX)ld + +ifneq ($(VERBOSE),1) +TOOL_PREFIX := @$(TOOL_PREFIX) +endif + +FLAGS = -g -Os -nostdlib -std=gnu99 -iquote $(ROOT)/inc +FLAGS += -Wall -Werror -Wno-format -Wdeclaration-after-statement +FLAGS += -Wstrict-prototypes -Wredundant-decls -Wnested-externs +FLAGS += -fno-common -fno-exceptions -fno-strict-aliasing +FLAGS += -mlittle-endian -mthumb -mcpu=cortex-m3 -mfloat-abi=soft +FLAGS += -Wno-unused-value + +ifneq ($(debug),y) +FLAGS += -DNDEBUG +endif + +FLAGS += -MMD -MF .$(@F).d +DEPS = .*.d + +FLAGS += $(FLAGS-y) + +CFLAGS += $(CFLAGS-y) $(FLAGS) -include decls.h +AFLAGS += $(AFLAGS-y) $(FLAGS) -D__ASSEMBLY__ +LDFLAGS += $(LDFLAGS-y) $(FLAGS) -Wl,--gc-sections + +RULES_MK := y + +include Makefile + +SUBDIRS += $(SUBDIRS-y) +OBJS += $(OBJS-y) $(patsubst %,%/build.o,$(SUBDIRS)) + +# Force execution of pattern rules (for which PHONY cannot be directly used). +.PHONY: FORCE +FORCE: + +.PHONY: clean + +.SECONDARY: + +build.o: $(OBJS) + $(LD) -r -o $@ $^ + +%/build.o: FORCE + $(MAKE) -f $(ROOT)/Rules.mk -C $* build.o + +%.o: %.c Makefile + @echo CC $@ + $(CC) $(CFLAGS) -c $< -o $@ + +%.o: %.S Makefile + @echo AS $@ + $(CC) $(AFLAGS) -c $< -o $@ + +%.ld: %.ld.S Makefile + @echo CPP $@ + $(CC) -P -E $(AFLAGS) $< -o $@ + +%.elf: $(OBJS) %.ld Makefile + @echo LD $@ + $(CC) $(LDFLAGS) -T$(*F).ld $(OBJS) -o $@ + chmod a-x $@ + +%.hex: %.elf + @echo OBJCOPY $@ + $(OBJCOPY) -O ihex $< $@ + chmod a-x $@ + +%.bin: %.elf + @echo OBJCOPY $@ + $(OBJCOPY) -O binary $< $@ + chmod a-x $@ + +%.o: $(RPATH)/%.c Makefile + @echo CC $@ + $(CC) $(CFLAGS) -c $< -o $@ + +%.o: $(RPATH)/%.S Makefile + @echo AS $@ + $(CC) $(AFLAGS) -c $< -o $@ + +clean:: $(addprefix _clean_,$(SUBDIRS) $(SUBDIRS-n) $(SUBDIRS-)) + rm -f *~ *.o *.elf *.hex *.bin *.ld $(DEPS) +_clean_%: FORCE + $(MAKE) -f $(ROOT)/Rules.mk -C $* clean + +-include $(DEPS) diff --git a/firmware/doc/pins.txt b/attic/pins.txt similarity index 62% rename from firmware/doc/pins.txt rename to attic/pins.txt index 0d7f35d..85bcf09 100644 --- a/firmware/doc/pins.txt +++ b/attic/pins.txt @@ -10,32 +10,32 @@ USB: PA11 USB_DM PA12 USB_DP GPIO: (GPI_float, GPO_pushpull) - PB0 (connect via 1.5kohm to PA12/USB_DP) + PA0 (connect via 1.5kohm to PA12/USB_DP) Floppy: ------- -GPIn: (GPI_float, active low, 5v tolerant) - PB13 8: IDX - PB4 26: TRK0 +GPIn: (GPI_float, active low) + PB6 8: IDX + PB7 26: TRK0 PB8 28: WRPROT -GPOut: (GPO_pushpull, active high) - PA7 2: DENSEL - PA6 12: SEL_A - PA5 16: MTR_A - PA4 18: DIR - PA3 20: STEP - PA1 24: DKWE - PA0 32: SIDE -TimerIn: (GPI_float, active low, 5v tolerant) - PB6 30: DKRD (Timer4/1, CC1:DMA1/1) -TimerOut: (AFO_pushpull, active high) - PA2 22: DKWD (Timer2/3, UP:DMA1/2) +GPOut: (GPO_opendrain or GPO_pushpull) + PB9 2: DENSEL + PB10 12: SEL_A + PB11 16: MTR_A + PB12 18: DIR + PB13 20: STEP + PB14 24: DKWE + PB15 32: SIDE +TimerIn: (GPI_float, active low) + PB3 30: DKRD (Timer2/2, CC2:DMA1/7) +TimerOut: (AFO_opendrain or AFO_pushpull) + PB4 22: DKWD (Timer3/1, UP:DMA1/3) Unused Pins: ------------ All to be pulled high: - PA8,13-15 - PB1-3,5,7,9-12,14-15 + PA1-8,13-15 + PB0-2,5 PC0-15 Floppy bus connections: diff --git a/inc/cancellation.h b/inc/cancellation.h new file mode 100644 index 0000000..772db56 --- /dev/null +++ b/inc/cancellation.h @@ -0,0 +1,32 @@ +/* + * cancellation.h + * + * Asynchronously-cancellable function calls. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +struct cancellation { + uint32_t *sp; +}; + +#define cancellation_is_active(c) ((c)->sp != NULL) + +/* Execute fn() in a wrapped cancellable environment. */ +int call_cancellable_fn(struct cancellation *c, int (*fn)(void *), void *arg); + +/* From IRQ content: stop running fn() and immediately return -1. */ +void cancel_call(struct cancellation *c); + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/inc/decls.h b/inc/decls.h new file mode 100644 index 0000000..dec2334 --- /dev/null +++ b/inc/decls.h @@ -0,0 +1,34 @@ +/* + * decls.h + * + * Pull in all other header files in an orderly fashion. Source files include + * only this header, and only once. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +#include +#include +#include + +#include "util.h" +#include "stm32f10x_regs.h" +#include "stm32f10x.h" +#include "intrinsics.h" + +#include "time.h" +#include "cancellation.h" +#include "timer.h" + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/inc/intrinsics.h b/inc/intrinsics.h new file mode 100644 index 0000000..b20875f --- /dev/null +++ b/inc/intrinsics.h @@ -0,0 +1,164 @@ +/* + * intrinsics.h + * + * Compiler intrinsics for ARMv7-M core. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +struct exception_frame { + uint32_t r0, r1, r2, r3, r12, lr, pc, psr; +}; + +#define _STR(x) #x +#define STR(x) _STR(x) + +/* Force a compilation error if condition is true */ +#define BUILD_BUG_ON(cond) ({ _Static_assert(!(cond), "!(" #cond ")"); }) + +#define __aligned(x) __attribute__((aligned(x))) +#define __packed __attribute((packed)) +#define always_inline __inline__ __attribute__((always_inline)) +#define noinline __attribute__((noinline)) + +#define likely(x) __builtin_expect(!!(x),1) +#define unlikely(x) __builtin_expect(!!(x),0) + +#define illegal() asm volatile (".short 0xde00"); + +#define barrier() asm volatile ("" ::: "memory") +#define cpu_sync() asm volatile("dsb; isb" ::: "memory") +#define cpu_relax() asm volatile ("nop" ::: "memory") + +#define sv_call(imm) asm volatile ( "svc %0" : : "i" (imm) ) + +#define read_special(reg) ({ \ + uint32_t __x; \ + asm volatile ("mrs %0,"#reg : "=r" (__x) ::); \ + __x; \ +}) + +#define write_special(reg,val) ({ \ + uint32_t __x = (uint32_t)(val); \ + asm volatile ("msr "#reg",%0" :: "r" (__x) :); \ +}) + +/* CONTROL[1] == 0 => running on Master Stack (Exception Handler mode). */ +#define CONTROL_SPSEL 2 +#define in_exception() (!(read_special(control) & CONTROL_SPSEL)) + +#define global_disable_exceptions() \ + asm volatile ("cpsid f; cpsid i" ::: "memory") +#define global_enable_exceptions() \ + asm volatile ("cpsie f; cpsie i" ::: "memory") + +/* NB. IRQ disable via CPSID/MSR is self-synchronising. No barrier needed. */ +#define IRQ_global_disable() asm volatile ("cpsid i" ::: "memory") +#define IRQ_global_enable() asm volatile ("cpsie i" ::: "memory") + +/* Save/restore IRQ priority levels. + * NB. IRQ disable via MSR is self-synchronising. I have confirmed this on + * Cortex-M3: any pending IRQs are handled before they are disabled by + * a BASEPRI update. Hence no barrier is needed here. */ +#define IRQ_save(newpri) ({ \ + uint8_t __newpri = (newpri)<<4; \ + uint8_t __oldpri = read_special(basepri); \ + if (!__oldpri || (__oldpri > __newpri)) \ + write_special(basepri, __newpri); \ + __oldpri; }) +/* NB. Same as CPSIE, any pending IRQ enabled by this BASEPRI update may + * execute a couple of instructions after the MSR instruction. This has been + * confirmed on Cortex-M3. */ +#define IRQ_restore(oldpri) write_special(basepri, (oldpri)) + +static inline uint16_t _rev16(uint16_t x) +{ + uint16_t result; + asm volatile ("rev16 %0,%1" : "=r" (result) : "r" (x)); + return result; +} + +static inline uint32_t _rev32(uint32_t x) +{ + uint32_t result; + asm volatile ("rev %0,%1" : "=r" (result) : "r" (x)); + return result; +} + +static inline uint32_t _rbit32(uint32_t x) +{ + uint32_t result; + asm volatile ("rbit %0,%1" : "=r" (result) : "r" (x)); + return result; +} + +extern void __bad_cmpxchg(volatile void *ptr, int size); + +static always_inline unsigned long __cmpxchg( + volatile void *ptr, unsigned long old, unsigned long new, int size) +{ + unsigned long oldval, res; + + switch (size) { + case 1: + do { + asm volatile(" ldrexb %1,[%2] \n" + " movs %0,#0 \n" + " cmp %1,%3 \n" + " it eq \n" + " strexbeq %0,%4,[%2] \n" + : "=&r" (res), "=&r" (oldval) + : "r" (ptr), "Ir" (old), "r" (new) + : "memory", "cc"); + } while (res); + break; + case 2: + do { + asm volatile(" ldrexh %1,[%2] \n" + " movs %0,#0 \n" + " cmp %1,%3 \n" + " it eq \n" + " strexheq %0,%4,[%2] \n" + : "=&r" (res), "=&r" (oldval) + : "r" (ptr), "Ir" (old), "r" (new) + : "memory", "cc"); + } while (res); + break; + case 4: + do { + asm volatile(" ldrex %1,[%2] \n" + " movs %0,#0 \n" + " cmp %1,%3 \n" + " it eq \n" + " strexeq %0,%4,[%2] \n" + : "=&r" (res), "=&r" (oldval) + : "r" (ptr), "Ir" (old), "r" (new) + : "memory", "cc"); + } while (res); + break; + default: + __bad_cmpxchg(ptr, size); + oldval = 0; + } + + return oldval; +} + +#define cmpxchg(ptr,o,n) \ + ((__typeof__(*(ptr)))__cmpxchg((ptr), \ + (unsigned long)(o), \ + (unsigned long)(n), \ + sizeof(*(ptr)))) + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/inc/stm32f10x.h b/inc/stm32f10x.h new file mode 100644 index 0000000..7a0c0b6 --- /dev/null +++ b/inc/stm32f10x.h @@ -0,0 +1,149 @@ +/* + * stm32f10x.h + * + * Core and peripheral registers. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +/* C pointer types */ +#define STK volatile struct stk * const +#define SCB volatile struct scb * const +#define NVIC volatile struct nvic * const +#define FLASH volatile struct flash * const +#define PWR volatile struct pwr * const +#define BKP volatile struct bkp * const +#define RCC volatile struct rcc * const +#define IWDG volatile struct iwdg * const +#define GPIO volatile struct gpio * const +#define AFIO volatile struct afio * const +#define EXTI volatile struct exti * const +#define DMA volatile struct dma * const +#define TIM volatile struct tim * const +#define SPI volatile struct spi * const +#define I2C volatile struct i2c * const +#define USART volatile struct usart * const +#define USB volatile struct usb * const +#define USB_BUFD volatile struct usb_bufd * const +#define USB_BUF volatile uint32_t * const +#define USB_OTG volatile struct usb_otg * const + +/* C-accessible registers. */ +static STK stk = (struct stk *)STK_BASE; +static SCB scb = (struct scb *)SCB_BASE; +static NVIC nvic = (struct nvic *)NVIC_BASE; +static FLASH flash = (struct flash *)FLASH_BASE; +static PWR pwr = (struct pwr *)PWR_BASE; +static BKP bkp = (struct bkp *)BKP_BASE; +static RCC rcc = (struct rcc *)RCC_BASE; +static IWDG iwdg = (struct iwdg *)IWDG_BASE; +static GPIO gpioa = (struct gpio *)GPIOA_BASE; +static GPIO gpiob = (struct gpio *)GPIOB_BASE; +static GPIO gpioc = (struct gpio *)GPIOC_BASE; +static GPIO gpiod = (struct gpio *)GPIOD_BASE; +static GPIO gpioe = (struct gpio *)GPIOE_BASE; +static GPIO gpiof = (struct gpio *)GPIOF_BASE; +static GPIO gpiog = (struct gpio *)GPIOG_BASE; +static AFIO afio = (struct afio *)AFIO_BASE; +static EXTI exti = (struct exti *)EXTI_BASE; +static DMA dma1 = (struct dma *)DMA1_BASE; +static DMA dma2 = (struct dma *)DMA2_BASE; +static TIM tim1 = (struct tim *)TIM1_BASE; +static TIM tim2 = (struct tim *)TIM2_BASE; +static TIM tim3 = (struct tim *)TIM3_BASE; +static TIM tim4 = (struct tim *)TIM4_BASE; +static TIM tim5 = (struct tim *)TIM5_BASE; +static TIM tim6 = (struct tim *)TIM6_BASE; +static TIM tim7 = (struct tim *)TIM7_BASE; +static SPI spi1 = (struct spi *)SPI1_BASE; +static SPI spi2 = (struct spi *)SPI2_BASE; +static SPI spi3 = (struct spi *)SPI3_BASE; +static I2C i2c1 = (struct i2c *)I2C1_BASE; +static I2C i2c2 = (struct i2c *)I2C2_BASE; +static USART usart1 = (struct usart *)USART1_BASE; +static USART usart2 = (struct usart *)USART2_BASE; +static USART usart3 = (struct usart *)USART3_BASE; +static USB usb = (struct usb *)USB_BASE; +static USB_BUFD usb_bufd = (struct usb_bufd *)USB_BUF_BASE; +static USB_BUF usb_buf = (uint32_t *)USB_BUF_BASE; +static USB_OTG usb_otg = (struct usb_otg *)USB_OTG_BASE; + +/* NVIC table */ +extern uint32_t vector_table[]; + +/* System */ +void stm32_init(void); +void stm32_bootloader_enter(void); +void system_reset(void); + +/* Clocks */ +#define SYSCLK_MHZ 72 +#define SYSCLK (SYSCLK_MHZ * 1000000) +#define sysclk_ns(x) (((x) * SYSCLK_MHZ) / 1000) +#define sysclk_us(x) ((x) * SYSCLK_MHZ) +#define sysclk_ms(x) ((x) * SYSCLK_MHZ * 1000) +#define sysclk_stk(x) ((x) * (SYSCLK_MHZ / STK_MHZ)) + +/* SysTick Timer */ +#define STK_MHZ (SYSCLK_MHZ / 8) +void delay_ticks(unsigned int ticks); +void delay_ns(unsigned int ns); +void delay_us(unsigned int us); +void delay_ms(unsigned int ms); + +typedef uint32_t stk_time_t; +#define stk_now() (stk->val) +#define stk_diff(x,y) (((x)-(y)) & STK_MASK) /* d = y - x */ +#define stk_add(x,d) (((x)-(d)) & STK_MASK) /* y = x + d */ +#define stk_sub(x,d) (((x)+(d)) & STK_MASK) /* y = x - d */ +#define stk_timesince(x) stk_diff(x,stk_now()) + +#define stk_us(x) ((x) * STK_MHZ) +#define stk_ms(x) stk_us((x) * 1000) +#define stk_sysclk(x) ((x) / (SYSCLK_MHZ / STK_MHZ)) + +/* NVIC */ +#define IRQx_enable(x) do { \ + barrier(); \ + nvic->iser[(x)>>5] = 1u<<((x)&31); \ +} while (0) +#define IRQx_disable(x) do { \ + nvic->icer[(x)>>5] = 1u<<((x)&31); \ + cpu_sync(); \ +} while (0) +#define IRQx_is_enabled(x) ((nvic->iser[(x)>>5]>>((x)&31))&1) +#define IRQx_set_pending(x) (nvic->ispr[(x)>>5] = 1u<<((x)&31)) +#define IRQx_clear_pending(x) (nvic->icpr[(x)>>5] = 1u<<((x)&31)) +#define IRQx_is_pending(x) ((nvic->ispr[(x)>>5]>>((x)&31))&1) +#define IRQx_set_prio(x,y) (nvic->ipr[x] = (y) << 4) +#define IRQx_get_prio(x) (nvic->ipr[x] >> 4) + +/* GPIO */ +void gpio_configure_pin(GPIO gpio, unsigned int pin, unsigned int mode); +#define gpio_write_pin(gpio, pin, level) \ + ((gpio)->bsrr = ((level) ? 0x1u : 0x10000u) << (pin)) +#define gpio_write_pins(gpio, mask, level) \ + ((gpio)->bsrr = (uint32_t)(mask) << ((level) ? 0 : 16)) +#define gpio_read_pin(gpio, pin) (((gpio)->idr >> (pin)) & 1) +bool_t gpio_pins_connected(GPIO gpio1, unsigned int pin1, + GPIO gpio2, unsigned int pin2); + +/* FPEC */ +void fpec_init(void); +void fpec_page_erase(uint32_t flash_address); +void fpec_write(const void *data, unsigned int size, uint32_t flash_address); + +#define FLASH_PAGE_SIZE 1024 + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/inc/stm32f10x_regs.h b/inc/stm32f10x_regs.h new file mode 100644 index 0000000..80ffb40 --- /dev/null +++ b/inc/stm32f10x_regs.h @@ -0,0 +1,1007 @@ +/* + * stm32f10x_regs.h + * + * Core and peripheral register definitions. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +/* SysTick timer */ +struct stk { + uint32_t ctrl; /* 00: Control and status */ + uint32_t load; /* 04: Reload value */ + uint32_t val; /* 08: Current value */ + uint32_t calib; /* 0C: Calibration value */ +}; + +#define STK_CTRL_COUNTFLAG (1u<<16) +#define STK_CTRL_CLKSOURCE (1u<< 2) +#define STK_CTRL_TICKINT (1u<< 1) +#define STK_CTRL_ENABLE (1u<< 0) + +#define STK_MASK ((1u<<24)-1) + +#define STK_BASE 0xe000e010 + +/* System control block */ +struct scb { + uint32_t cpuid; /* 00: CPUID base */ + uint32_t icsr; /* 04: Interrupt control and state */ + uint32_t vtor; /* 08: Vector table offset */ + uint32_t aircr; /* 0C: Application interrupt and reset control */ + uint32_t scr; /* 10: System control */ + uint32_t ccr; /* 14: Configuration and control */ + uint32_t shpr1; /* 18: System handler priority reg #1 */ + uint32_t shpr2; /* 1C: system handler priority reg #2 */ + uint32_t shpr3; /* 20: System handler priority reg #3 */ + uint32_t shcsr; /* 24: System handler control and state */ + uint32_t cfsr; /* 28: Configurable fault status */ + uint32_t hfsr; /* 2C: Hard fault status */ + uint32_t _unused; /* 30: - */ + uint32_t mmar; /* 34: Memory management fault address */ + uint32_t bfar; /* 38: Bus fault address */ +}; + +#define SCB_CCR_STKALIGN (1u<<9) +#define SCB_CCR_BFHFNMIGN (1u<<8) +#define SCB_CCR_DIV_0_TRP (1u<<4) +#define SCB_CCR_UNALIGN_TRP (1u<<3) +#define SCB_CCR_USERSETMPEND (1u<<1) +#define SCB_CCR_NONBASETHRDENA (1u<<0) + +#define SCB_SHCSR_USGFAULTENA (1u<<18) +#define SCB_SHCSR_BUSFAULTENA (1u<<17) +#define SCB_SHCSR_MEMFAULTENA (1u<<16) +#define SCB_SHCSR_SVCALLPENDED (1u<<15) +#define SCB_SHCSR_BUSFAULTPENDED (1u<<14) +#define SCB_SHCSR_MEMFAULTPENDED (1u<<13) +#define SCB_SHCSR_USGFAULTPENDED (1u<<12) +#define SCB_SHCSR_SYSTICKACT (1u<<11) +#define SCB_SHCSR_PENDSVACT (1u<<10) +#define SCB_SHCSR_MONITORACT (1u<< 8) +#define SCB_SHCSR_SVCALLACT (1u<< 7) +#define SCB_SHCSR_USGFAULTACT (1u<< 3) +#define SCB_SHCSR_BUSFAULTACT (1u<< 1) +#define SCB_SHCSR_MEMFAULTACT (1u<< 0) + +#define SCB_CFSR_DIVBYZERO (1u<<25) +#define SCB_CFSR_UNALIGNED (1u<<24) +#define SCB_CFSR_NOCP (1u<<19) +#define SCB_CFSR_INVPC (1u<<18) +#define SCB_CFSR_INVSTATE (1u<<17) +#define SCB_CFSR_UNDEFINSTR (1u<<16) +#define SCB_CFSR_BFARVALID (1u<<15) +#define SCB_CFSR_STKERR (1u<<12) +#define SCB_CFSR_UNSTKERR (1u<<11) +#define SCB_CFSR_IMPRECISERR (1u<<10) +#define SCB_CFSR_PRECISERR (1u<< 9) +#define SCB_CFSR_IBUSERR (1u<< 8) +#define SCB_CFSR_MMARVALID (1u<< 7) +#define SCB_CFSR_MSTKERR (1u<< 4) +#define SCB_CFSR_MUNSTKERR (1u<< 3) +#define SCB_CFSR_DACCVIOL (1u<< 1) +#define SCB_CFSR_IACCVIOL (1u<< 0) + +#define SCB_AIRCR_VECTKEY (0x05fau<<16) +#define SCB_AIRCR_SYSRESETREQ (1u<<2) + +#define SCB_BASE 0xe000ed00 + +/* Nested vectored interrupt controller */ +struct nvic { + uint32_t iser[32]; /* 00: Interrupt set-enable */ + uint32_t icer[32]; /* 80: Interrupt clear-enable */ + uint32_t ispr[32]; /* 100: Interrupt set-pending */ + uint32_t icpr[32]; /* 180: Interrupt clear-pending */ + uint32_t iabr[64]; /* 200: Interrupt active */ + uint8_t ipr[80]; /* 300: Interrupt priority */ +}; + +#define NVIC_BASE 0xe000e100 + +/* Flash memory interface */ +struct flash { + uint32_t acr; /* 00: Flash access control */ + uint32_t keyr; /* 04: FPEC key */ + uint32_t optkeyr; /* 08: Flash OPTKEY */ + uint32_t sr; /* 0C: Flash status */ + uint32_t cr; /* 10: Flash control */ + uint32_t ar; /* 14: Flash address */ + uint32_t rsvd; /* 18: - */ + uint32_t obr; /* 1C: Option byte */ + uint32_t wrpr; /* 20: Write protection */ +}; + +#define FLASH_ACR_PRFTBS (1u<< 5) +#define FLASH_ACR_PRFTBE (1u<< 4) +#define FLASH_ACR_HLFCYA (1u<< 3) +#define FLASH_ACR_LATENCY(w) ((w)<<0) /* wait states */ + +#define FLASH_SR_EOP (1u<< 5) +#define FLASH_SR_WRPRTERR (1u<< 4) +#define FLASH_SR_PGERR (1u<< 2) +#define FLASH_SR_BSY (1u<< 0) + +#define FLASH_CR_EOPIE (1u<<12) +#define FLASH_CR_ERRIE (1u<<10) +#define FLASH_CR_OPTWRE (1u<< 9) +#define FLASH_CR_LOCK (1u<< 7) +#define FLASH_CR_STRT (1u<< 6) +#define FLASH_CR_OPTER (1u<< 5) +#define FLASH_CR_OPTPG (1u<< 4) +#define FLASH_CR_MER (1u<< 2) +#define FLASH_CR_PER (1u<< 1) +#define FLASH_CR_PG (1u<< 0) + +#define FLASH_BASE 0x40022000 + +/* Power control */ +struct pwr { + uint32_t cr; /* 00: Power control */ + uint32_t csr; /* 04: Power control/status */ +}; + +#define PWR_CR_DBP (1u<< 8) + +#define PWR_BASE 0x40007000 + +/* Backup */ +struct bkp { + uint32_t _0[1]; /* 00: - */ + uint32_t dr1[10]; /* 04-28: Data block #1 */ + uint32_t rtccr; /* 2C: RTC clock calibration */ + uint32_t cr; /* 30: Control */ + uint32_t csr; /* 34: Control/status */ + uint32_t _1[2]; /* 38-3C: - */ + uint32_t dr2[32]; /* 40-BC: Data block #2 */ +}; + +#define BKP_BASE 0x40006c00 + +/* Reset and clock control */ +struct rcc { + uint32_t cr; /* 00: Clock control */ + uint32_t cfgr; /* 04: Clock configuration */ + uint32_t cir; /* 08: Clock interrupt */ + uint32_t apb2rstr; /* 0C: APB2 peripheral reset */ + uint32_t apb1rstr; /* 10: APB1 peripheral reset */ + uint32_t ahbenr; /* 14: AHB periphernal clock enable */ + uint32_t apb2enr; /* 18: APB2 peripheral clock enable */ + uint32_t apb1enr; /* 1C: APB1 peripheral clock enable */ + uint32_t bdcr; /* 20: Backup domain control */ + uint32_t csr; /* 24: Control/status */ + uint32_t ahbrstr; /* 28: AHB peripheral clock reset */ + uint32_t cfgr2; /* 2C: Clock configuration 2 */ +}; + +#define RCC_CR_PLL3RDY (1u<<29) +#define RCC_CR_PLL3ON (1u<<28) +#define RCC_CR_PLL2RDY (1u<<27) +#define RCC_CR_PLL2ON (1u<<26) +#define RCC_CR_PLLRDY (1u<<25) +#define RCC_CR_PLLON (1u<<24) +#define RCC_CR_CSSON (1u<<19) +#define RCC_CR_HSEBYP (1u<<18) +#define RCC_CR_HSERDY (1u<<17) +#define RCC_CR_HSEON (1u<<16) +#define RCC_CR_HSIRDY (1u<<1) +#define RCC_CR_HSION (1u<<0) + +#define RCC_CFGR_PLLMUL(x) (((x)-2)<<18) +#define RCC_CFGR_PLLXTPRE (1u<<17) +#define RCC_CFGR_PLLSRC_HSI (0u<<16) +#define RCC_CFGR_PLLSRC_PREDIV1 (1u<<16) +#define RCC_CFGR_ADCPRE_DIV8 (3u<<14) +#define RCC_CFGR_PPRE1_DIV2 (4u<<8) +#define RCC_CFGR_SWS_HSI (0u<<2) +#define RCC_CFGR_SWS_HSE (1u<<2) +#define RCC_CFGR_SWS_PLL (2u<<2) +#define RCC_CFGR_SWS_MASK (3u<<2) +#define RCC_CFGR_SW_HSI (0u<<0) +#define RCC_CFGR_SW_HSE (1u<<0) +#define RCC_CFGR_SW_PLL (2u<<0) +#define RCC_CFGR_SW_MASK (3u<<0) + +#define RCC_AHBENR_ETHMACRXEN (1u<<16) +#define RCC_AHBENR_ETHMACTXEN (1u<<15) +#define RCC_AHBENR_ETHMACEN (1u<<14) +#define RCC_AHBENR_OTGFSEN (1u<<12) +#define RCC_AHBENR_CRCEN (1u<< 6) +#define RCC_AHBENR_FLITFEN (1u<< 4) +#define RCC_AHBENR_SRAMEN (1u<< 2) +#define RCC_AHBENR_DMA2EN (1u<< 1) +#define RCC_AHBENR_DMA1EN (1u<< 0) + +#define RCC_APB1ENR_DACEN (1u<<29) +#define RCC_APB1ENR_PWREN (1u<<28) +#define RCC_APB1ENR_BKPEN (1u<<27) +#define RCC_APB1ENR_CAN2EN (1u<<26) +#define RCC_APB1ENR_CAN1EN (1u<<25) +#define RCC_APB1ENR_USBEN (1u<<23) +#define RCC_APB1ENR_I2C2EN (1u<<22) +#define RCC_APB1ENR_I2C1EN (1u<<21) +#define RCC_APB1ENR_USART5EN (1u<<20) +#define RCC_APB1ENR_USART4EN (1u<<19) +#define RCC_APB1ENR_USART3EN (1u<<18) +#define RCC_APB1ENR_USART2EN (1u<<17) +#define RCC_APB1ENR_SPI3EN (1u<<15) +#define RCC_APB1ENR_SPI2EN (1u<<14) +#define RCC_APB1ENR_WWDGEN (1u<<11) +#define RCC_APB1ENR_TIM7EN (1u<< 5) +#define RCC_APB1ENR_TIM6EN (1u<< 4) +#define RCC_APB1ENR_TIM5EN (1u<< 3) +#define RCC_APB1ENR_TIM4EN (1u<< 2) +#define RCC_APB1ENR_TIM3EN (1u<< 1) +#define RCC_APB1ENR_TIM2EN (1u<< 0) + +#define RCC_APB2ENR_USART1EN (1u<<14) +#define RCC_APB2ENR_SPI1EN (1u<<12) +#define RCC_APB2ENR_TIM1EN (1u<<11) +#define RCC_APB2ENR_ADC2EN (1u<<10) +#define RCC_APB2ENR_ADC1EN (1u<< 9) +#define RCC_APB2ENR_IOPEEN (1u<< 6) +#define RCC_APB2ENR_IOPDEN (1u<< 5) +#define RCC_APB2ENR_IOPCEN (1u<< 4) +#define RCC_APB2ENR_IOPBEN (1u<< 3) +#define RCC_APB2ENR_IOPAEN (1u<< 2) +#define RCC_APB2ENR_AFIOEN (1u<< 0) + +#define RCC_CSR_LPWRRSTF (1u<<31) +#define RCC_CSR_WWDGRSTF (1u<<30) +#define RCC_CSR_IWDGRSTF (1u<<29) +#define RCC_CSR_SFTRSTF (1u<<28) +#define RCC_CSR_PORRSTF (1u<<27) +#define RCC_CSR_PINRSTF (1u<<26) +#define RCC_CSR_RMVF (1u<<24) +#define RCC_CSR_LSIRDY (1u<< 1) +#define RCC_CSR_LSION (1u<< 0) + +#define RCC_AHBRSTR_ETHMACRST (1u<<14) +#define RCC_AHBRSTR_OTGFSRST (1u<<12) + +#define RCC_BASE 0x40021000 + +/* Independent Watchdog */ +struct iwdg { + uint32_t kr; /* 00: Key */ + uint32_t pr; /* 04: Prescaler */ + uint32_t rlr; /* 08: Reload */ + uint32_t sr; /* 0C: Status */ +}; + +#define IWDG_BASE 0x40003000 + +/* General-purpose I/O */ +struct gpio { + uint32_t crl; /* 00: Port configuration low */ + uint32_t crh; /* 04: Port configuration high */ + uint32_t idr; /* 08: Port input data */ + uint32_t odr; /* 0C: Port output data */ + uint32_t bsrr; /* 10: Port bit set/reset */ + uint32_t brr; /* 14: Port bit reset */ + uint32_t lckr; /* 18: Port configuration lock */ +}; + +#define _GPI_pulled(level) (0x8u|((level)<<4)) +#define GPI_analog 0x0u +#define GPI_floating 0x4u +#define GPI_pull_down _GPI_pulled(LOW) +#define GPI_pull_up _GPI_pulled(HIGH) + +#define GPO_pushpull(speed,level) (0x0u|(speed)|((level)<<4)) +#define GPO_opendrain(speed,level) (0x4u|(speed)|((level)<<4)) +#define AFO_pushpull(speed) (0x8u|(speed)) +#define AFO_opendrain(speed) (0xcu|(speed)) +#define _2MHz 2 +#define _10MHz 1 +#define _50MHz 3 +#define LOW 0 +#define HIGH 1 + +#define GPIOA_BASE 0x40010800 +#define GPIOB_BASE 0x40010c00 +#define GPIOC_BASE 0x40011000 +#define GPIOD_BASE 0x40011400 +#define GPIOE_BASE 0x40011800 +#define GPIOF_BASE 0x40011c00 +#define GPIOG_BASE 0x40012000 + +/* Alternative-function I/O */ +struct afio { + uint32_t evcr; /* 00: Event control */ + uint32_t mapr; /* 04: AF remap and debug I/O configuration */ + uint32_t exticr1; /* 08: External interrupt configuration #1 */ + uint32_t exticr2; /* 0C: External interrupt configuration #2 */ + uint32_t exticr3; /* 10: External interrupt configuration #3 */ + uint32_t exticr4; /* 14: External interrupt configuration #4 */ + uint32_t rsvd; /* 18: - */ + uint32_t mapr2; /* 1C: AF remap and debug I/O configuration #2 */ +}; + +#define AFIO_MAPR_SWJ_CFG_DISABLED (4u<<24) +#define AFIO_MAPR_TIM4_REMAP_FULL (1u<<12) +#define AFIO_MAPR_TIM3_REMAP_FULL (3u<<10) +#define AFIO_MAPR_TIM3_REMAP_PARTIAL (2u<<10) +#define AFIO_MAPR_TIM2_REMAP_FULL (3u<< 8) +#define AFIO_MAPR_TIM2_REMAP_PARTIAL_1 (1u<< 8) +#define AFIO_MAPR_TIM2_REMAP_PARTIAL_2 (2u<< 8) +#define AFIO_MAPR_TIM1_REMAP_FULL (3u<< 6) +#define AFIO_MAPR_TIM1_REMAP_PARTIAL (1u<< 6) +#define AFIO_MAPR_USART3_REMAP_FULL (3u<< 4) +#define AFIO_MAPR_USART3_REMAP_PARTIAL (1u<< 4) + +#define AFIO_BASE 0x40010000 + +struct exti { + uint32_t imr; /* 00: Interrupt mask */ + uint32_t emr; /* 04: Event mask */ + uint32_t rtsr; /* 08: Rising trigger selection */ + uint32_t ftsr; /* 0C: Falling trigger selection */ + uint32_t swier; /* 10: Software interrupt event */ + uint32_t pr; /* 14: Pending */ +}; + +#define EXTI_BASE 0x40010400 + +/* DMA */ +struct dma_chn { + uint32_t ccr; /* +00: Configuration */ + uint32_t cndtr; /* +04: Number of data */ + uint32_t cpar; /* +08: Peripheral address */ + uint32_t cmar; /* +0C: Memory address */ + uint32_t rsvd; /* +10: - */ +}; +struct dma { + uint32_t isr; /* 00: Interrupt status */ + uint32_t ifcr; /* 04: Interrupt flag clear */ + struct dma_chn ch1; /* 08: Channel 1 */ + struct dma_chn ch2; /* 1C: Channel 2 */ + struct dma_chn ch3; /* 30: Channel 3 */ + struct dma_chn ch4; /* 44: Channel 4 */ + struct dma_chn ch5; /* 58: Channel 5 */ + struct dma_chn ch6; /* 6C: Channel 6 */ + struct dma_chn ch7; /* 80: Channel 7 */ +}; + +/* n=1..7 */ +#define DMA_ISR_TEIF(n) (8u<<(((n)-1)*4)) +#define DMA_ISR_HTIF(n) (4u<<(((n)-1)*4)) +#define DMA_ISR_TCIF(n) (2u<<(((n)-1)*4)) +#define DMA_ISR_GIF(n) (1u<<(((n)-1)*4)) + +/* n=1..7 */ +#define DMA_IFCR_CTEIF(n) (8u<<(((n)-1)*4)) +#define DMA_IFCR_CHTIF(n) (4u<<(((n)-1)*4)) +#define DMA_IFCR_CTCIF(n) (2u<<(((n)-1)*4)) +#define DMA_IFCR_CGIF(n) (1u<<(((n)-1)*4)) + +#define DMA_CCR_MEM2MEM (1u<<14) +#define DMA_CCR_PL_LOW (0u<<12) +#define DMA_CCR_PL_MEDIUM (1u<<12) +#define DMA_CCR_PL_HIGH (2u<<12) +#define DMA_CCR_PL_V_HIGH (3u<<12) +#define DMA_CCR_MSIZE_8BIT (0u<<10) +#define DMA_CCR_MSIZE_16BIT (1u<<10) +#define DMA_CCR_MSIZE_32BIT (2u<<10) +#define DMA_CCR_PSIZE_8BIT (0u<< 8) +#define DMA_CCR_PSIZE_16BIT (1u<< 8) +#define DMA_CCR_PSIZE_32BIT (2u<< 8) +#define DMA_CCR_MINC (1u<< 7) +#define DMA_CCR_PINC (1u<< 6) +#define DMA_CCR_CIRC (1u<< 5) +#define DMA_CCR_DIR_P2M (0u<< 4) +#define DMA_CCR_DIR_M2P (1u<< 4) +#define DMA_CCR_TEIE (1u<< 3) +#define DMA_CCR_HTIE (1u<< 2) +#define DMA_CCR_TCIE (1u<< 1) +#define DMA_CCR_EN (1u<< 0) + +#define DMA1_BASE 0x40020000 +#define DMA2_BASE 0x40020400 + +/* Timer */ +struct tim { + uint32_t cr1; /* 00: Control 1 */ + uint32_t cr2; /* 04: Control 2 */ + uint32_t smcr; /* 08: Slave mode control */ + uint32_t dier; /* 0C: DMA/interrupt enable */ + uint32_t sr; /* 10: Status */ + uint32_t egr; /* 14: Event generation */ + uint32_t ccmr1; /* 18: Capture/compare mode 1 */ + uint32_t ccmr2; /* 1C: Capture/compare mode 2 */ + uint32_t ccer; /* 20: Capture/compare enable */ + uint32_t cnt; /* 24: Counter */ + uint32_t psc; /* 28: Prescaler */ + uint32_t arr; /* 2C: Auto-reload */ + uint32_t rcr; /* 30: Repetition counter */ + uint32_t ccr1; /* 34: Capture/compare 1 */ + uint32_t ccr2; /* 38: Capture/compare 2 */ + uint32_t ccr3; /* 3C: Capture/compare 3 */ + uint32_t ccr4; /* 40: Capture/compare 4 */ + uint32_t bdtr; /* 44: Break and dead-time */ + uint32_t dcr; /* 48: DMA control */ + uint32_t dmar; /* 4C: DMA address for full transfer */ +}; + +#define TIM_CR1_ARPE (1u<<7) +#define TIM_CR1_DIR (1u<<4) +#define TIM_CR1_OPM (1u<<3) +#define TIM_CR1_URS (1u<<2) +#define TIM_CR1_UDIS (1u<<1) +#define TIM_CR1_CEN (1u<<0) + +#define TIM_CR2_TI1S (1u<<7) +#define TIM_CR2_MMS(x) ((x)<<4) +#define TIM_CR2_CCDS (1u<<3) + +#define TIM_SMCR_ETP (1u<<15) +#define TIM_SMCR_ETC (1u<<14) +#define TIM_SMCR_ETPS(x) ((x)<<12) +#define TIM_SMCR_ETF(x) ((x)<<8) +#define TIM_SMCR_MSM (1u<<7) +#define TIM_SMCR_TS(x) ((x)<<4) +#define TIM_SMCR_SMS(x) ((x)<<0) + +#define TIM_DIER_TDE (1u<<14) +#define TIM_DIER_CC4DE (1u<<12) +#define TIM_DIER_CC3DE (1u<<11) +#define TIM_DIER_CC2DE (1u<<10) +#define TIM_DIER_CC1DE (1u<<9) +#define TIM_DIER_UDE (1u<<8) +#define TIM_DIER_TIE (1u<<6) +#define TIM_DIER_CC4IE (1u<<4) +#define TIM_DIER_CC3IE (1u<<3) +#define TIM_DIER_CC2IE (1u<<2) +#define TIM_DIER_CC1IE (1u<<1) +#define TIM_DIER_UIE (1u<<0) + +#define TIM_SR_CC4OF (1u<<12) +#define TIM_SR_CC3OF (1u<<11) +#define TIM_SR_CC2OF (1u<<10) +#define TIM_SR_CC1OF (1u<<9) +#define TIM_SR_TIF (1u<<6) +#define TIM_SR_CC4IF (1u<<4) +#define TIM_SR_CC3IF (1u<<3) +#define TIM_SR_CC2IF (1u<<2) +#define TIM_SR_CC1IF (1u<<1) +#define TIM_SR_UIF (1u<<0) + +#define TIM_EGR_TG (1u<<6) +#define TIM_EGR_CC4G (1u<<4) +#define TIM_EGR_CC3G (1u<<3) +#define TIM_EGR_CC2G (1u<<2) +#define TIM_EGR_CC1G (1u<<1) +#define TIM_EGR_UG (1u<<0) + +#define TIM_CCMR1_OC2CE (1u <<15) +#define TIM_CCMR1_OC2M(x) ((x)<<12) +#define TIM_CCMR1_OC2PE (1u <<11) +#define TIM_CCMR1_OC2FE (1u <<10) +#define TIM_CCMR1_CC2S(x) ((x)<< 8) +#define TIM_CCMR1_OC1CE (1u << 7) +#define TIM_CCMR1_OC1M(x) ((x)<< 4) +#define TIM_CCMR1_OC1PE (1u << 3) +#define TIM_CCMR1_OC1FE (1u << 2) +#define TIM_CCMR1_CC1S(x) ((x)<< 0) + +#define TIM_CCMR1_IC2F(x) ((x)<<12) +#define TIM_CCMR1_IC2PSC(x) ((x)<<10) +#define TIM_CCMR1_IC1F(x) ((x)<< 4) +#define TIM_CCMR1_IC1PSC(x) ((x)<< 2) + +#define TIM_CCMR2_OC4CE (1u <<15) +#define TIM_CCMR2_OC4M(x) ((x)<<12) +#define TIM_CCMR2_OC4PE (1u <<11) +#define TIM_CCMR2_OC4FE (1u <<10) +#define TIM_CCMR2_CC4S(x) ((x)<< 8) +#define TIM_CCMR2_OC3CE (1u << 7) +#define TIM_CCMR2_OC3M(x) ((x)<< 4) +#define TIM_CCMR2_OC3PE (1u << 3) +#define TIM_CCMR2_OC3FE (1u << 2) +#define TIM_CCMR2_CC3S(x) ((x)<< 0) + +#define TIM_CCMR2_IC4F(x) ((x)<<12) +#define TIM_CCMR2_IC4PSC(x) ((x)<<10) +#define TIM_CCMR2_IC3F(x) ((x)<< 4) +#define TIM_CCMR2_IC3PSC(x) ((x)<< 2) + +#define TIM_OCM_FROZEN (0u) +#define TIM_OCM_SET_HIGH (1u) +#define TIM_OCM_SET_LOW (2u) +#define TIM_OCM_TOGGLE (3u) +#define TIM_OCM_FORCE_LOW (4u) +#define TIM_OCM_FORCE_HIGH (5u) +#define TIM_OCM_PWM1 (6u) +#define TIM_OCM_PWM2 (7u) +#define TIM_OCM_MASK (7u) + +#define TIM_CCS_OUTPUT (0u) +#define TIM_CCS_INPUT_TI1 (1u) +#define TIM_CCS_INPUT_TI2 (2u) +#define TIM_CCS_INPUT_TRC (3u) +#define TIM_CCS_MASK (3u) + +#define TIM_CCER_CC4P (1u<<13) +#define TIM_CCER_CC4E (1u<<12) +#define TIM_CCER_CC3P (1u<< 9) +#define TIM_CCER_CC3E (1u<< 8) +#define TIM_CCER_CC2P (1u<< 5) +#define TIM_CCER_CC2E (1u<< 4) +#define TIM_CCER_CC1P (1u<< 1) +#define TIM_CCER_CC1E (1u<< 0) + +#define TIM_BDTR_MOE (1u<<15) +#define TIM_BDTR_AOE (1u<<14) +#define TIM_BDTR_BKP (1u<<13) +#define TIM_BDTR_BKE (1u<<12) +#define TIM_BDTR_OSSR (1u<<11) +#define TIM_BDTR_OSSI (1u<<10) +#define TIM_BDTR_LOCK(x) ((x)<<8) +#define TIM_BDTR_DTG(x) ((x)<<0) + +#define TIM1_BASE 0x40012c00 +#define TIM2_BASE 0x40000000 +#define TIM3_BASE 0x40000400 +#define TIM4_BASE 0x40000800 +#define TIM5_BASE 0x40000c00 +#define TIM6_BASE 0x40001000 +#define TIM7_BASE 0x40001400 + +/* SPI/I2S */ +struct spi { + uint32_t cr1; /* 00: Control 1 */ + uint32_t cr2; /* 04: Control 2 */ + uint32_t sr; /* 08: Status */ + uint32_t dr; /* 0C: Data */ + uint32_t crcpr; /* 10: CRC polynomial */ + uint32_t rxcrcr; /* 14: RX CRC */ + uint32_t txcrcr; /* 18: TX CRC */ + uint32_t i2scfgr; /* 1C: I2S configuration */ + uint32_t i2spr; /* 20: I2S prescaler */ +}; + +#define SPI_CR1_BIDIMODE (1u<<15) +#define SPI_CR1_BIDIOE (1u<<14) +#define SPI_CR1_CRCEN (1u<<13) +#define SPI_CR1_CRCNEXT (1u<<12) +#define SPI_CR1_DFF (1u<<11) +#define SPI_CR1_RXONLY (1u<<10) +#define SPI_CR1_SSM (1u<< 9) +#define SPI_CR1_SSI (1u<< 8) +#define SPI_CR1_LSBFIRST (1u<< 7) +#define SPI_CR1_SPE (1u<< 6) +#define SPI_CR1_BR_DIV2 (0u<< 3) +#define SPI_CR1_BR_DIV4 (1u<< 3) +#define SPI_CR1_BR_DIV8 (2u<< 3) +#define SPI_CR1_BR_DIV16 (3u<< 3) +#define SPI_CR1_BR_DIV32 (4u<< 3) +#define SPI_CR1_BR_DIV64 (5u<< 3) +#define SPI_CR1_BR_DIV128 (6u<< 3) +#define SPI_CR1_BR_DIV256 (7u<< 3) +#define SPI_CR1_BR_MASK (7u<< 3) +#define SPI_CR1_MSTR (1u<< 2) +#define SPI_CR1_CPOL (1u<< 1) +#define SPI_CR1_CPHA (1u<< 0) + +#define SPI_CR2_TXEIE (1u<< 7) +#define SPI_CR2_RXNEIE (1u<< 6) +#define SPI_CR2_ERRIE (1u<< 5) +#define SPI_CR2_SSOE (1u<< 2) +#define SPI_CR2_TXDMAEN (1u<< 1) +#define SPI_CR2_RXDMAEN (1u<< 0) + +#define SPI_SR_BSY (1u<< 7) +#define SPI_SR_OVR (1u<< 6) +#define SPI_SR_MODF (1u<< 5) +#define SPI_SR_CRCERR (1u<< 4) +#define SPI_SR_USR (1u<< 3) +#define SPI_SR_CHSIDE (1u<< 2) +#define SPI_SR_TXE (1u<< 1) +#define SPI_SR_RXNE (1u<< 0) + +#define SPI1_BASE 0x40013000 +#define SPI2_BASE 0x40003800 +#define SPI3_BASE 0x40003C00 + +/* I2C */ +struct i2c { + uint32_t cr1; /* 00: Control 1 */ + uint32_t cr2; /* 04: Control 2 */ + uint32_t oar1; /* 08: Own address 1 */ + uint32_t oar2; /* 0C: Own address 2 */ + uint32_t dr; /* 10: Data */ + uint32_t sr1; /* 14: Status 1 */ + uint32_t sr2; /* 18: Status 2 */ + uint32_t ccr; /* 1C: Clock control */ + uint32_t trise; /* 20: Rise time */ +}; + +#define I2C_CR1_SWRST (1u<<15) +#define I2C_CR1_ALERT (1u<<13) +#define I2C_CR1_PEC (1u<<12) +#define I2C_CR1_POS (1u<<11) +#define I2C_CR1_ACK (1u<<10) +#define I2C_CR1_STOP (1u<< 9) +#define I2C_CR1_START (1u<< 8) +#define I2C_CR1_NOSTRETCH (1u<< 7) +#define I2C_CR1_ENGC (1u<< 6) +#define I2C_CR1_ENPEC (1u<< 5) +#define I2C_CR1_ENARP (1u<< 4) +#define I2C_CR1_SMBTYPE (1u<< 3) +#define I2C_CR1_SMBUS (1u<< 1) +#define I2C_CR1_PE (1u<< 0) + +#define I2C_CR2_LAST (1u<<12) +#define I2C_CR2_DMAEN (1u<<11) +#define I2C_CR2_ITBUFEN (1u<<10) +#define I2C_CR2_ITEVTEN (1u<< 9) +#define I2C_CR2_ITERREN (1u<< 8) +#define I2C_CR2_FREQ(x) (x) + +#define I2C_SR1_SMBALERT (1u<<15) +#define I2C_SR1_TIMEOUT (1u<<14) +#define I2C_SR1_PECERR (1u<<12) +#define I2C_SR1_OVR (1u<<11) +#define I2C_SR1_AF (1u<<10) +#define I2C_SR1_ARLO (1u<< 9) +#define I2C_SR1_BERR (1u<< 8) +#define I2C_SR1_ERRORS 0xdf00 +#define I2C_SR1_TXE (1u<< 7) +#define I2C_SR1_RXNE (1u<< 6) +#define I2C_SR1_STOPF (1u<< 4) +#define I2C_SR1_ADD10 (1u<< 3) +#define I2C_SR1_BTF (1u<< 2) +#define I2C_SR1_ADDR (1u<< 1) +#define I2C_SR1_SB (1u<< 0) +#define I2C_SR1_EVENTS 0x001f + +#define I2C_SR2_PEC(x) ((x)<<15) +#define I2C_SR2_DUALF (1u<< 7) +#define I2C_SR2_SMBHOST (1u<< 6) +#define I2C_SR2_SMBDEFAULT (1u<< 5) +#define I2C_SR2_GENCALL (1u<< 4) +#define I2C_SR2_TRA (1u<< 2) +#define I2C_SR2_BUSY (1u<< 1) +#define I2C_SR2_MSL (1u<< 0) + +#define I2C_CCR_FS (1u<<15) +#define I2C_CCR_DUTY (1u<<14) +#define I2C_CCR_CCR(x) (x) + +#define I2C1_BASE 0x40005400 +#define I2C2_BASE 0x40005800 + +/* USART */ +struct usart { + uint32_t sr; /* 00: Status */ + uint32_t dr; /* 04: Data */ + uint32_t brr; /* 08: Baud rate */ + uint32_t cr1; /* 0C: Control 1 */ + uint32_t cr2; /* 10: Control 2 */ + uint32_t cr3; /* 14: Control 3 */ + uint32_t gtpr; /* 18: Guard time and prescaler */ +}; + +#define USART_SR_CTS (1u<<9) +#define USART_SR_LBD (1u<<8) +#define USART_SR_TXE (1u<<7) +#define USART_SR_TC (1u<<6) +#define USART_SR_RXNE (1u<<5) +#define USART_SR_IDLE (1u<<4) +#define USART_SR_ORE (1u<<3) +#define USART_SR_NE (1u<<2) +#define USART_SR_FE (1u<<1) +#define USART_SR_PE (1u<<0) + +#define USART_CR1_UE (1u<<13) +#define USART_CR1_M (1u<<12) +#define USART_CR1_WAKE (1u<<11) +#define USART_CR1_PCE (1u<<10) +#define USART_CR1_PS (1u<< 9) +#define USART_CR1_PEIE (1u<< 8) +#define USART_CR1_TXEIE (1u<< 7) +#define USART_CR1_TCIE (1u<< 6) +#define USART_CR1_RXNEIE (1u<< 5) +#define USART_CR1_IDLEIE (1u<< 4) +#define USART_CR1_TE (1u<< 3) +#define USART_CR1_RE (1u<< 2) +#define USART_CR1_RWU (1u<< 1) +#define USART_CR1_SBK (1u<< 0) + +#define USART_CR3_CTSIE (1u<<10) +#define USART_CR3_CTSE (1u<< 9) +#define USART_CR3_RTSE (1u<< 8) +#define USART_CR3_DMAT (1u<< 7) +#define USART_CR3_DMAR (1u<< 6) +#define USART_CR3_SCEN (1u<< 5) +#define USART_CR3_NACK (1u<< 4) +#define USART_CR3_HDSEL (1u<< 3) +#define USART_CR3_IRLP (1u<< 2) +#define USART_CR3_IREN (1u<< 1) +#define USART_CR3_EIE (1u<< 0) + +#define USART1_BASE 0x40013800 +#define USART2_BASE 0x40004400 +#define USART3_BASE 0x40004800 + +/* USB Full Speed */ +struct usb { + uint32_t epr[8]; /* 4*n: Endpoint n */ + uint32_t rsvd[8]; + uint32_t cntr; /* 40: Control */ + uint32_t istr; /* 44: Interrupt status */ + uint32_t fnr; /* 48: Frame number */ + uint32_t daddr; /* 4C: Device address */ + uint32_t btable; /* 50: Buffer table address */ +}; + +struct usb_bufd { + uint32_t addr_tx; /* 00: Transmission buffer address */ + uint32_t count_tx; /* 04: Transmission byte count */ + uint32_t addr_rx; /* 08: Reception buffer address */ + uint32_t count_rx; /* 0C: Reception byte count */ +}; + +#define USB_EPR_CTR_RX (1u<<15) +#define USB_EPR_DTOG_RX (1u<<14) +#define USB_EPR_STAT_RX(x) ((x)<<12) +#define USB_EPR_SETUP (1u<<11) +#define USB_EPR_EP_TYPE(x) ((x)<<9) +#define USB_EPR_EP_KIND_DBL_BUF (1<<8) /* USB_EP_TYPE_BULK */ +#define USB_EPR_EP_KIND_STATUS_OUT (1<<8) /* USB_EP_TYPE_CONTROL */ +#define USB_EPR_CTR_TX (1u<< 7) +#define USB_EPR_DTOG_TX (1u<< 6) +#define USB_EPR_STAT_TX(x) ((x)<<4) +#define USB_EPR_EA(x) ((x)<<0) + +#define USB_STAT_DISABLED (0u) +#define USB_STAT_STALL (1u) +#define USB_STAT_NAK (2u) +#define USB_STAT_VALID (3u) +#define USB_STAT_MASK (3u) + +#define USB_EP_TYPE_BULK (0u) +#define USB_EP_TYPE_CONTROL (1u) +#define USB_EP_TYPE_ISO (2u) +#define USB_EP_TYPE_INTERRUPT (3u) +#define USB_EP_TYPE_MASK (3u) + +#define USB_CNTR_CTRM (1u<<15) +#define USB_CNTR_PMAOVRM (1u<<14) +#define USB_CNTR_ERRM (1u<<13) +#define USB_CNTR_WKUPM (1u<<12) +#define USB_CNTR_SUSPM (1u<<11) +#define USB_CNTR_RESETM (1u<<10) +#define USB_CNTR_SOFM (1u<< 9) +#define USB_CNTR_ESOFM (1u<< 8) +#define USB_CNTR_RESUME (1u<< 4) +#define USB_CNTR_FSUSP (1u<< 3) +#define USB_CNTR_LP_MODE (1u<< 2) +#define USB_CNTR_PDWN (1u<< 1) +#define USB_CNTR_FRES (1u<< 0) + +#define USB_ISTR_CTR (1u<<15) +#define USB_ISTR_PMAOVR (1u<<14) +#define USB_ISTR_ERR (1u<<13) +#define USB_ISTR_WKUP (1u<<12) +#define USB_ISTR_SUSP (1u<<11) +#define USB_ISTR_RESET (1u<<10) +#define USB_ISTR_SOF (1u<< 9) +#define USB_ISTR_ESOF (1u<< 8) +#define USB_ISTR_DIR (1u<< 4) +#define USB_ISTR_GET_EP_ID(x) ((x)&0xf) + +#define USB_FNR_RXDP (1u<<15) +#define USB_FNR_RXDM (1u<<14) +#define USB_FNR_LCK (1u<<13) +#define USB_FNR_GET_LSOF(x) (((x)>>11)&3) +#define USB_FNR_GET_FN(x) ((x)&0x7ff) + +#define USB_DADDR_EF (1u<< 7) +#define USB_DADDR_ADD(x) ((x)<<0) + +#define USB_BASE 0x40005c00 +#define USB_BUF_BASE 0x40006000 + +/* USB On-The-Go Full Speed interface */ +struct usb_otg { + uint32_t gotctl; /* 00: Control and status */ + uint32_t gotgint; /* 04: Interrupt */ + uint32_t gahbcfg; /* 08: AHB configuration */ + uint32_t gusbcfg; /* 0C: USB configuration */ + uint32_t grstctl; /* 10: Reset */ + uint32_t gintsts; /* 14: Core interrupt */ + uint32_t gintmsk; /* 18: Interrupt mask */ + uint32_t grxstsr; /* 1C: Receive status debug read */ + uint32_t grxstsp; /* 20: Receive status debug pop */ + uint32_t grxfsiz; /* 24: Receive FIFO size */ + union { + uint32_t hnptxfsiz; /* 28: Host non-periodic transmit FIFO size */ + uint32_t dieptxf0; /* 28: Endpoint 0 transmit FIFO size */ + }; + uint32_t hnptxsts; /* 2C: Non-periodic transmit FIFO/queue status */ + uint32_t _0[2]; + uint32_t gccfg; /* 38: General core configuration */ + uint32_t cid; /* 3C: Core ID */ + uint32_t _1[48]; + uint32_t hptxfsiz; /* 100: Host periodic transmit FIFO size */ + uint32_t dieptxf1; /* 104: Device IN endpoint transmit FIFO #1 size */ + uint32_t dieptxf2; /* 108: Device IN endpoint transmit FIFO #2 size */ + uint32_t dieptxf3; /* 10C: Device IN endpoint transmit FIFO #3 size */ + uint32_t _2[188]; + uint32_t hcfg; /* 400: Host configuration */ + uint32_t hfir; /* 404: Host frame interval */ + uint32_t hfnum; /* 408: Host frame number / frame time remaining */ + uint32_t _3[1]; /* 40C: */ + uint32_t hptxsts; /* 410: Host periodic transmit FIFO / queue status */ + uint32_t haint; /* 414: Host all channels interrupt status */ + uint32_t haintmsk; /* 418: Host all channels interrupt mask */ + uint32_t _4[9]; + uint32_t hprt; /* 440: Host port control and status */ + uint32_t _5[47]; + struct { + uint32_t charac; /* +00: Host channel-x characteristics */ + uint32_t _0[1]; + uint32_t intsts; /* +08: Host channel-x interrupt status */ + uint32_t intmsk; /* +0C: Host channel-x interrupt mask */ + uint32_t tsiz; /* +10: Host channel x transfer size */ + uint32_t _1[3]; + } hc[8]; /* 500..5E0: */ + uint32_t _6[128]; + + uint32_t dcfg; /* 800: Device configuration */ + uint32_t dctl; /* 804: Device control */ + uint32_t dsts; /* 808: Device status */ + uint32_t _7[1]; + uint32_t diepmsk; /* 810: Device IN endpoint common interrupt mask */ + uint32_t doepmsk; /* 814: Device OUT endpoint common interrupt mask */ + uint32_t daint; /* 818: Device all endpoints interrupt status */ + uint32_t daintmsk; /* 81C: Device all endpoints interrupt mask */ + uint32_t _8[2]; + uint32_t dvbusdis; /* 828: Device VBUS discharge time */ + uint32_t dvbuspulse; /* 82C: Device VBUS pulsing time */ + uint32_t _9[1]; + uint32_t diepempmsk; /* 834: Device IN endpoint FIFO empty int. mask */ + uint32_t _10[50]; + struct { + uint32_t ctl; /* +00: Device IN endpoint-x control */ + uint32_t _0[1]; + uint32_t intsts; /* +08: Device IN endpoint-x interrupt status */ + uint32_t _1[3]; + uint32_t txfsts; /* +18: Device IN endpoint-x transmit FIFO status */ + uint32_t _2[1]; + } diep[4]; /* 900..960: */ + uint32_t _11[96]; + struct { + uint32_t ctl; /* +00: Device OUT endpoint-x control */ + uint32_t _0[1]; + uint32_t intsts; /* +08: Device OUT endpoint-x interrupt status */ + uint32_t _1[1]; + uint32_t tsiz; /* +10: Device OUT endpoint-x transmit FIFO status */ + uint32_t _2[3]; + } doep[4]; /* B00..B60: */ + uint32_t _12[160]; + + uint32_t pcgcctl; /* E00: Power and clock gating control */ +}; + +#define OTG_GAHBCFG_PTXFELVL (1u<< 8) +#define OTG_GAHBCFG_TXFELVL (1u<< 7) +#define OTG_GAHBCFG_GINTMSK (1u<< 0) + +#define OTG_GUSBCFG_CTXPKT (1u<<31) +#define OTG_GUSBCFG_FDMOD (1u<<30) +#define OTG_GUSBCFG_FHMOD (1u<<29) +#define OTG_GUSBCFG_TRDT(x) ((x)<<10) +#define OTG_GUSBCFG_HNPCAP (1u<< 9) +#define OTG_GUSBCFG_SRPCAP (1u<< 8) +#define OTG_GUSBCFG_PHYSEL (1u<< 6) +#define OTG_GUSBCFG_TOCAL(x) ((x)<< 0) + +/* GINTSTS and GINTMSK */ +#define OTG_GINT_WKUPINT (1u<<31) /* Host + Device */ +#define OTG_GINT_SRQINT (1u<<30) /* H + D */ +#define OTG_GINT_DISCINT (1u<<29) /* H */ +#define OTG_GINT_CIDSCHG (1u<<28) /* H + D */ +#define OTG_GINT_PTXFE (1u<<26) /* H */ +#define OTG_GINT_HCINT (1u<<25) /* H */ +#define OTG_GINT_HPRTINT (1u<<24) /* H */ +#define OTG_GINT_IPXFR (1u<<21) /* H */ +#define OTG_GINT_IISOIXFR (1u<<20) /* D */ +#define OTG_GINT_OEPINT (1u<<19) /* D */ +#define OTG_GINT_IEPINT (1u<<18) /* D */ +#define OTG_GINT_EOPF (1u<<15) /* D */ +#define OTG_GINT_ISOODRP (1u<<14) /* D */ +#define OTG_GINT_ENUMDNE (1u<<13) /* D */ +#define OTG_GINT_USBRST (1u<<12) /* D */ +#define OTG_GINT_USBSUSP (1u<<11) /* D */ +#define OTG_GINT_ESUSP (1u<<10) /* D */ +#define OTG_GINT_GONAKEFF (1u<< 7) /* D */ +#define OTG_GINT_GINAKEFF (1u<< 6) /* D */ +#define OTG_GINT_NPTXFE (1u<< 5) /* H */ +#define OTG_GINT_RXFLVL (1u<< 4) /* H + D */ +#define OTG_GINT_SOF (1u<< 3) /* H + D */ +#define OTG_GINT_OTGINT (1u<< 2) /* H + D */ +#define OTG_GINT_MMIS (1u<< 1) /* H + D */ +#define OTG_GINT_CMOD (1u<< 0) /* H + D */ + +#define OTG_RXSTS_PKTSTS_IN (2u) +#define OTG_RXSTS_PKTSTS(r) (((r)>>17)&0xf) +#define OTG_RXSTS_BCNT(r) (((r)>>4)&0x7ff) +#define OTG_RXSTS_CHNUM(r) ((r)&0xf) + +#define OTG_GCCFG_SOFOUTEN (1u<<20) +#define OTG_GCCFG_VBUSBSEN (1u<<19) +#define OTG_GCCFG_VBUSASEN (1u<<18) +#define OTG_GCCFG_PWRDWN (1u<<16) + +#define OTG_HCFG_FSLSS (1u<<2) +#define OTG_HCFG_FSLSPCS (3u<<0) +#define OTG_HCFG_FSLSPCS_48 (1u<<0) +#define OTG_HCFG_FSLSPCS_6 (2u<<0) + +#define OTG_HPRT_PSPD_FULL (1u<<17) +#define OTG_HPRT_PSPD_LOW (2u<<17) +#define OTG_HPRT_PSPD_MASK (1u<<17) /* read-only */ +#define OTG_HPRT_PPWR (1u<<12) +#define OTG_HPRT_PRST (1u<< 8) +#define OTG_HPRT_PSUSP (1u<< 7) +#define OTG_HPRT_PRES (1u<< 6) +#define OTG_HPRT_POCCHNG (1u<< 5) /* raises HPRTINT */ +#define OTG_HPRT_POCA (1u<< 4) +#define OTG_HPRT_PENCHNG (1u<< 3) /* raises HPRTINT */ +#define OTG_HPRT_PENA (1u<< 2) +#define OTG_HPRT_PCDET (1u<< 1) /* raises HPRTINT */ +#define OTG_HPRT_PCSTS (1u<< 0) +#define OTG_HPRT_INTS (OTG_HPRT_POCCHNG|OTG_HPRT_PENCHNG|OTG_HPRT_PCDET| \ + OTG_HPRT_PENA) /* PENA is also set-to-clear */ + +/* HCINTSTS and HCINTMSK */ +#define OTG_HCINT_DTERR (1u<<10) +#define OTG_HCINT_FRMOR (1u<< 9) +#define OTG_HCINT_BBERR (1u<< 8) +#define OTG_HCINT_TXERR (1u<< 7) +#define OTG_HCINT_NYET (1u<< 6) /* high-speed only; not STM32F10x */ +#define OTG_HCINT_ACK (1u<< 5) +#define OTG_HCINT_NAK (1u<< 4) +#define OTG_HCINT_STALL (1u<< 3) +#define OTG_HCINT_CHH (1u<< 1) +#define OTG_HCINT_XFRC (1u<< 0) + +#define OTG_HCCHAR_CHENA (1u<<31) +#define OTG_HCCHAR_CHDIS (1u<<30) +#define OTG_HCCHAR_ODDFRM (1u<<29) +#define OTG_HCCHAR_DAD(x) ((x)<<22) +#define OTG_HCCHAR_MCNT(x) ((x)<<20) +#define OTG_HCCHAR_ETYP_CTRL (0u<<18) +#define OTG_HCCHAR_ETYP_ISO (1u<<18) +#define OTG_HCCHAR_ETYP_BULK (2u<<18) +#define OTG_HCCHAR_ETYP_INT (3u<<18) +#define OTG_HCCHAR_LSDEV (1u<<17) +#define OTG_HCCHAR_EPDIR_OUT (0u<<15) +#define OTG_HCCHAR_EPDIR_IN (1u<<15) +#define OTG_HCCHAR_EPNUM(x) ((x)<<11) +#define OTG_HCCHAR_MPSIZ(x) ((x)<< 0) + +#define OTG_HCTSIZ_DPID_DATA0 (0u<<29) +#define OTG_HCTSIZ_DPID_DATA2 (1u<<29) +#define OTG_HCTSIZ_DPID_DATA1 (2u<<29) +#define OTG_HCTSIZ_DPID_MDATA (3u<<29) +#define OTG_HCTSIZ_DPID_SETUP (3u<<29) +#define OTG_HCTSIZ_PKTCNT(x) ((x)<<19) +#define OTG_HCTSIZ_XFRSIZ(x) ((x)<< 0) + +#define USB_OTG_BASE 0x50000000 + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/inc/time.h b/inc/time.h new file mode 100644 index 0000000..b1503f0 --- /dev/null +++ b/inc/time.h @@ -0,0 +1,37 @@ +/* + * time.h + * + * System-time abstraction over STM32 STK timer. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +typedef uint32_t time_t; + +#define TIME_MHZ STK_MHZ +#define time_us(x) stk_us(x) +#define time_ms(x) stk_ms(x) +#define time_sysclk(x) stk_sysclk(x) +#define sysclk_time(x) sysclk_stk(x) + +time_t time_now(void); + +#define time_diff(x,y) ((int32_t)((y)-(x))) /* d = y - x */ +#define time_add(x,d) ((time_t)((x)+(d))) /* y = x + d */ +#define time_sub(x,d) ((time_t)((x)-(d))) /* y = x - d */ +#define time_since(x) time_diff(x, time_now()) + +void time_init(void); + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/inc/timer.h b/inc/timer.h new file mode 100644 index 0000000..c8f3013 --- /dev/null +++ b/inc/timer.h @@ -0,0 +1,34 @@ +/* + * timer.h + * + * Deadline-based timer callbacks. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +struct timer { + time_t deadline; + void (*cb_fn)(void *); + void *cb_dat; + struct timer *next; +}; + +/* Safe to call from any priority level same or lower than TIMER_IRQ_PRI. */ +void timer_init(struct timer *timer, void (*cb_fn)(void *), void *cb_dat); +void timer_set(struct timer *timer, time_t deadline); +void timer_cancel(struct timer *timer); + +void timers_init(void); + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/inc/util.h b/inc/util.h new file mode 100644 index 0000000..956d9b3 --- /dev/null +++ b/inc/util.h @@ -0,0 +1,164 @@ +/* + * util.h + * + * Utility definitions. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +#define FW_VER "0.0.1a" + +#ifndef NDEBUG +#define ASSERT(p) do { if (!(p)) illegal(); } while (0) +#else +#define ASSERT(p) do { if (0 && (p)) {} } while (0) +#endif + +typedef char bool_t; +#define TRUE 1 +#define FALSE 0 + +#define LONG_MAX ((long int)((~0UL)>>1)) +#define LONG_MIN ((long int)~LONG_MAX) + +#ifndef offsetof +#define offsetof(a,b) __builtin_offsetof(a,b) +#endif +#define container_of(ptr, type, member) ({ \ + typeof( ((type *)0)->member ) *__mptr = (ptr); \ + (type *)( (char *)__mptr - offsetof(type,member) );}) +#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) + +#define min(x,y) ({ \ + const typeof(x) _x = (x); \ + const typeof(y) _y = (y); \ + (void) (&_x == &_y); \ + _x < _y ? _x : _y; }) + +#define max(x,y) ({ \ + const typeof(x) _x = (x); \ + const typeof(y) _y = (y); \ + (void) (&_x == &_y); \ + _x > _y ? _x : _y; }) + +#define min_t(type,x,y) \ + ({ type __x = (x); type __y = (y); __x < __y ? __x: __y; }) +#define max_t(type,x,y) \ + ({ type __x = (x); type __y = (y); __x > __y ? __x: __y; }) + +/* Fast memset/memcpy: Pointers must be word-aligned, count must be a non-zero + * multiple of 32 bytes. */ +void memset_fast(void *s, int c, size_t n); +void memcpy_fast(void *dest, const void *src, size_t n); + +void *memset(void *s, int c, size_t n); +void *memcpy(void *dest, const void *src, size_t n); +void *memmove(void *dest, const void *src, size_t n); +int memcmp(const void *s1, const void *s2, size_t n); + +size_t strlen(const char *s); +size_t strnlen(const char *s, size_t maxlen); +int strcmp(const char *s1, const char *s2); +int strncmp(const char *s1, const char *s2, size_t n); +char *strrchr(const char *s, int c); +int tolower(int c); +int isspace(int c); + +long int strtol(const char *nptr, char **endptr, int base); + +int vsnprintf(char *str, size_t size, const char *format, va_list ap) + __attribute__ ((format (printf, 3, 0))); + +int snprintf(char *str, size_t size, const char *format, ...) + __attribute__ ((format (printf, 3, 4))); + +#define le16toh(x) (x) +#define le32toh(x) (x) +#define htole16(x) (x) +#define htole32(x) (x) +#define be16toh(x) _rev16(x) +#define be32toh(x) _rev32(x) +#define htobe16(x) _rev16(x) +#define htobe32(x) _rev32(x) + +/* Arena-based memory allocation */ +void *arena_alloc(uint32_t sz); +uint32_t arena_total(void); +uint32_t arena_avail(void); +void arena_init(void); + +/* Board-specific callouts */ +void board_init(void); +extern uint8_t board_id; + +#ifndef NDEBUG + +/* Serial console control */ +void console_init(void); +void console_sync(void); +void console_crash_on_input(void); + +/* Serial console output */ +int vprintk(const char *format, va_list ap) + __attribute__ ((format (printf, 1, 0))); +int printk(const char *format, ...) + __attribute__ ((format (printf, 1, 2))); + +#else /* NDEBUG */ + +#define console_init() ((void)0) +#define console_sync() IRQ_global_disable() +#define console_crash_on_input() ((void)0) +static inline int vprintk(const char *format, va_list ap) { return 0; } +static inline int printk(const char *format, ...) { return 0; } + +#endif + +/* USB */ +void usb_init(void); +void usb_process(void); +int ep_rx_ready(uint8_t ep); +void usb_read(uint8_t ep, void *buf, uint32_t len); +bool_t ep_tx_ready(uint8_t ep); +void usb_write(uint8_t ep, const void *buf, uint32_t len); + +/* Floppy */ +void floppy_init(void); +void floppy_reset(void); +void floppy_configured(void); +void floppy_process(void); + +/* CRC-CCITT */ +uint16_t crc16_ccitt(const void *buf, size_t len, uint16_t crc); + +/* Text/data/BSS address ranges. */ +extern char _stext[], _etext[]; +extern char _sdat[], _edat[], _ldat[]; +extern char _sbss[], _ebss[]; + +/* Stacks. */ +extern uint32_t _thread_stacktop[], _thread_stackbottom[]; +extern uint32_t _irq_stacktop[], _irq_stackbottom[]; + +/* Default exception handler. */ +void EXC_unused(void); + +/* IRQ priorities, 0 (highest) to 15 (lowest). */ +#define RESET_IRQ_PRI 0 +#define INDEX_IRQ_PRI 2 +#define TIMER_IRQ_PRI 4 +#define USB_IRQ_PRI 14 +#define CONSOLE_IRQ_PRI 15 + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/scripts/49-greaseweazle.rules b/scripts/49-greaseweazle.rules new file mode 100644 index 0000000..bea55f6 --- /dev/null +++ b/scripts/49-greaseweazle.rules @@ -0,0 +1,26 @@ +# UDEV Rules for Greaseweazle +# +# This file must be placed at: +# +# /etc/udev/rules.d/49-greaseweazle.rules (preferred location) +# or +# /lib/udev/rules.d/49-greaseweazle.rules (req'd on some broken systems) +# +# To install, type this command in a terminal: +# sudo cp 49-greaseweazle.rules /etc/udev/rules.d/49-greaseweazle.rules +# +# After this file is installed, physically unplug and reconnect Greaseweazle. +# +ATTRS{manufacturer}=="Keir Fraser", ATTRS{product}=="GreaseWeazle", \ + ENV{ID_MM_DEVICE_IGNORE}="1" +ATTRS{manufacturer}=="Keir Fraser", ATTRS{product}=="GreaseWeazle", \ + ENV{MTP_NO_PROBE}="1" +ATTRS{manufacturer}=="Keir Fraser", ATTRS{product}=="GreaseWeazle", \ + SUBSYSTEMS=="usb", MODE:="0666" +ATTRS{manufacturer}=="Keir Fraser", ATTRS{product}=="GreaseWeazle", \ + KERNEL=="ttyACM*", MODE:="0666" +# +# If you share your linux system with other users, or just don't like the +# idea of write permission for everybody, you can replace MODE:="0666" with +# OWNER:="yourusername" to create the device owned by you, or with +# GROUP:="somegroupname" and mange access using standard unix groups. diff --git a/scripts/foop b/scripts/foop new file mode 100644 index 0000000..bcef74d --- /dev/null +++ b/scripts/foop @@ -0,0 +1,20 @@ +# UDEV Rules for Greaseweazle +# +# To install, type this command in a terminal: +# sudo cp 49-greaseweazle.rules /etc/udev/rules.d/. +# +# After this file is installed, physically unplug and reconnect Greaseweazle. +# +ATTRS{manufacturer}=="Keir Fraser", ATTRS{product}=="GreaseWeazle", \ + ENV{ID_MM_DEVICE_IGNORE}="1" +ATTRS{manufacturer}=="Keir Fraser", ATTRS{product}=="GreaseWeazle", \ + ENV{MTP_NO_PROBE}="1" +ATTRS{manufacturer}=="Keir Fraser", ATTRS{product}=="GreaseWeazle", \ + SUBSYSTEMS=="usb", MODE:="0666" +ATTRS{manufacturer}=="Keir Fraser", ATTRS{product}=="GreaseWeazle", \ + KERNEL=="ttyACM*", MODE:="0666" +# +# If you share your linux system with other users, or just don't like the +# idea of write permission for everybody, you can replace MODE:="0666" with +# OWNER:="yourusername" to create the device owned by you, or with +# GROUP:="somegroupname" and mange access using standard unix groups. diff --git a/scripts/gw.py b/scripts/gw.py new file mode 100644 index 0000000..485241b --- /dev/null +++ b/scripts/gw.py @@ -0,0 +1,346 @@ +# gw.py +# +# Greaseweazle control script. +# +# Written & released by Keir Fraser +# +# This is free and unencumbered software released into the public domain. +# See the file COPYING for more details, or visit . + +import sys, struct, argparse, serial, collections +from timeit import default_timer as timer + +# 40MHz +scp_freq = 40000000 + +CMD_GET_INFO = 0 +CMD_SEEK = 1 +CMD_SIDE = 2 +CMD_SET_DELAYS = 3 +CMD_GET_DELAYS = 4 +CMD_MOTOR = 5 +CMD_READ_FLUX = 6 +CMD_WRITE_FLUX = 7 +CMD_GET_FLUX_STATUS = 8 +CMD_GET_READ_INFO = 9 + +ACK_OKAY = 0 +ACK_BAD_COMMAND = 1 +ACK_NO_INDEX = 2 +ACK_NO_TRK0 = 3 +ACK_FLUX_OVERFLOW = 4 +ACK_FLUX_UNDERFLOW = 5 +ACK_WRPROT = 6 +ACK_MAX = 6 + +ack_str = [ + "Okay", "Bad Command", "No Index", "Track 0 not found", + "Flux Overflow", "Flux Underflow", "Disk is Write Protected" ] + +class CmdError(Exception): + def __init__(self, code): + self.code = code + def __str__(self): + if self.code <= ACK_MAX: + return ack_str[self.code] + return "Unknown Error (%u)" % self.code + +def send_cmd(cmd): + ser.write(cmd) + (c,r) = struct.unpack("2B", ser.read(2)) + assert c == cmd[0] + if r != 0: + raise CmdError(r) + +def get_fw_info(): + send_cmd(struct.pack("3B", CMD_GET_INFO, 3, 0)) + x = struct.unpack("<4BI24x", ser.read(32)) + return x + +def print_fw_info(info): + (major, minor, max_revs, max_cmd, freq) = info + print("Greaseweazle v%u.%u" % (major, minor)) + print("Max revs %u" % (max_revs)) + print("Max cmd %u" % (max_cmd)) + print("Sample frequency: %.2f MHz" % (freq / 1000000)) + +def seek(cyl, side): + send_cmd(struct.pack("3B", CMD_SEEK, 3, cyl)) + send_cmd(struct.pack("3B", CMD_SIDE, 3, side)) + +def get_delays(): + send_cmd(struct.pack("2B", CMD_GET_DELAYS, 2)) + return struct.unpack("<4H", ser.read(4*2)) + +def print_delays(x): + (step_delay, seek_settle, motor_delay, auto_off) = x + print("Step Delay: %ums" % step_delay) + print("Settle Time: %ums" % seek_settle) + print("Motor Delay: %ums" % motor_delay) + print("Auto Off: %ums" % auto_off) + +def set_delays(step_delay = None, seek_settle = None, + motor_delay = None, auto_off = None): + (_step_delay, _seek_settle, _motor_delay, _auto_off) = get_delays() + if not step_delay: step_delay = _step_delay + if not seek_settle: seek_settle = _seek_settle + if not motor_delay: motor_delay = _motor_delay + if not auto_off: auto_off = _auto_off + send_cmd(struct.pack("<2B4H", CMD_SET_DELAYS, 10, + step_delay, seek_settle, motor_delay, auto_off)) + +def motor(state): + send_cmd(struct.pack("3B", CMD_MOTOR, 3, int(state))) + +def get_read_info(): + send_cmd(struct.pack("2B", CMD_GET_READ_INFO, 2)) + x = [] + for i in range(7): + x.append(struct.unpack("<2I", ser.read(2*4))) + return x + +def print_read_info(info): + for (time, samples) in info: + print("%u ticks, %u samples" % (time, samples)) + +def write_flux(flux): + start = timer() + x = bytearray() + for val in flux: + if val == 0: + pass + elif val < 250: + x.append(val) + else: + high = val // 250 + if high <= 5: + x.append(249+high) + x.append(1 + val%250) + else: + x.append(255) + x.append(1 | (val<<1) & 255) + x.append(1 | (val>>6) & 255) + x.append(1 | (val>>13) & 255) + x.append(1 | (val>>20) & 255) + x.append(0) # End of Stream + end = timer() + #print("%u flux -> %u bytes in %f seconds" % (len(flux), len(x), end-start)) + retry = 0 + while True: + start = timer() + send_cmd(struct.pack("2B", CMD_WRITE_FLUX, 2)) + ser.write(x) + ser.read(1) # Sync with Greaseweazle + try: + send_cmd(struct.pack("2B", CMD_GET_FLUX_STATUS, 2)) + except CmdError as error: + if error.code == ACK_FLUX_UNDERFLOW and retry < 5: + retry += 1 + print("Retry #%u..." % retry) + continue; + raise + end = timer() + #print("Track written in %f seconds" % (end-start)) + break + +def read_flux(nr_revs): + retry = 0 + while True: + start = timer() + x = collections.deque() + send_cmd(struct.pack("3B", CMD_READ_FLUX, 3, nr_revs)) + nr = 0 + while True: + x += ser.read(1) + x += ser.read(ser.in_waiting) + nr += 1; + if x[-1] == 0: + break + try: + send_cmd(struct.pack("2B", CMD_GET_FLUX_STATUS, 2)) + except CmdError as error: + if error.code == ACK_FLUX_OVERFLOW and retry < 5: + retry += 1 + print("Retry #%u..." % retry) + del x + continue; + raise + end = timer() + break + + #print("Read %u bytes in %u batches in %f seconds" % (len(x), nr, end-start)) + + start = timer() + y = [] + while x: + i = x.popleft() + if i < 250: + y.append(i) + elif i == 255: + val = (x.popleft() & 254) >> 1 + val += (x.popleft() & 254) << 6 + val += (x.popleft() & 254) << 13 + val += (x.popleft() & 254) << 20 + y.append(val) + else: + val = (i - 249) * 250 + val += x.popleft() - 1 + y.append(val) + assert y[-1] == 0 + y = y[:-1] + end = timer() + + #print("Processed %u flux values in %f seconds" % (len(y), end-start)) + + return y + +def read(args): + factor = scp_freq / sample_freq + trk_dat = bytearray() + trk_offs = [] + if args.single_sided: + track_range = range(args.scyl, args.ecyl+1) + nr_sides = 1 + else: + track_range = range(args.scyl*2, (args.ecyl+1)*2) + nr_sides = 2 + for i in track_range: + cyl = i >> (nr_sides - 1) + side = i & (nr_sides - 1) + print("\rReading Track %u.%u..." % (cyl, side), end="") + trk_offs.append(len(trk_dat)) + seek(cyl, side) + flux = read_flux(args.revs) + info = get_read_info()[:args.revs] + #print_read_info(info) + trk_dat += struct.pack("<3sB", b"TRK", i) + dat_off = 4 + args.revs*12 + for (time, samples) in info: + time = int(round(time * factor)) + trk_dat += struct.pack("= 65536: + trk_dat.append(0) + trk_dat.append(0) + val -= 65536 + if val == 0: + val = 1 + trk_dat.append(val>>8) + trk_dat.append(val&255) + print() + csum = 0 + for x in trk_dat: + csum += x + trk_offs_dat = bytearray() + for x in trk_offs: + trk_offs_dat += struct.pack("> (nr_sides - 1) + side = i & (nr_sides - 1) + print("\rWriting Track %u.%u..." % (cyl, side), end="") + if trk_offs[i] == 0: + continue + seek(cyl, side) + thdr = struct.unpack("<3sBIII", dat[trk_offs[i]:trk_offs[i]+16]) + (sig,_,_,samples,off) = thdr + assert sig == b"TRK" + tdat = dat[trk_offs[i]+off:trk_offs[i]+off+samples*2] + flux = [] + rem = 0.0 + for i in range(0,len(tdat),2): + x = tdat[i]*256 + tdat[i+1] + if x == 0: + rem += 65536.0 + continue + y = x * factor + rem + val = int(round(y)) + rem = y - val + flux.append(val) + write_flux(flux) + print() + +def main(argv): + + actions = { + "read" : read, + "write" : write + } + + parser = argparse.ArgumentParser( + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("action") + parser.add_argument("--revs", type=int, default=3, + help="number of revolutions to read per track") + parser.add_argument("--scyl", type=int, default=0, + help="first cylinder to read/write") + parser.add_argument("--ecyl", type=int, default=81, + help="last cylinder to read/write") + parser.add_argument("--single-sided", action="store_true") +# parser.add_argument("--total", type=float, default=8.0, +# help="total length, seconds") + parser.add_argument("file", help="in/out filename") + args = parser.parse_args(argv[1:]) + + global ser + ser = serial.Serial("/dev/ttyACM0") + ser.send_break() + ser.reset_input_buffer() + + global sample_freq + info = get_fw_info() + #print_fw_info(info) + sample_freq = info[4] + + #set_delays(step_delay=3) + #print_delays(get_delays()) + + actions[args.action](args) + + motor(False) + +if __name__ == "__main__": + try: + main(sys.argv) + except CmdError as error: + print("Command Failed: %s" % error) diff --git a/src/Greaseweazle.ld.S b/src/Greaseweazle.ld.S new file mode 100644 index 0000000..ab1cc25 --- /dev/null +++ b/src/Greaseweazle.ld.S @@ -0,0 +1,56 @@ +ENTRY(vector_table) + +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 64K + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 20K +} +REGION_ALIAS("RO", FLASH); +REGION_ALIAS("RW", RAM); + +SECTIONS +{ + .text : { + _stext = .; + *(.vector_table) + *(.text) + *(.text*) + *(.rodata) + *(.rodata*) + KEEP (*(.init)) + KEEP (*(.fini)) + . = ALIGN(4); + _etext = .; + } >RO + + .data : AT (_etext) { + . = ALIGN(4); + _sdat = .; + *(.data) + *(.data*) + . = ALIGN(4); + _edat = .; + _ldat = LOADADDR(.data); + } >RW + + .bss : { + . = ALIGN(8); + _irq_stackbottom = .; + . = . + 512; + _irq_stacktop = .; + _thread_stackbottom = .; + . = . + 1024; + _thread_stacktop = .; + _sbss = .; + *(.bss) + *(.bss*) + . = ALIGN(4); + _ebss = .; + } >RW + + /DISCARD/ : { + *(.eh_frame) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/src/Makefile b/src/Makefile new file mode 100644 index 0000000..d0fb828 --- /dev/null +++ b/src/Makefile @@ -0,0 +1,16 @@ +OBJS += arena.o +OBJS += board.o +OBJS += cancellation.o +OBJS += crc.o +OBJS += vectors.o +OBJS += main.o +OBJS += string.o +OBJS += stm32f10x.o +OBJS += time.o +OBJS += timer.o +OBJS += util.o +OBJS += floppy.o + +OBJS-$(debug) += console.o + +SUBDIRS += usb diff --git a/src/arena.c b/src/arena.c new file mode 100644 index 0000000..b106c86 --- /dev/null +++ b/src/arena.c @@ -0,0 +1,51 @@ +/* + * arena.c + * + * Arena-based memory allocation. Only one arena, for now. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +#define ram_kb 20 +#define ram_bytes (ram_kb*1024) + +#define heap_bot (_ebss) +#define heap_top ((char *)0x20000000 + ram_bytes) + +static char *heap_p; + +void *arena_alloc(uint32_t sz) +{ + void *p = heap_p; + heap_p += (sz + 3) & ~3; + ASSERT(heap_p <= heap_top); + return p; +} + +uint32_t arena_total(void) +{ + return heap_top - heap_bot; +} + +uint32_t arena_avail(void) +{ + return heap_top - heap_p; +} + +void arena_init(void) +{ + heap_p = heap_bot; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/board.c b/src/board.c new file mode 100644 index 0000000..c27572b --- /dev/null +++ b/src/board.c @@ -0,0 +1,54 @@ +/* + * gotek/board.c + * + * Gotek board-specific setup and management. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +uint8_t board_id; + +/* Pull up currently unused and possibly-floating pins. */ +static void gpio_pull_up_pins(GPIO gpio, uint16_t mask) +{ + unsigned int i; + for (i = 0; i < 16; i++) { + if (mask & 1) + gpio_configure_pin(gpio, i, GPI_pull_up); + mask >>= 1; + } +} + +void board_init(void) +{ + uint16_t pa_skip, pb_skip; + + /* PA0-7 (floppy outputs), PA9-10 (serial console), PA11-12 (USB) */ + pa_skip = 0x1eff; + + /* PB0 (USB disconnect), PB4,6,8,13 (floppy inputs). */ + pb_skip = 0x2151; + + /* Pull up all PCx pins. */ + gpio_pull_up_pins(gpioc, ~0x0000); + + /* Wait for ID to stabilise at PC[15:12]. */ + delay_us(5); + board_id = (gpioc->idr >> 12) & 0xf; + + gpio_pull_up_pins(gpioa, ~pa_skip); + gpio_pull_up_pins(gpiob, ~pb_skip); +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/cancellation.c b/src/cancellation.c new file mode 100644 index 0000000..bfd94f7 --- /dev/null +++ b/src/cancellation.c @@ -0,0 +1,86 @@ +/* + * cancellation.c + * + * Asynchronously-cancellable function calls. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +asm ( + ".global call_cancellable_fn\n" + ".thumb_func \n" + "call_cancellable_fn:\n" + " stmdb.w sp!, {r0, r4, r5, r6, r7, r8, r9, r10, r11, lr}\n" + " str sp, [r0]\n" /* c->sp = PSP */ + " mov r0, r2\n" /* r0 = arg */ + " blx r1\n" /* (*fn)(arg) */ + " ldr r2, [sp]\n" + " movs r1, #0\n" + " str r1, [r2]\n" /* c->sp = NULL */ + "do_cancel:\n" + " add sp, #4\n" + " ldmia.w sp!, {r4, r5, r6, r7, r8, r9, r10, r11, pc}\n" + ); + +void do_cancel(void); + +/* An exception context for cancel_call(), when initially called from Thread + * context. */ +void EXC_sv_call(void) __attribute__((alias("EXC_do_cancel"))); +static struct cancellation *exc_cancel; +static void EXC_do_cancel(void) +{ + cancel_call(exc_cancel); + exc_cancel = NULL; +} + +void cancel_call(struct cancellation *c) +{ + struct exception_frame *frame; + uint32_t *new_frame; + + /* Bail if the cancellable context is inactive/cancelled. */ + if (c->sp == NULL) + return; + + /* Switch to exception context if we are not there already. */ + if (!in_exception()) { + exc_cancel = c; + sv_call(0); + ASSERT(0); /* unreachable */ + } + + /* Modify return frame: Jump to exit of call_cancellable_fn() with + * return code -1 and clean xPSR. */ + frame = (struct exception_frame *)read_special(psp); + frame->r0 = -1; + frame->pc = (uint32_t)do_cancel; + frame->psr &= 1u<<24; /* Preserve Thumb mode; clear everything else */ + + /* Find new frame address, set STKALIGN if misaligned. */ + new_frame = c->sp - 8; + if ((uint32_t)new_frame & 4) { + new_frame--; + frame->psr |= 1u<<9; + } + + /* Copy the stack frame and update Process SP. */ + memmove(new_frame, frame, 32); + write_special(psp, new_frame); + + /* Do this work at most once per invocation of call_cancellable_fn. */ + c->sp = NULL; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/console.c b/src/console.c new file mode 100644 index 0000000..833aa28 --- /dev/null +++ b/src/console.c @@ -0,0 +1,168 @@ +/* + * console.c + * + * printf-style interface to USART1. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +#define BAUD 3000000 /* 3Mbaud */ + +#define DMA1_CH4_IRQ 14 +void IRQ_14(void) __attribute__((alias("IRQ_dma1_ch4_tc"))); + +#define USART1_IRQ 37 + +/* We stage serial output in a ring buffer. DMA occurs from the ring buffer; + * the consumer index being updated each time a DMA sequence completes. */ +static char ring[2048]; +#define MASK(x) ((x)&(sizeof(ring)-1)) +static unsigned int cons, prod, dma_sz; + +/* The console can be set into synchronous mode in which case DMA is disabled + * and the transmit-empty flag is polled manually for each byte. */ +static bool_t sync_console; + +static void kick_tx(void) +{ + if (sync_console) { + + while (cons != prod) { + while (!(usart1->sr & USART_SR_TXE)) + cpu_relax(); + usart1->dr = ring[MASK(cons++)]; + } + + } else if (!dma_sz && (cons != prod)) { + + dma_sz = min(MASK(prod-cons), sizeof(ring)-MASK(cons)); + dma1->ch4.cmar = (uint32_t)(unsigned long)&ring[MASK(cons)]; + dma1->ch4.cndtr = dma_sz; + dma1->ch4.ccr = (DMA_CCR_MSIZE_8BIT | + /* The manual doesn't allow byte accesses to usart. */ + DMA_CCR_PSIZE_16BIT | + DMA_CCR_MINC | + DMA_CCR_DIR_M2P | + DMA_CCR_TCIE | + DMA_CCR_EN); + + } +} + +static void IRQ_dma1_ch4_tc(void) +{ + /* Clear the DMA controller. */ + dma1->ch4.ccr = 0; + dma1->ifcr = DMA_IFCR_CGIF(4); + + /* Update ring state. */ + cons += dma_sz; + dma_sz = 0; + + /* Kick off more transmit activity. */ + kick_tx(); +} + +int vprintk(const char *format, va_list ap) +{ + static char str[128]; + char *p, c; + int n; + + IRQ_global_disable(); + + n = vsnprintf(str, sizeof(str), format, ap); + + p = str; + while (((c = *p++) != '\0') && ((prod-cons) != (sizeof(ring) - 1))) { + switch (c) { + case '\r': /* CR: ignore as we generate our own CR/LF */ + break; + case '\n': /* LF: convert to CR/LF (usual terminal behaviour) */ + ring[MASK(prod++)] = '\r'; + /* fall through */ + default: + ring[MASK(prod++)] = c; + break; + } + } + + kick_tx(); + + IRQ_global_enable(); + + return n; +} + +int printk(const char *format, ...) +{ + va_list ap; + int n; + + va_start(ap, format); + n = vprintk(format, ap); + va_end(ap); + + return n; +} + +void console_sync(void) +{ + if (sync_console) + return; + + IRQ_global_disable(); + + sync_console = TRUE; + + /* Wait for DMA completion and then kick off synchronous mode. */ + while (dma1->ch4.cndtr) + cpu_relax(); + IRQ_dma1_ch4_tc(); + + /* Leave IRQs globally disabled. */ +} + +void console_init(void) +{ + /* Turn on the clocks. */ + rcc->apb2enr |= RCC_APB2ENR_USART1EN; + + /* Enable TX pin (PA9) for USART output, RX pin (PA10) as input. */ + gpio_configure_pin(gpioa, 9, AFO_pushpull(_10MHz)); + gpio_configure_pin(gpioa, 10, GPI_pull_up); + + /* BAUD, 8n1. */ + usart1->brr = SYSCLK / BAUD; + usart1->cr1 = (USART_CR1_UE | USART_CR1_TE | USART_CR1_RE); + usart1->cr3 = USART_CR3_DMAT; + + /* Initialise DMA1 channel 4 and its completion interrupt. */ + dma1->ch4.cpar = (uint32_t)(unsigned long)&usart1->dr; + dma1->ifcr = DMA_IFCR_CGIF(4); + IRQx_set_prio(DMA1_CH4_IRQ, CONSOLE_IRQ_PRI); + IRQx_enable(DMA1_CH4_IRQ); +} + +/* Debug helper: if we get stuck somewhere, calling this beforehand will cause + * any serial input to cause a crash dump of the stuck context. */ +void console_crash_on_input(void) +{ + (void)usart1->dr; /* clear UART_SR_RXNE */ + usart1->cr1 |= USART_CR1_RXNEIE; + IRQx_set_prio(USART1_IRQ, RESET_IRQ_PRI); + IRQx_enable(USART1_IRQ); +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/crc.c b/src/crc.c new file mode 100644 index 0000000..a3a43b5 --- /dev/null +++ b/src/crc.c @@ -0,0 +1,64 @@ +/* + * crc.c + * + * Table-based CRC16-CCITT. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +static const uint16_t crc16tab[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, + 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, + 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, + 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, + 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, + 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, + 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, + 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, + 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, + 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, + 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, + 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, + 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, + 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, + 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, + 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, + 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, + 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, + 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, + 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, + 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, + 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, + 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0 +}; + +uint16_t crc16_ccitt(const void *buf, size_t len, uint16_t crc) +{ + unsigned int i; + const uint8_t *b = buf; + for (i = 0; i < len; i++) + crc = crc16tab[(crc>>8)^*b++] ^ (crc<<8); + return crc; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/floppy.c b/src/floppy.c new file mode 100644 index 0000000..e1b6e5f --- /dev/null +++ b/src/floppy.c @@ -0,0 +1,940 @@ +/* + * floppy.c + * + * Floppy interface control. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +#define O_FALSE 1 +#define O_TRUE 0 + +#define GPO_bus GPO_opendrain(_2MHz,O_FALSE) +#define AFO_bus (AFO_opendrain(_2MHz) | (O_FALSE<<4)) + +#define m(bitnr) (1u<<(bitnr)) + +#define gpio_floppy gpiob + +/* Input pins */ +#define pin_index 6 /* PB6 */ +#define pin_trk0 7 /* PB7 */ +#define pin_wrprot 8 /* PB8 */ +#define get_index() gpio_read_pin(gpio_floppy, pin_index) +#define get_trk0() gpio_read_pin(gpio_floppy, pin_trk0) +#define get_wrprot() gpio_read_pin(gpio_floppy, pin_wrprot) + +/* Output pins. */ +#define pin_densel 9 /* PB9 */ +#define pin_sel0 10 /* PB10 */ +#define pin_motor 11 /* PB11 */ +#define pin_dir 12 /* PB12 */ +#define pin_step 13 /* PB13 */ +#define pin_wgate 14 /* PB14 */ +#define pin_side 15 /* PB15 */ + +/* Track and modify states of output pins. */ +static struct { + bool_t densel; + bool_t sel0; + bool_t motor; + bool_t dir; + bool_t step; + bool_t wgate; + bool_t side; +} pins; +#define read_pin(pin) pins.pin +#define write_pin(pin, level) ({ \ + gpio_write_pin(gpio_floppy, pin_##pin, level ? O_TRUE : O_FALSE); \ + pins.pin = level; }) + +#define gpio_data gpiob + +#define pin_rdata 3 +#define tim_rdata (tim2) +#define dma_rdata (dma1->ch7) + +#define pin_wdata 4 +#define tim_wdata (tim3) +#define dma_wdata (dma1->ch3) + +#define irq_index 23 +void IRQ_23(void) __attribute__((alias("IRQ_INDEX_changed"))); /* EXTI9_5 */ +static volatile struct index { + unsigned int count; + time_t timestamp; + time_t duration; + unsigned int read_prod; +} index; + +/* A DMA buffer for running a timer associated with a floppy-data I/O pin. */ +static struct dma_ring { + /* Indexes into the buf[] ring buffer. */ + uint16_t cons; /* dma_rd: our consumer index for flux samples */ + union { + uint16_t prod; /* dma_wr: our producer index for flux samples */ + uint16_t prev_sample; /* dma_rd: previous CCRx sample value */ + }; + /* DMA ring buffer of timer values (ARR or CCRx). */ + uint16_t buf[512]; +} dma; + +static enum { + ST_inactive, + ST_command_wait, + ST_read_flux_wait_index, + ST_read_flux, + ST_read_flux_drain, + ST_write_flux_wait_data, + ST_write_flux_wait_index, + ST_write_flux, + ST_write_flux_drain, +} floppy_state = ST_inactive; + +#define FLOPPY_EP 2 +#define FLOPPY_MPS 64 + +static uint8_t u_buf[8192]; +static uint32_t u_cons, u_prod; +#define U_MASK(x) ((x)&(sizeof(u_buf)-1)) + +static struct delay_params { + uint16_t step_delay; + uint16_t seek_settle; + uint16_t motor_delay; + uint16_t auto_off; +} delay_params = { + .step_delay = 3, + .seek_settle = 15, + .motor_delay = 750, + .auto_off = 3000 +}; + +static void step_one_out(void) +{ + write_pin(dir, FALSE); + delay_us(10); + write_pin(step, TRUE); + delay_us(10); + write_pin(step, FALSE); + delay_ms(delay_params.step_delay); +} + +static void step_one_in(void) +{ + write_pin(dir, TRUE); + delay_us(10); + write_pin(step, TRUE); + delay_us(10); + write_pin(step, FALSE); + delay_ms(delay_params.step_delay); +} + +static int cur_cyl = -1; + +static void drive_select(void) +{ + write_pin(sel0, TRUE); + delay_us(10); +} + +static void drive_deselect(void) +{ + delay_us(10); + write_pin(sel0, FALSE); +} + +static bool_t floppy_seek(unsigned int cyl) +{ + drive_select(); + + if ((cyl == 0) || (cur_cyl < 0)) { + + unsigned int i; + for (i = 0; i < 256; i++) { + if (get_trk0() == LOW) + break; + step_one_out(); + } + cur_cyl = 0; + if (get_trk0() == HIGH) { + drive_deselect(); + cur_cyl = -1; + return FALSE; + } + + } + + if (cur_cyl < 0) { + + } else if (cur_cyl <= cyl) { + + unsigned int nr = cyl - cur_cyl; + while (nr--) + step_one_in(); + + } else { + + unsigned int nr = cur_cyl - cyl; + while (nr--) + step_one_out(); + + } + + drive_deselect(); + + delay_ms(delay_params.seek_settle); + cur_cyl = cyl; + + return TRUE; +} + +static void floppy_motor(bool_t on) +{ + if (read_pin(motor) == on) + return; + write_pin(motor, on); + if (on) + delay_ms(delay_params.motor_delay); +} + +static void floppy_flux_end(void) +{ + /* Turn off timers. */ + tim_rdata->ccer = 0; + tim_rdata->cr1 = 0; + tim_rdata->sr = 0; /* dummy, drains any pending DMA */ + tim_wdata->cr1 = 0; + tim_wdata->sr = 0; /* dummy, drains any pending DMA */ + + /* Turn off DMA. */ + dma_rdata.ccr = 0; + dma_wdata.ccr = 0; + + /* Turn off write pins. */ + write_pin(wgate, FALSE); + gpio_configure_pin(gpio_data, pin_wdata, GPO_bus); +} + +void floppy_reset(void) +{ + unsigned int i; + + floppy_state = ST_inactive; + + floppy_flux_end(); + + /* Turn off all output pins. */ + for (i = 9; i <= 15; i++) + gpio_write_pin(gpio_floppy, i, O_FALSE); + memset(&pins, 0, sizeof(pins)); +} + +void floppy_init(void) +{ + unsigned int i, GPI_bus; + + /* Output pins, unbuffered. */ + for (i = 9; i <= 15; i++) + gpio_configure_pin(gpio_floppy, i, GPO_bus); + + gpio_configure_pin(gpio_floppy, pin_index, GPI_pull_down); + delay_us(10); + GPI_bus = (get_index() == LOW) ? GPI_pull_up : GPI_floating; + printk("Floppy Inputs: %sternal Pullup\n", + (GPI_bus == GPI_pull_up) ? "In" : "Ex"); + + /* Input pins. */ + for (i = 6; i <= 8; i++) + gpio_configure_pin(gpio_floppy, i, GPI_bus); + + /* RDATA/WDATA */ + gpio_configure_pin(gpio_data, pin_rdata, GPI_bus); + gpio_configure_pin(gpio_data, pin_wdata, GPO_bus); + afio->mapr |= (AFIO_MAPR_TIM2_REMAP_PARTIAL_1 + | AFIO_MAPR_TIM3_REMAP_PARTIAL); + + /* PB[15:0] -> EXT[15:0] */ + afio->exticr1 = afio->exticr2 = afio->exticr3 = afio->exticr4 = 0x1111; + + /* Configure INDEX-changed IRQ. */ + exti->rtsr = 0; + exti->imr = exti->ftsr = m(pin_index); + IRQx_set_prio(irq_index, INDEX_IRQ_PRI); + IRQx_enable(irq_index); + + /* RDATA Timer setup: + * The counter runs from 0x0000-0xFFFF inclusive at full SYSCLK rate. + * + * Ch.2 (RDATA) is in Input Capture mode, sampling on every clock and with + * no input prescaling or filtering. Samples are captured on the falling + * edge of the input (CCxP=1). DMA is used to copy the sample into a ring + * buffer for batch processing in the DMA-completion ISR. */ + tim_rdata->psc = 0; + tim_rdata->arr = 0xffff; + tim_rdata->ccmr1 = TIM_CCMR1_CC2S(TIM_CCS_INPUT_TI1); + tim_rdata->dier = TIM_DIER_CC2DE; + tim_rdata->cr2 = 0; + + /* RDATA DMA setup: From the RDATA Timer's CCRx into a circular buffer. */ + dma_rdata.cpar = (uint32_t)(unsigned long)&tim_rdata->ccr2; + dma_rdata.cmar = (uint32_t)(unsigned long)dma.buf; + + /* WDATA Timer setup: + * The counter is incremented at full SYSCLK rate. + * + * Ch.1 (WDATA) is in PWM mode 1. It outputs O_TRUE for 400ns and then + * O_FALSE until the counter reloads. By changing the ARR via DMA we alter + * the time between (fixed-width) O_TRUE pulses, mimicking floppy drive + * timings. */ + tim_wdata->psc = 0; + tim_wdata->ccmr1 = (TIM_CCMR1_CC1S(TIM_CCS_OUTPUT) | + TIM_CCMR1_OC1M(TIM_OCM_PWM1)); + tim_wdata->ccer = TIM_CCER_CC1E | ((O_TRUE==0) ? TIM_CCER_CC1P : 0); + tim_wdata->ccr1 = sysclk_ns(400); + tim_wdata->dier = TIM_DIER_UDE; + tim_wdata->cr2 = 0; + + /* WDATA DMA setup: From a circular buffer into the WDATA Timer's ARR. */ + dma_wdata.cpar = (uint32_t)(unsigned long)&tim_wdata->arr; + dma_wdata.cmar = (uint32_t)(unsigned long)dma.buf; +} + + +/* CMD_GET_INFO, length=3, 0. Returns 32 bytes after ACK. */ +#define CMD_GET_INFO 0 +/* CMD_SEEK, length=3, cyl# */ +#define CMD_SEEK 1 +/* CMD_SIDE, length=3, side# (0=bottom) */ +#define CMD_SIDE 2 +/* CMD_SET_DELAYS, length=2+4*2, */ +#define CMD_SET_DELAYS 3 +/* CMD_GET_DELAYS, length=2. Returns 4*2 bytes after ACK. */ +#define CMD_GET_DELAYS 4 +/* CMD_MOTOR, length=3, motor_state */ +#define CMD_MOTOR 5 +/* CMD_READ_FLUX, length=3, #revs. Returns flux readings until EOStream. */ +#define CMD_READ_FLUX 6 +/* CMD_WRITE_FLUX, length=2. Host follows with flux readings until EOStream. */ +#define CMD_WRITE_FLUX 7 +/* CMD_GET_FLUX_STATUS, length=2. Last read/write status returned in ACK. */ +#define CMD_GET_FLUX_STATUS 8 +/* CMD_GET_READ_INFO, length=2. Returns 7*8 bytes after ACK. */ +#define CMD_GET_READ_INFO 9 + +#define ACK_OKAY 0 +#define ACK_BAD_COMMAND 1 +#define ACK_NO_INDEX 2 +#define ACK_NO_TRK0 3 +#define ACK_FLUX_OVERFLOW 4 +#define ACK_FLUX_UNDERFLOW 5 +#define ACK_WRPROT 6 + +const static struct __packed gw_info { + uint8_t fw_major; + uint8_t fw_minor; + uint8_t max_rev; + uint8_t max_cmd; + uint32_t sample_freq; +} gw_info = { + .fw_major = 0, .fw_minor = 1, + .max_rev = 7, .max_cmd = CMD_GET_READ_INFO, + .sample_freq = SYSCLK_MHZ * 1000000u +}; + +/* + * READ PATH + */ + +static struct { + time_t start; + uint8_t status; + uint8_t rev; + uint8_t nr_revs; + bool_t packet_ready; + bool_t write_finished; + unsigned int packet_len; + unsigned int nr_samples; + uint8_t packet[FLOPPY_MPS]; +} rw; + +static struct { + uint32_t time; + uint32_t samples; +} read_info[7]; + +static bool_t rdata_encode_flux(void) +{ + const uint16_t buf_mask = ARRAY_SIZE(dma.buf) - 1; + uint16_t cons, prod, prev = dma.prev_sample, curr, next; + unsigned int nr_samples; + + /* Find out where the DMA engine's producer index has got to. */ + prod = ARRAY_SIZE(dma.buf) - dma_rdata.cndtr; + nr_samples = (prod - dma.cons) & buf_mask; + + if (rw.rev != index.count) { + unsigned int final_samples = U_MASK(index.read_prod - dma.cons); + read_info[rw.rev].time = sysclk_stk(index.duration); + read_info[rw.rev].samples = rw.nr_samples + final_samples; + rw.rev++; + nr_samples -= final_samples; + rw.nr_samples = 0; + } + + rw.nr_samples += nr_samples; + + /* Process the flux timings into the raw bitcell buffer. */ + for (cons = dma.cons; cons != prod; cons = (cons+1) & buf_mask) { + next = dma.buf[cons]; + curr = next - prev; + + if (curr == 0) { + /* 0: Skip. */ + } else if (curr < 250) { + /* 1-249: One byte. */ + u_buf[U_MASK(u_prod++)] = curr; + } else { + unsigned int high = curr / 250; + if (high <= 5) { + /* 250-1500: Two bytes. */ + u_buf[U_MASK(u_prod++)] = 249 + high; + u_buf[U_MASK(u_prod++)] = 1 + (curr % 250); + } else { + /* 1501-(2^28-1): Five bytes */ + u_buf[U_MASK(u_prod++)] = 0xff; + u_buf[U_MASK(u_prod++)] = 1 | (curr << 1); + u_buf[U_MASK(u_prod++)] = 1 | (curr >> 6); + u_buf[U_MASK(u_prod++)] = 1 | (curr >> 13); + u_buf[U_MASK(u_prod++)] = 1 | (curr >> 20); + } + } + + prev = next; + } + + /* Save our progress for next time. */ + dma.cons = cons; + dma.prev_sample = prev; + return FALSE; +} + +static void floppy_read_prep(unsigned int revs) +{ + /* Start DMA. */ + dma_rdata.cndtr = ARRAY_SIZE(dma.buf); + dma_rdata.ccr = (DMA_CCR_PL_HIGH | + DMA_CCR_MSIZE_16BIT | + DMA_CCR_PSIZE_16BIT | + DMA_CCR_MINC | + DMA_CCR_CIRC | + DMA_CCR_DIR_P2M | + DMA_CCR_EN); + + /* DMA soft state. */ + dma.cons = 0; + dma.prev_sample = tim_rdata->cnt; + + drive_select(); + floppy_motor(TRUE); + + index.count = 0; + floppy_state = ST_read_flux_wait_index; + memset(&rw, 0, sizeof(rw)); + rw.nr_revs = revs; + rw.start = time_now(); + rw.status = ACK_OKAY; +} + +static void floppy_read_wait_index(void) +{ + if (index.count == 0) { + if (time_since(rw.start) > time_ms(2000)) { + /* Timeout */ + printk("NO INDEX\n"); + floppy_flux_end(); + rw.status = ACK_NO_INDEX; + floppy_state = ST_read_flux_drain; + } + return; + } + + /* Start timer. */ + tim_rdata->ccer = TIM_CCER_CC2E | TIM_CCER_CC2P; + tim_rdata->cr1 = TIM_CR1_CEN; + + index.count = 0; + floppy_state = ST_read_flux; +} + +static void make_read_packet(unsigned int n) +{ + unsigned int c = U_MASK(u_cons); + unsigned int l = ARRAY_SIZE(u_buf) - c; + if (l < n) { + memcpy(rw.packet, &u_buf[c], l); + memcpy(&rw.packet[l], u_buf, n-l); + } else { + memcpy(rw.packet, &u_buf[c], n); + } + u_cons += n; + rw.packet_ready = TRUE; +} + +static void floppy_read(void) +{ + unsigned int avail = (uint32_t)(u_prod - u_cons); + + if (floppy_state == ST_read_flux) { + + if (index.count >= rw.nr_revs) { + floppy_flux_end(); + floppy_state = ST_read_flux_drain; + } + + rdata_encode_flux(); + avail = (uint32_t)(u_prod - u_cons); + + } else if ((avail < FLOPPY_MPS) + && !rw.packet_ready + && ep_tx_ready(FLOPPY_EP)) { + + memset(rw.packet, 0, FLOPPY_MPS); + make_read_packet(avail); + usb_write(FLOPPY_EP, rw.packet, avail+1); + floppy_configured(); + drive_deselect(); + return; /* FINISHED */ + + } + + if (avail > sizeof(u_buf)) { + /* Overflow */ + printk("OVERFLOW %u %u %u %u\n", u_cons, u_prod, + rw.packet_ready, ep_tx_ready(FLOPPY_EP)); + floppy_flux_end(); + rw.status = ACK_FLUX_OVERFLOW; + floppy_state = ST_read_flux_drain; + u_cons = u_prod = 0; + } + + if (!rw.packet_ready && (avail >= FLOPPY_MPS)) + make_read_packet(FLOPPY_MPS); + + if (rw.packet_ready && ep_tx_ready(FLOPPY_EP)) { + usb_write(FLOPPY_EP, rw.packet, FLOPPY_MPS); + rw.packet_ready = FALSE; + } +} + + +/* + * WRITE PATH + */ + +static unsigned int _wdata_decode_flux(uint16_t *tbuf, unsigned int nr) +{ + unsigned int todo = nr; + + if (todo == 0) + return 0; + + while (u_cons != u_prod) { + uint8_t x = u_buf[U_MASK(u_cons)]; + uint32_t val = x; + if (x == 0) { + /* 0: Terminate */ + u_cons++; + rw.write_finished = TRUE; + goto out; + } else if (x < 250) { + /* 1-249: One byte */ + u_cons++; + } else if (x == 255) { + /* 255: Five bytes */ + uint32_t val; + if ((uint32_t)(u_prod - u_cons) < 5) + goto out; + u_cons++; + val = (u_buf[U_MASK(u_cons++)] ) >> 1; + val |= (u_buf[U_MASK(u_cons++)] & 0xfe) << 6; + val |= (u_buf[U_MASK(u_cons++)] & 0xfe) << 13; + val |= (u_buf[U_MASK(u_cons++)] & 0xfe) << 20; + val = val ?: 1; /* Force non-zero */ + } else { + /* 250-254: Two bytes */ + if ((uint32_t)(u_prod - u_cons) < 2) + goto out; + u_cons++; + val = (x - 249) * 250; + val += u_buf[U_MASK(u_cons++)] - 1; + } + + *tbuf++ = val - 1 ?: 1; /* Force non-zero */ + if (!--todo) + goto out; + } + +out: + return nr - todo; +} + +static void wdata_decode_flux(void) +{ + const uint16_t buf_mask = ARRAY_SIZE(dma.buf) - 1; + uint16_t nr_to_wrap, nr_to_cons, nr, dmacons; + + /* Find out where the DMA engine's consumer index has got to. */ + dmacons = ARRAY_SIZE(dma.buf) - dma_wdata.cndtr; + + /* Find largest contiguous stretch of ring buffer we can fill. */ + nr_to_wrap = ARRAY_SIZE(dma.buf) - dma.prod; + nr_to_cons = (dmacons - dma.prod - 1) & buf_mask; + nr = min(nr_to_wrap, nr_to_cons); + + /* Now attempt to fill the contiguous stretch with flux data calculated + * from buffered bitcell data. */ + dma.prod += _wdata_decode_flux(&dma.buf[dma.prod], nr); + dma.prod &= buf_mask; +} + +static void floppy_process_write_packet(void) +{ + int len = ep_rx_ready(FLOPPY_EP); + + if ((len >= 0) && !rw.packet_ready) { + usb_read(FLOPPY_EP, rw.packet, len); + rw.packet_ready = TRUE; + rw.packet_len = len; + } + + if (rw.packet_ready) { + unsigned int avail = ARRAY_SIZE(u_buf) - (uint32_t)(u_prod - u_cons); + unsigned int n = rw.packet_len; + if (avail >= n) { + unsigned int p = U_MASK(u_prod); + unsigned int l = ARRAY_SIZE(u_buf) - p; + if (l < n) { + memcpy(&u_buf[p], rw.packet, l); + memcpy(u_buf, &rw.packet[l], n-l); + } else { + memcpy(&u_buf[p], rw.packet, n); + } + u_prod += n; + rw.packet_ready = FALSE; + } + } +} + +static void floppy_write_prep(void) +{ + /* Initialise DMA ring indexes (consumer index is implicit). */ + dma_wdata.cndtr = ARRAY_SIZE(dma.buf); + dma.prod = 0; + + drive_select(); + floppy_motor(TRUE); + + floppy_state = ST_write_flux_wait_data; + memset(&rw, 0, sizeof(rw)); + rw.status = ACK_OKAY; + + if (get_wrprot() == LOW) { + floppy_flux_end(); + rw.status = ACK_WRPROT; + floppy_state = ST_write_flux_drain; + } +} + +static void floppy_write_wait_data(void) +{ + floppy_process_write_packet(); + wdata_decode_flux(); + + /* Wait for DMA and input buffers to fill, or write stream to end. */ + if (((dma.prod != (ARRAY_SIZE(dma.buf)-1)) + || ((uint32_t)(u_prod - u_cons) < (ARRAY_SIZE(u_buf) - 512))) + && !rw.write_finished) + return; + + index.count = 0; + floppy_state = ST_write_flux_wait_index; + rw.start = time_now(); + + /* Enable DMA only after flux values are generated. */ + dma_wdata.ccr = (DMA_CCR_PL_HIGH | + DMA_CCR_MSIZE_16BIT | + DMA_CCR_PSIZE_16BIT | + DMA_CCR_MINC | + DMA_CCR_CIRC | + DMA_CCR_DIR_M2P | + DMA_CCR_EN); +} + +static void floppy_write_wait_index(void) +{ + if (index.count == 0) { + if (time_since(rw.start) > time_ms(2000)) { + /* Timeout */ + floppy_flux_end(); + rw.status = ACK_NO_INDEX; + floppy_state = ST_write_flux_drain; + } + return; + } + + /* Start timer. */ + tim_wdata->egr = TIM_EGR_UG; + tim_wdata->sr = 0; /* dummy write, gives h/w time to process EGR.UG=1 */ + tim_wdata->cr1 = TIM_CR1_CEN; + + /* Enable output. */ + gpio_configure_pin(gpio_data, pin_wdata, AFO_bus); + write_pin(wgate, TRUE); + + index.count = 0; + floppy_state = ST_write_flux; +} + +static void floppy_write_check_underflow(void) +{ + uint32_t avail = u_prod - u_cons; + + if (/* We've run the input buffer dry. */ + (avail == 0) + /* The input buffer is nearly dry, and doesn't contain EOStream. */ + || ((avail < 16) && (u_buf[U_MASK(u_prod-1)] != 0))) { + + /* Underflow */ + printk("UNDERFLOW %u %u %u %u\n", u_cons, u_prod, + rw.packet_ready, ep_rx_ready(FLOPPY_EP)); + floppy_flux_end(); + rw.status = ACK_FLUX_UNDERFLOW; + floppy_state = ST_write_flux_drain; + + } +} + +static void floppy_write(void) +{ + uint16_t dmacons, todo, prev_todo; + + floppy_process_write_packet(); + wdata_decode_flux(); + + if (!rw.write_finished) { + floppy_write_check_underflow(); + return; + } + + /* Wait for DMA ring to drain. */ + todo = ~0; + do { + /* Early termination on index pulse? */ +// if (wr->terminate_at_index && (index.count != index_count)) +// goto out; + /* Check progress of draining the DMA ring. */ + prev_todo = todo; + dmacons = ARRAY_SIZE(dma.buf) - dma_wdata.cndtr; + todo = (dma.prod - dmacons) & (ARRAY_SIZE(dma.buf) - 1); + } while ((todo != 0) && (todo <= prev_todo)); + + floppy_flux_end(); + floppy_state = ST_write_flux_drain; +} + +static void floppy_write_drain(void) +{ + /* Drain the write stream. */ + if (!rw.write_finished) { + floppy_process_write_packet(); + (void)_wdata_decode_flux(dma.buf, ARRAY_SIZE(dma.buf)); + return; + } + + /* Wait for space to write ACK packet. */ + if (!ep_tx_ready(FLOPPY_EP)) + return; + + /* ACK with Status byte. */ + u_buf[0] = rw.status; + usb_write(FLOPPY_EP, u_buf, 1); + + /* Reset for next command. */ + floppy_configured(); + drive_deselect(); +} + +static void process_command(void) +{ + uint8_t cmd = u_buf[0]; + uint8_t len = u_buf[1]; + uint8_t resp_sz = 2; + + switch (cmd) { + case CMD_GET_INFO: { + uint8_t idx = u_buf[2]; + if (len != 3) goto bad_command; + if (idx != 0) goto bad_command; + memset(&u_buf[2], 0, 32); + memcpy(&u_buf[2], &gw_info, sizeof(gw_info)); + resp_sz += 32; + break; + } + case CMD_SEEK: { + uint8_t cyl = u_buf[2]; + if (len != 3) goto bad_command; + if (cyl > 85) goto bad_command; + u_buf[1] = floppy_seek(cyl) ? ACK_OKAY : ACK_NO_TRK0; + goto out; + } + case CMD_SIDE: { + uint8_t side = u_buf[2]; + if (len != 3) goto bad_command; + if (side > 1) goto bad_command; + write_pin(side, side); + break; + } + case CMD_SET_DELAYS: { + if (len != (2+sizeof(delay_params))) goto bad_command; + memcpy(&delay_params, &u_buf[2], sizeof(delay_params)); + break; + } + case CMD_GET_DELAYS: { + if (len != 2) goto bad_command; + memcpy(&u_buf[2], &delay_params, sizeof(delay_params)); + resp_sz += sizeof(delay_params); + break; + } + case CMD_MOTOR: { + uint8_t state = u_buf[2]; + if (len != 3) goto bad_command; + if (state > 1) goto bad_command; + floppy_motor(state); + break; + } + case CMD_READ_FLUX: { + uint8_t revs = u_buf[2]; + if (len != 3) goto bad_command; + if ((revs == 0) || (revs > 7)) goto bad_command; + floppy_read_prep(revs); + break; + } + case CMD_WRITE_FLUX: { + if (len != 2) goto bad_command; + floppy_write_prep(); + break; + } + case CMD_GET_FLUX_STATUS: { + if (len != 2) goto bad_command; + u_buf[1] = rw.status; + goto out; + } + case CMD_GET_READ_INFO: { + if (len != 2) goto bad_command; + memcpy(&u_buf[2], &read_info, sizeof(read_info)); + resp_sz += sizeof(read_info); + break; + } + default: + goto bad_command; + } + + u_buf[1] = ACK_OKAY; +out: + usb_write(FLOPPY_EP, u_buf, resp_sz); + return; + +bad_command: + u_buf[1] = ACK_BAD_COMMAND; + goto out; +} + +void floppy_configured(void) +{ + floppy_state = ST_command_wait; + u_cons = u_prod = 0; +} + +void floppy_process(void) +{ + int len; + + switch (floppy_state) { + + case ST_command_wait: + + len = ep_rx_ready(FLOPPY_EP); + if ((len >= 0) && (len < (sizeof(u_buf)-u_prod))) { + usb_read(FLOPPY_EP, &u_buf[u_prod], len); + u_prod += len; + } + + if ((u_prod >= 2) && (u_prod >= u_buf[1]) && ep_tx_ready(FLOPPY_EP)) { + /* Process command and reset for next command. */ + process_command(); + u_cons = u_prod = 0; + } + + break; + + case ST_read_flux_wait_index: + floppy_read_wait_index(); + break; + + case ST_read_flux: + case ST_read_flux_drain: + floppy_read(); + break; + + case ST_write_flux_wait_data: + floppy_write_wait_data(); + break; + + case ST_write_flux_wait_index: + floppy_write_wait_index(); + break; + + case ST_write_flux: + floppy_write(); + break; + + case ST_write_flux_drain: + floppy_write_drain(); + break; + + default: + break; + + } +} + +/* + * INTERRUPT HANDLERS + */ + +static void IRQ_INDEX_changed(void) +{ + time_t now = time_now(); + + /* Clear INDEX-changed flag. */ + exti->pr = m(pin_index); + + index.count++; + index.duration = time_diff(index.timestamp, now); + index.timestamp = now; + index.read_prod = ARRAY_SIZE(dma.buf) - dma_rdata.cndtr; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/main.c b/src/main.c new file mode 100644 index 0000000..50a6595 --- /dev/null +++ b/src/main.c @@ -0,0 +1,64 @@ +/* + * main.c + * + * System initialisation and navigation main loop. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +int EXC_reset(void) __attribute__((alias("main"))); + +static void canary_init(void) +{ + _irq_stackbottom[0] = _thread_stackbottom[0] = 0xdeadbeef; +} + +static void canary_check(void) +{ + ASSERT(_irq_stackbottom[0] == 0xdeadbeef); + ASSERT(_thread_stackbottom[0] == 0xdeadbeef); +} + +int main(void) +{ + /* Relocate DATA. Initialise BSS. */ + if (_sdat != _ldat) + memcpy(_sdat, _ldat, _edat-_sdat); + memset(_sbss, 0, _ebss-_sbss); + + canary_init(); + stm32_init(); + time_init(); + console_init(); + console_crash_on_input(); + board_init(); + delay_ms(200); /* 5v settle */ + + printk("\n** Greaseweazle v%s\n", FW_VER); + printk("** Keir Fraser \n"); + printk("** https://github.com/keirf/Greaseweazle\n\n"); + + floppy_init(); + usb_init(); + + for (;;) { + canary_check(); + usb_process(); + floppy_process(); + } + + return 0; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/stm32f10x.c b/src/stm32f10x.c new file mode 100644 index 0000000..9e8ce18 --- /dev/null +++ b/src/stm32f10x.c @@ -0,0 +1,212 @@ +/* + * stm32f10x.c + * + * Core and peripheral registers. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +struct extra_exception_frame { + uint32_t r4, r5, r6, r7, r8, r9, r10, r11, lr; +}; + +void EXC_unexpected(struct extra_exception_frame *extra) +{ + struct exception_frame *frame; + uint8_t exc = (uint8_t)read_special(psr); + uint32_t msp, psp; + + if (extra->lr & 4) { + frame = (struct exception_frame *)read_special(psp); + psp = (uint32_t)(frame + 1); + msp = (uint32_t)(extra + 1); + } else { + frame = (struct exception_frame *)(extra + 1); + psp = read_special(psp); + msp = (uint32_t)(frame + 1); + } + + printk("Unexpected %s #%u at PC=%08x (%s):\n", + (exc < 16) ? "Exception" : "IRQ", + (exc < 16) ? exc : exc - 16, + frame->pc, (extra->lr & 8) ? "Thread" : "Handler"); + printk(" r0: %08x r1: %08x r2: %08x r3: %08x\n", + frame->r0, frame->r1, frame->r2, frame->r3); + printk(" r4: %08x r5: %08x r6: %08x r7: %08x\n", + extra->r4, extra->r5, extra->r6, extra->r7); + printk(" r8: %08x r9: %08x r10: %08x r11: %08x\n", + extra->r8, extra->r9, extra->r10, extra->r11); + printk(" r12: %08x sp: %08x lr: %08x pc: %08x\n", + frame->r12, (extra->lr & 4) ? psp : msp, frame->lr, frame->pc); + printk(" msp: %08x psp: %08x psr: %08x\n", + msp, psp, frame->psr); + + system_reset(); +} + +static void exception_init(void) +{ + /* Initialise and switch to Process SP. Explicit asm as must be + * atomic wrt updates to SP. We can't guarantee that in C. */ + asm volatile ( + " mrs r1,msp \n" + " msr psp,r1 \n" /* Set up Process SP */ + " movs r1,%0 \n" + " msr control,r1 \n" /* Switch to Process SP */ + " isb \n" /* Flush the pipeline */ + :: "i" (CONTROL_SPSEL) : "r1" ); + + /* Set up Main SP for IRQ/Exception context. */ + write_special(msp, _irq_stacktop); + + /* Initialise interrupts and exceptions. */ + scb->vtor = (uint32_t)(unsigned long)vector_table; + scb->ccr |= SCB_CCR_STKALIGN | SCB_CCR_DIV_0_TRP; + /* GCC inlines memcpy() using full-word load/store regardless of buffer + * alignment. Hence it is unsafe to trap on unaligned accesses. */ + /*scb->ccr |= SCB_CCR_UNALIGN_TRP;*/ + scb->shcsr |= (SCB_SHCSR_USGFAULTENA | + SCB_SHCSR_BUSFAULTENA | + SCB_SHCSR_MEMFAULTENA); + + /* SVCall/PendSV exceptions have lowest priority. */ + scb->shpr2 = 0xff<<24; + scb->shpr3 = 0xff<<16; +} + +static void clock_init(void) +{ + /* Flash controller: reads require 2 wait states at 72MHz. */ + flash->acr = FLASH_ACR_PRFTBE | FLASH_ACR_LATENCY(2); + + /* Start up the external oscillator. */ + rcc->cr |= RCC_CR_HSEON; + while (!(rcc->cr & RCC_CR_HSERDY)) + cpu_relax(); + + /* PLLs, scalers, muxes. */ + rcc->cfgr = (RCC_CFGR_PLLMUL(9) | /* PLL = 9*8MHz = 72MHz */ + RCC_CFGR_PLLSRC_PREDIV1 | + RCC_CFGR_ADCPRE_DIV8 | + RCC_CFGR_PPRE1_DIV2); + + /* Enable and stabilise the PLL. */ + rcc->cr |= RCC_CR_PLLON; + while (!(rcc->cr & RCC_CR_PLLRDY)) + cpu_relax(); + + /* Switch to the externally-driven PLL for system clock. */ + rcc->cfgr |= RCC_CFGR_SW_PLL; + while ((rcc->cfgr & RCC_CFGR_SWS_MASK) != RCC_CFGR_SWS_PLL) + cpu_relax(); + + /* Internal oscillator no longer needed. */ + rcc->cr &= ~RCC_CR_HSION; + + /* Enable SysTick counter at 72/8=9MHz. */ + stk->load = STK_MASK; + stk->ctrl = STK_CTRL_ENABLE; +} + +static void gpio_init(GPIO gpio) +{ + /* Floating Input. Reference Manual states that JTAG pins are in PU/PD + * mode at reset, so ensure all PU/PD are disabled. */ + gpio->crl = gpio->crh = 0x44444444u; +} + +static void peripheral_init(void) +{ + /* Enable basic GPIO and AFIO clocks, all timers, and DMA. */ + rcc->apb1enr = (RCC_APB1ENR_TIM2EN | + RCC_APB1ENR_TIM3EN | + RCC_APB1ENR_TIM4EN); + rcc->apb2enr = (RCC_APB2ENR_IOPAEN | + RCC_APB2ENR_IOPBEN | + RCC_APB2ENR_IOPCEN | + RCC_APB2ENR_AFIOEN | + RCC_APB2ENR_TIM1EN); + rcc->ahbenr = RCC_AHBENR_DMA1EN; + + /* Turn off serial-wire JTAG and reclaim the GPIOs. */ + afio->mapr = AFIO_MAPR_SWJ_CFG_DISABLED; + + /* All pins in a stable state. */ + gpio_init(gpioa); + gpio_init(gpiob); + gpio_init(gpioc); +} + +void stm32_init(void) +{ + exception_init(); + clock_init(); + peripheral_init(); + cpu_sync(); +} + +void delay_ticks(unsigned int ticks) +{ + unsigned int diff, cur, prev = stk->val; + + for (;;) { + cur = stk->val; + diff = (prev - cur) & STK_MASK; + if (ticks <= diff) + break; + ticks -= diff; + prev = cur; + } +} + +void delay_ns(unsigned int ns) +{ + delay_ticks((ns * STK_MHZ) / 1000u); +} + +void delay_us(unsigned int us) +{ + delay_ticks(us * STK_MHZ); +} + +void delay_ms(unsigned int ms) +{ + delay_ticks(ms * 1000u * STK_MHZ); +} + +void gpio_configure_pin(GPIO gpio, unsigned int pin, unsigned int mode) +{ + gpio_write_pin(gpio, pin, mode >> 4); + mode &= 0xfu; + if (pin >= 8) { + pin -= 8; + gpio->crh = (gpio->crh & ~(0xfu<<(pin<<2))) | (mode<<(pin<<2)); + } else { + gpio->crl = (gpio->crl & ~(0xfu<<(pin<<2))) | (mode<<(pin<<2)); + } +} + +void system_reset(void) +{ + console_sync(); + printk("Resetting...\n"); + /* Wait for serial console TX to idle. */ + while (!(usart1->sr & USART_SR_TXE) || !(usart1->sr & USART_SR_TC)) + cpu_relax(); + /* Request reset and loop waiting for it to happen. */ + scb->aircr = SCB_AIRCR_VECTKEY | SCB_AIRCR_SYSRESETREQ; + for (;;) ; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/string.c b/src/string.c new file mode 100644 index 0000000..cabc24d --- /dev/null +++ b/src/string.c @@ -0,0 +1,178 @@ +/* + * string.c + * + * String manipulation functions. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +static void do_putch(char **p, char *end, char c) +{ + if (*p < end) + **p = c; + (*p)++; +} + +int vsnprintf(char *str, size_t size, const char *format, va_list ap) +{ + unsigned int x, flags; + int width; + char c, *p = str, *end = p + size - 1, tmp[12], *q; + + while ((c = *format++) != '\0') { + if (c != '%') { + do_putch(&p, end, c); + continue; + } + + flags = width = 0; +#define BASE (31u << 0) +#define UPPER ( 1u << 8) +#define SIGN ( 1u << 9) +#define ALTERNATE ( 1u << 10) +#define ZEROPAD ( 1u << 11) +#define CHAR ( 1u << 12) +#define SHORT ( 1u << 13) + + more: + switch (c = *format++) { + case '*': + width = va_arg(ap, unsigned int); + goto more; + case '#': + flags |= ALTERNATE; + goto more; + case '0': + flags |= ZEROPAD; + goto more; + case '1'...'9': + width = c-'0'; + while (((c = *format) >= '0') && (c <= '9')) { + width = width*10 + c-'0'; + format++; + } + goto more; + case 'h': + if ((c = *format) == 'h') { + flags |= CHAR; + format++; + } else { + flags |= SHORT; + } + goto more; + case 'o': + flags |= 8; + break; + case 'd': + case 'i': + flags |= SIGN; + case 'u': + flags |= 10; + break; + case 'X': + flags |= UPPER; + case 'x': + case 'p': + flags |= 16; + break; + case 's': + q = va_arg(ap, char *); + while ((c = *q++) != '\0') { + do_putch(&p, end, c); + width--; + } + while (width-- > 0) + do_putch(&p, end, ' '); + continue; + case 'c': + c = va_arg(ap, unsigned int); + default: + do_putch(&p, end, c); + continue; + } + + x = va_arg(ap, unsigned int); + + if (flags & CHAR) { + if (flags & SIGN) + x = (char)x; + else + x = (unsigned char)x; + } else if (flags & SHORT) { + if (flags & SIGN) + x = (short)x; + else + x = (unsigned short)x; + } + + if ((flags & SIGN) && ((int)x < 0)) { + if (flags & ZEROPAD) { + do_putch(&p, end, '-'); + flags &= ~SIGN; + } + width--; + x = -x; + } else { + flags &= ~SIGN; + } + + if (flags & ALTERNATE) { + if (((flags & BASE) == 8) || ((flags & BASE) == 16)) { + do_putch(&p, end, '0'); + width--; + } + if ((flags & BASE) == 16) { + do_putch(&p, end, 'x'); + width--; + } + } + + if (x == 0) { + q = tmp; + *q++ = '0'; + } else { + for (q = tmp; x; q++, x /= (flags&BASE)) + *q = ((flags & UPPER) + ? "0123456789ABCDEF" + : "0123456789abcdef") [x % (flags&BASE)]; + } + while (width-- > (q-tmp)) + do_putch(&p, end, (flags & ZEROPAD) ? '0' : ' '); + if (flags & SIGN) + do_putch(&p, end, '-'); + while (q != tmp) + do_putch(&p, end, *--q); + }; + + if (p <= end) + *p = '\0'; + else if (str <= end) + *end = '\0'; + + return p - str; +} + +int snprintf(char *str, size_t size, const char *format, ...) +{ + va_list ap; + int n; + + va_start(ap, format); + n = vsnprintf(str, size, format, ap); + va_end(ap); + + return n; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/time.c b/src/time.c new file mode 100644 index 0000000..1efa2e1 --- /dev/null +++ b/src/time.c @@ -0,0 +1,49 @@ +/* + * time.c + * + * System-time abstraction over STM32 STK timer. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +static volatile time_t time_stamp; +static struct timer time_stamp_timer; + +static void time_stamp_update(void *unused) +{ + time_t now = time_now(); + time_stamp = ~now; + timer_set(&time_stamp_timer, now + time_ms(500)); +} + +time_t time_now(void) +{ + time_t s, t; + s = time_stamp; + t = stk_now() | (s & (0xff << 24)); + if (t > s) + t -= 1u << 24; + return ~t; +} + +void time_init(void) +{ + timers_init(); + time_stamp = stk_now(); + timer_init(&time_stamp_timer, time_stamp_update, NULL); + timer_set(&time_stamp_timer, time_now() + time_ms(500)); +} + + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/timer.c b/src/timer.c new file mode 100644 index 0000000..2963e23 --- /dev/null +++ b/src/timer.c @@ -0,0 +1,142 @@ +/* + * timer.c + * + * Deadline-based timer callbacks. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +/* TIM1_UP: IRQ 25. */ +void IRQ_25(void) __attribute__((alias("IRQ_timer"))); +#define TIMER_IRQ 25 +#define tim tim1 + +/* IRQ only on counter overflow, one-time enable. */ +#define TIM_CR1 (TIM_CR1_URS | TIM_CR1_OPM) + +/* Empirically-determined offset applied to timer deadlines to counteract the + * latency incurred by reprogram_timer() and IRQ_timer(). */ +#define SLACK_TICKS 12 + +#define TIMER_INACTIVE ((struct timer *)1ul) + +static struct timer *head; + +static void reprogram_timer(int32_t delta) +{ + tim->cr1 = TIM_CR1; + if (delta < 0x10000) { + /* Fine-grained deadline (sub-microsecond accurate) */ + tim->psc = SYSCLK_MHZ/TIME_MHZ-1; + tim->arr = (delta <= SLACK_TICKS) ? 1 : delta-SLACK_TICKS; + } else { + /* Coarse-grained deadline, fires in time to set a shorter, + * fine-grained deadline. */ + tim->psc = sysclk_us(100)-1; + tim->arr = min_t(uint32_t, 0xffffu, + delta/time_us(100)-50); /* 5ms early */ + } + tim->egr = TIM_EGR_UG; /* update CNT, PSC, ARR */ + tim->sr = 0; /* dummy write, gives hardware time to process EGR.UG=1 */ + tim->cr1 = TIM_CR1 | TIM_CR1_CEN; +} + +void timer_init(struct timer *timer, void (*cb_fn)(void *), void *cb_dat) +{ + timer->cb_fn = cb_fn; + timer->cb_dat = cb_dat; + timer->next = TIMER_INACTIVE; +} + +static bool_t timer_is_active(struct timer *timer) +{ + return timer->next != TIMER_INACTIVE; +} + +static void _timer_cancel(struct timer *timer) +{ + struct timer *t, **pprev; + + if (!timer_is_active(timer)) + return; + + for (pprev = &head; (t = *pprev) != timer; pprev = &t->next) + continue; + + *pprev = t->next; + t->next = TIMER_INACTIVE; +} + +void timer_set(struct timer *timer, time_t deadline) +{ + struct timer *t, **pprev; + time_t now; + int32_t delta; + uint32_t oldpri; + + oldpri = IRQ_save(TIMER_IRQ_PRI); + + _timer_cancel(timer); + + timer->deadline = deadline; + + now = time_now(); + delta = time_diff(now, deadline); + for (pprev = &head; (t = *pprev) != NULL; pprev = &t->next) + if (delta <= time_diff(now, t->deadline)) + break; + timer->next = *pprev; + *pprev = timer; + + if (head == timer) + reprogram_timer(delta); + + IRQ_restore(oldpri); +} + +void timer_cancel(struct timer *timer) +{ + uint32_t oldpri; + oldpri = IRQ_save(TIMER_IRQ_PRI); + _timer_cancel(timer); + IRQ_restore(oldpri); +} + +void timers_init(void) +{ + tim->cr2 = 0; + tim->dier = TIM_DIER_UIE; + IRQx_set_prio(TIMER_IRQ, TIMER_IRQ_PRI); + IRQx_enable(TIMER_IRQ); +} + +static void IRQ_timer(void) +{ + struct timer *t; + int32_t delta; + + tim->sr = 0; + + while ((t = head) != NULL) { + if ((delta = time_diff(time_now(), t->deadline)) > SLACK_TICKS) { + reprogram_timer(delta); + break; + } + head = t->next; + t->next = TIMER_INACTIVE; + (*t->cb_fn)(t->cb_dat); + } +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/usb/Makefile b/src/usb/Makefile new file mode 100644 index 0000000..99b26ed --- /dev/null +++ b/src/usb/Makefile @@ -0,0 +1,6 @@ +OBJS += config.o +OBJS += core.o +OBJS += cdc_acm.o +OBJS += hw_f1.o + +$(OBJS): CFLAGS += -include defs.h diff --git a/src/usb/cdc_acm.c b/src/usb/cdc_acm.c new file mode 100644 index 0000000..9f1ee81 --- /dev/null +++ b/src/usb/cdc_acm.c @@ -0,0 +1,109 @@ +/* + * cdc_acm.c + * + * USB CDC ACM handling (Communications Device Class, Abstract Control Model). + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +#define TRACE 1 + +/* CDC Communications Class requests */ +#define CDC_SET_LINE_CODING 0x20 +#define CDC_GET_LINE_CODING 0x21 +#define CDC_SET_CONTROL_LINE_STATE 0x22 +#define CDC_SEND_BREAK 0x23 + +static struct __packed line_coding { + uint32_t baud; + uint8_t nr_stop; + uint8_t parity; + uint8_t nr_data; +} line_coding; + +#if TRACE +#define TRC printk +#else +static inline void TRC(const char *format, ...) { } +#endif + +static void dump_line_coding(const struct line_coding *lc) +{ + int parity = (lc->parity > 4) ? 5 : lc->parity; + + TRC("%u,%u%c%s\n", lc->baud, lc->nr_data, + "noems?"[parity], + (lc->nr_stop == 0) ? "1" + : (lc->nr_stop == 1) ? "1.5" + : (lc->nr_stop == 2) ? "2" : "X"); +} + +bool_t cdc_acm_handle_class_request(void) +{ + struct usb_device_request *req = &ep0.req; + bool_t handled = TRUE; + + switch (req->bRequest) { + + case CDC_SET_LINE_CODING: { + struct line_coding *lc = (struct line_coding *)ep0.data; + TRC("SET_LINE_CODING: "); + dump_line_coding(lc); + line_coding = *lc; + break; + } + + case CDC_GET_LINE_CODING: { + struct line_coding *lc = (struct line_coding *)ep0.data; + TRC("GET_LINE_CODING: "); + line_coding = *lc; + dump_line_coding(lc); + ep0.data_len = sizeof(*lc); + break; + } + + case CDC_SET_CONTROL_LINE_STATE: + /* wValue = DTR/RTS. We ignore them and return success. */ + break; + + case CDC_SEND_BREAK: + /* wValue = #millisecs. We ignore it and return success. */ + TRC("BREAK\n"); + floppy_reset(); + floppy_configured(); + break; + + default: + WARN("[Class-specific: %02x]\n", req->bRequest); + handled = FALSE; + break; + + } + + return handled; +} + +bool_t cdc_acm_set_configuration(void) +{ + /* Set up endpoints: XXX Do we need the Notification Element? */ + usb_configure_ep(0x81, 0, 0); /* Notification Element (D->H) */ + usb_configure_ep(0x02, 0, 64); /* Bulk Pipe (H->D) */ + usb_configure_ep(0x82, 0, 64); /* Bulk Pipe (D->H) */ + + floppy_configured(); + + return TRUE; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/usb/config.c b/src/usb/config.c new file mode 100644 index 0000000..b91be94 --- /dev/null +++ b/src/usb/config.c @@ -0,0 +1,114 @@ +/* + * config.c + * + * USB device and configuration descriptors. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +const uint8_t device_descriptor[] __aligned(2) = { + 18, /* Length */ + DESC_DEVICE, /* Descriptor Type */ + 0x10,0x01, /* USB 1.1 */ + 2, 0, 0, /* Class, Subclass, Protocol: CDC */ + 64, /* Max Packet Size */ + 0x09,0x12, /* VID = pid.codes Open Source projects */ + 0x01,0x00, /* PID = Test PID #1 */ + 0,1, /* Device Release 1.0 */ + 1,2,0, /* Manufacturer, Product, Serial */ + 1 /* Number of configurations */ +}; + +const uint8_t config_descriptor[] __aligned(2) = { + 0x09, /* 0 bLength */ + DESC_CONFIGURATION, /* 1 bDescriptortype - Configuration*/ + 0x43, 0x00, /* 2 wTotalLength */ + 0x02, /* 4 bNumInterfaces */ + 0x01, /* 5 bConfigurationValue */ + 0x00, /* 6 iConfiguration - index of string */ + 0x80, /* 7 bmAttributes - Bus powered */ + 0xC8, /* 8 bMaxPower - 400mA */ +/* CDC Communication interface */ + 0x09, /* 0 bLength */ + DESC_INTERFACE, /* 1 bDescriptorType - Interface */ + 0x00, /* 2 bInterfaceNumber - Interface 0 */ + 0x00, /* 3 bAlternateSetting */ + 0x01, /* 4 bNumEndpoints */ + 2, 2, 1, /* CDC ACM, AT Command Protocol */ + 0x00, /* 8 iInterface - No string descriptor */ +/* Header Functional descriptor */ + 0x05, /* 0 bLength */ + DESC_CS_INTERFACE, /* 1 bDescriptortype, CS_INTERFACE */ + 0x00, /* 2 bDescriptorsubtype, HEADER */ + 0x10, 0x01, /* 3 bcdCDC */ +/* ACM Functional descriptor */ + 0x04, /* 0 bLength */ + DESC_CS_INTERFACE, /* 1 bDescriptortype, CS_INTERFACE */ + 0x02, /* 2 bDescriptorsubtype, ABSTRACT CONTROL MANAGEMENT */ + 0x02, /* 3 bmCapabilities: Supports subset of ACM commands */ +/* Union Functional descriptor */ + 0x05, /* 0 bLength */ + DESC_CS_INTERFACE,/* 1 bDescriptortype, CS_INTERFACE */ + 0x06, /* 2 bDescriptorsubtype, UNION */ + 0x00, /* 3 bControlInterface - Interface 0 */ + 0x01, /* 4 bSubordinateInterface0 - Interface 1 */ +/* Call Management Functional descriptor */ + 0x05, /* 0 bLength */ + DESC_CS_INTERFACE,/* 1 bDescriptortype, CS_INTERFACE */ + 0x01, /* 2 bDescriptorsubtype, CALL MANAGEMENT */ + 0x03, /* 3 bmCapabilities, DIY */ + 0x01, /* 4 bDataInterface */ +/* Notification Endpoint descriptor */ + 0x07, /* 0 bLength */ + DESC_ENDPOINT, /* 1 bDescriptorType */ + 0x81, /* 2 bEndpointAddress */ + 0x03, /* 3 bmAttributes */ + 0x40, /* 4 wMaxPacketSize - Low */ + 0x00, /* 5 wMaxPacketSize - High */ + 0xFF, /* 6 bInterval */ +/* CDC Data interface */ + 0x09, /* 0 bLength */ + DESC_INTERFACE, /* 1 bDescriptorType */ + 0x01, /* 2 bInterfacecNumber */ + 0x00, /* 3 bAlternateSetting */ + 0x02, /* 4 bNumEndpoints */ + USB_CLASS_CDC_DATA, /* 5 bInterfaceClass */ + 0x00, /* 6 bInterfaceSubClass */ + 0x00, /* 7 bInterfaceProtocol*/ + 0x00, /* 8 iInterface - No string descriptor*/ +/* Data OUT Endpoint descriptor */ + 0x07, /* 0 bLength */ + DESC_ENDPOINT, /* 1 bDescriptorType */ + 0x02, /* 2 bEndpointAddress */ + 0x02, /* 3 bmAttributes */ + 0x40, /* 4 wMaxPacketSize - Low */ + 0x00, /* 5 wMaxPacketSize - High */ + 0x00, /* 6 bInterval */ +/* Data IN Endpoint descriptor */ + 0x07, /* 0 bLength */ + DESC_ENDPOINT, /* 1 bDescriptorType */ + 0x82, /* 2 bEndpointAddress */ + 0x02, /* 3 bmAttributes */ + 0x40, /* 4 wMaxPacketSize - Low byte */ + 0x00, /* 5 wMaxPacketSize - High byte */ + 0x00 /* 6 bInterval */ +}; + +const char *string_descriptors[] = { + "\x09\x04", /* LANGID: US English */ + "Keir Fraser", + "GreaseWeazle", +}; + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/usb/core.c b/src/usb/core.c new file mode 100644 index 0000000..7fca18f --- /dev/null +++ b/src/usb/core.c @@ -0,0 +1,100 @@ +/* + * core.c + * + * USB core. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +struct ep0 ep0; +uint8_t pending_addr; + +bool_t handle_control_request(void) +{ + struct usb_device_request *req = &ep0.req; + bool_t handled = TRUE; + + if (ep0_data_out() && (req->wLength > sizeof(ep0.data))) { + + WARN("Ctl OUT too long: %u>%u\n", req->wLength, sizeof(ep0.data)); + handled = FALSE; + + } else if ((req->bmRequestType == 0x80) + && (req->bRequest == GET_DESCRIPTOR)) { + + uint8_t type = req->wValue >> 8; + uint8_t idx = req->wValue; + if ((type == DESC_DEVICE) && (idx == 0)) { + ep0.data_len = device_descriptor[0]; /* bLength */ + memcpy(ep0.data, device_descriptor, ep0.data_len); + } else if ((type == DESC_CONFIGURATION) && (idx == 0)) { + ep0.data_len = config_descriptor[2]; /* wTotalLength */ + memcpy(ep0.data, config_descriptor, ep0.data_len); + } else if ((type == DESC_STRING) && (idx < NR_STRING_DESC)) { + const char *s = string_descriptors[idx]; + uint16_t *odat = (uint16_t *)ep0.data; + int i = 0; + if (idx == 0) { + odat[i++] = 4+(DESC_STRING<<8); + memcpy(&odat[i++], s, 2); + } else { + odat[i++] = (1+strlen(s))*2+(DESC_STRING<<8); + while (*s) + odat[i++] = *s++; + } + ep0.data_len = i*2; + } else { + WARN("[Unknown descriptor %u,%u]\n", type, idx); + handled = FALSE; + } + + } else if ((req->bmRequestType == 0x00) + && (req->bRequest == SET_ADDRESS)) { + + pending_addr = req->wValue & 0x7f; + + } else if ((req->bmRequestType == 0x00) + && (req->bRequest == SET_CONFIGURATION)) { + + handled = cdc_acm_set_configuration(); + + } else if ((req->bmRequestType&0x7f) == 0x21) { + + handled = cdc_acm_handle_class_request(); + + } else { + + uint8_t *pkt = (uint8_t *)req; + int i; + WARN("(%02x %02x %02x %02x %02x %02x %02x %02x)", + pkt[0], pkt[1], pkt[2], pkt[3], + pkt[4], pkt[5], pkt[6], pkt[7]); + if (ep0_data_out()) { + WARN("["); + for (i = 0; i < ep0.data_len; i++) + WARN("%02x ", ep0.data[i]); + WARN("]"); + } + WARN("\n"); + handled = FALSE; + + } + + if (ep0_data_in() && (ep0.data_len > req->wLength)) + ep0.data_len = req->wLength; + + return handled; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/usb/defs.h b/src/usb/defs.h new file mode 100644 index 0000000..ad1120d --- /dev/null +++ b/src/usb/defs.h @@ -0,0 +1,85 @@ +/* + * defs.h + * + * USB standard definitions and private interfaces. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +/* bRequest: Standard Request Codes */ +#define GET_STATUS 0 +#define CLEAR_FEATURE 1 +#define SET_FEATURE 3 +#define SET_ADDRESS 5 +#define GET_DESCRIPTOR 6 +#define SET_DESCRIPTOR 7 +#define GET_CONFIGURATION 8 +#define SET_CONFIGURATION 9 +#define GET_INTERFACE 10 +#define SET_INTERFACE 11 +#define SYNCH_FRAME 12 + +/* Descriptor Types */ +#define DESC_DEVICE 1 +#define DESC_CONFIGURATION 2 +#define DESC_STRING 3 +#define DESC_INTERFACE 4 +#define DESC_ENDPOINT 5 +#define DESC_DEVICE_QUALIFIER 6 +#define DESC_OTHER_SPEED_CONFIGURATION 7 +#define DESC_INTERFACE_POWER 8 +#define DESC_CS_INTERFACE 0x24 + +#define USB_CLASS_CDC_DATA 0x0a + +struct __packed usb_device_request { + uint8_t bmRequestType; + uint8_t bRequest; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; +}; + +extern const uint8_t device_descriptor[]; +extern const uint8_t config_descriptor[]; + +#define NR_STRING_DESC 3 +extern const char *string_descriptors[]; + +extern struct ep0 { + struct usb_device_request req; + uint8_t data[128]; + int data_len; + struct { + const uint8_t *p; + int todo; + } tx; +} ep0; +#define ep0_data_out() (!(ep0.req.bmRequestType & 0x80)) +#define ep0_data_in() (!ep0_data_out()) + +/* USB CDC ACM */ +bool_t cdc_acm_handle_class_request(void); +bool_t cdc_acm_set_configuration(void); + +/* USB Core */ +extern uint8_t pending_addr; +bool_t handle_control_request(void); + +/* USB Hardware */ +void usb_configure_ep(uint8_t ep, uint8_t type, uint32_t size); + +#define WARN printk + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/usb/hw_f1.c b/src/usb/hw_f1.c new file mode 100644 index 0000000..5c22280 --- /dev/null +++ b/src/usb/hw_f1.c @@ -0,0 +1,365 @@ +/* + * hw_f1.c + * + * USB handling for STM32F10x devices (except 105/107). + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +static uint16_t buf_end; + +/* We track which endpoints have been marked CTR (Correct TRansfer). + * On receive side, checking EPR_STAT_RX == NAK races update of the + * Buffer Descriptor's COUNT_RX by the hardware. */ +static bool_t _ep_rx_ready[8]; +static bool_t _ep_tx_ready[8]; + +void usb_init(void) +{ + /* Turn on clock. */ + rcc->apb1enr |= RCC_APB1ENR_USBEN; + + /* Exit power-down state. */ + usb->cntr &= ~USB_CNTR_PDWN; + delay_us(10); + + /* Exit reset state. */ + usb->cntr &= ~USB_CNTR_FRES; + delay_us(10); + + /* Clear IRQ state. */ + usb->istr = 0; + + /* Indicate we are connected by pulling up D+. */ + gpio_configure_pin(gpioa, 0, GPO_pushpull(_2MHz, HIGH)); +} + +#if 0 +static void dump_ep(uint8_t ep) +{ + const static char *names[] = { "DISA", "STAL", "NAK ", "VALI" }; + uint16_t epr; + ep &= 0x7f; + epr = usb->epr[ep]; + printk("[EP%u: Rx:%c%c(%s)%04x:%02u Tx:%c%c(%s)%04x:%02u %c]", + ep, + (epr & USB_EPR_CTR_RX) ? 'C' : ' ', + (epr & USB_EPR_DTOG_RX) ? 'D' : ' ', + names[(epr>>12)&3], + usb_bufd[ep].addr_rx, usb_bufd[ep].count_rx & 0x3ff, + (epr & USB_EPR_CTR_TX) ? 'C' : ' ', + (epr & USB_EPR_DTOG_TX) ? 'D' : ' ', + names[(epr>>4)&3], + usb_bufd[ep].addr_tx, usb_bufd[ep].count_tx & 0x3ff, + (epr & USB_EPR_SETUP) ? 'S' : ' '); +} +#endif + +int ep_rx_ready(uint8_t ep) +{ + return _ep_rx_ready[ep] ? usb_bufd[ep].count_rx & 0x3ff : -1; +} + +bool_t ep_tx_ready(uint8_t ep) +{ + return _ep_tx_ready[ep]; +} + +void usb_read(uint8_t ep, void *buf, uint32_t len) +{ + unsigned int i, base = (uint16_t)usb_bufd[ep].addr_rx >> 1; + uint16_t epr, *p = buf; + + for (i = 0; i < len/2; i++) + *p++ = usb_buf[base + i]; + if (len&1) + *(uint8_t *)p = usb_buf[base + i]; + + /* Clear CTR_RX and set status NAK->VALID. */ + epr = usb->epr[ep]; + epr &= 0x370f; + epr |= 0x0080; + epr ^= USB_EPR_STAT_RX(USB_STAT_VALID); + usb->epr[ep] = epr; + + /* Await next CTR_RX notification. */ + _ep_rx_ready[ep] = FALSE; +} + +void usb_write(uint8_t ep, const void *buf, uint32_t len) +{ + unsigned int i, base = (uint16_t)usb_bufd[ep].addr_tx >> 1; + uint16_t epr; + const uint16_t *p = buf; + + for (i = 0; i < len/2; i++) + usb_buf[base + i] = *p++; + if (len&1) + usb_buf[base + i] = *(const uint8_t *)p; + + usb_bufd[ep].count_tx = len; + + /* Set status NAK->VALID. */ + epr = usb->epr[ep]; + epr &= 0x073f; + epr |= 0x8080; + epr ^= USB_EPR_STAT_TX(USB_STAT_VALID); + usb->epr[ep] = epr; + + /* Await next CTR_TX notification. */ + _ep_tx_ready[ep] = FALSE; +} + +static void usb_continue_write_ep0(void) +{ + uint32_t len; + + if ((ep0.tx.todo < 0) || !ep_tx_ready(0)) + return; + + len = min_t(uint32_t, ep0.tx.todo, 64); + usb_write(0, ep0.tx.p, len); + + if ((ep0.tx.todo -= len) == 0) + ep0.tx.todo = -1; + ep0.tx.p += len; +} + +static void usb_start_write_ep0(const void *buf, uint32_t len) +{ + ep0.tx.p = buf; + ep0.tx.todo = len; + usb_continue_write_ep0(); +} + +static void usb_stall(uint8_t ep) +{ + uint16_t epr = usb->epr[ep]; + epr &= 0x073f; + epr |= 0x8080; + epr ^= USB_EPR_STAT_TX(USB_STAT_STALL); + usb->epr[ep] = epr; +} + +void usb_configure_ep(uint8_t ep, uint8_t type, uint32_t size) +{ + uint16_t old_epr, new_epr; + bool_t in; + + in = !!(ep & 0x80); + ep &= 0x7f; + + old_epr = usb->epr[ep]; + + /* Sets: Type and Endpoint Address. + * Clears: CTR_RX and CTR_TX. */ + new_epr = USB_EPR_EP_TYPE(type) | USB_EPR_EA(ep); + + if (in || (ep == 0)) { + usb_bufd[ep].addr_tx = buf_end; + buf_end += size; + usb_bufd[ep].count_tx = 0; + /* TX: Clears data toggle and sets status to NAK. */ + new_epr |= (old_epr & 0x0070) ^ USB_EPR_STAT_TX(USB_STAT_NAK); + /* IN Endpoint is immediately ready to transmit. */ + _ep_tx_ready[ep] = TRUE; + } + + if (!in) { + usb_bufd[ep].addr_rx = buf_end; + buf_end += size; + usb_bufd[ep].count_rx = 0x8400; /* 64 bytes */ + /* RX: Clears data toggle and sets status to VALID. */ + new_epr |= (old_epr & 0x7000) ^ USB_EPR_STAT_RX(USB_STAT_VALID); + /* OUT Endpoint must wait for a packet from the Host. */ + _ep_rx_ready[ep] = FALSE; + } + + usb->epr[ep] = new_epr; +} + +static void handle_reset(void) +{ + /* Reinitialise floppy subsystem. */ + floppy_reset(); + + /* All Endpoints in invalid Tx/Rx state. */ + memset(_ep_rx_ready, 0, sizeof(_ep_rx_ready)); + memset(_ep_tx_ready, 0, sizeof(_ep_tx_ready)); + + /* Clear any in-progress Control Transfer. */ + ep0.data_len = -1; + ep0.tx.todo = -1; + + /* Prepare for Enumeration: Set up Endpoint 0 at Address 0. */ + pending_addr = 0; + buf_end = 64; + usb_configure_ep(0, USB_EP_TYPE_CONTROL, 64); + usb->daddr = USB_DADDR_EF | USB_DADDR_ADD(0); + usb->istr &= ~USB_ISTR_RESET; +} + +static void handle_rx_transfer(uint8_t ep) +{ + uint16_t epr; + bool_t ready; + + /* Clear CTR_RX. */ + epr = usb->epr[ep]; + epr &= 0x070f; + epr |= 0x0080; + usb->epr[ep] = epr; + _ep_rx_ready[ep] = TRUE; + + /* We only handle Control Transfers here (endpoint 0). */ + if (ep != 0) + return; + + ready = FALSE; + epr = usb->epr[ep]; + + if (epr & USB_EPR_SETUP) { + + /* Control Transfer: Setup Stage. */ + ep0.data_len = 0; + ep0.tx.todo = -1; + usb_read(ep, &ep0.req, sizeof(ep0.req)); + ready = ep0_data_in() || (ep0.req.wLength == 0); + + } else if (ep0.data_len < 0) { + + /* Unexpected Transaction */ + usb_stall(0); + usb_read(ep, NULL, 0); + + } else if (ep0_data_out()) { + + /* OUT Control Transfer: Data from Host. */ + uint32_t len = usb_bufd[ep].count_rx & 0x3ff; + int l = 0; + if (ep0.data_len < sizeof(ep0.data)) + l = min_t(int, sizeof(ep0.data)-ep0.data_len, len); + usb_read(ep, &ep0.data[ep0.data_len], l); + ep0.data_len += len; + if (ep0.data_len >= ep0.req.wLength) { + ep0.data_len = ep0.req.wLength; /* clip */ + ready = TRUE; + } + + } else { + + /* IN Control Transfer: Status from Host. */ + usb_read(ep, NULL, 0); + ep0.data_len = -1; /* Complete */ + + } + + /* Are we ready to handle the Control Request? */ + if (!ready) + return; + + /* Attempt to handle the Control Request: */ + if (!handle_control_request()) { + + /* Unhandled Control Transfer: STALL */ + usb_stall(0); + ep0.data_len = -1; /* Complete */ + + } else if (ep0_data_in()) { + + /* IN Control Transfer: Send Data to Host. */ + usb_start_write_ep0(ep0.data, ep0.data_len); + + } else { + + /* OUT Control Transfer: Send Status to Host. */ + usb_start_write_ep0(NULL, 0); + ep0.data_len = -1; /* Complete */ + + } +} + +static void handle_tx_transfer(uint8_t ep) +{ + uint16_t epr; + + /* Clear CTR_TX. */ + epr = usb->epr[ep]; + epr &= 0x070f; + epr |= 0x8000; + usb->epr[ep] = epr; + _ep_tx_ready[ep] = TRUE; + + /* We only handle Control Transfers here (endpoint 0). */ + if (ep != 0) + return; + + usb_continue_write_ep0(); + + if (pending_addr) { + /* We have just completed the Status stage of a SET_ADDRESS request. + * Now is the time to apply the address update. */ + usb->daddr = USB_DADDR_EF | USB_DADDR_ADD(pending_addr); + pending_addr = 0; + } +} + +void usb_process(void) +{ + uint16_t istr = usb->istr; + usb->istr = ~istr & 0x7f00; + + if (istr & USB_ISTR_CTR) { + uint8_t ep = USB_ISTR_GET_EP_ID(istr); + //dump_ep(ep); + if (istr & USB_ISTR_DIR) + handle_rx_transfer(ep); + else + handle_tx_transfer(ep); + //printk(" -> "); dump_ep(ep); printk("\n"); + } + + if (istr & USB_ISTR_PMAOVR) { + printk("[PMAOVR]\n"); + } + + if (istr & USB_ISTR_ERR) { + printk("[ERR]\n"); + } + + if (istr & USB_ISTR_WKUP) { + printk("[WKUP]\n"); + } + + if (istr & USB_ISTR_RESET) { + printk("[RESET]\n"); + handle_reset(); + } + + /* We ignore all the below... */ + + if (istr & USB_ISTR_SUSP) { +// printk("[SUSP]\n"); + } + + if (istr & USB_ISTR_SOF) { +// printk("[SOF]\n"); + } + + if (istr & USB_ISTR_ESOF) { +// printk("[ESOF]\n"); + } +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/util.c b/src/util.c new file mode 100644 index 0000000..67870ef --- /dev/null +++ b/src/util.c @@ -0,0 +1,245 @@ +/* + * util.c + * + * General-purpose utility functions. + * + * Written & released by Keir Fraser + * + * This is free and unencumbered software released into the public domain. + * See the file COPYING for more details, or visit . + */ + +void filename_extension(const char *filename, char *extension, size_t size) +{ + const char *p; + unsigned int i; + + extension[0] = '\0'; + if ((p = strrchr(filename, '.')) == NULL) + return; + + for (i = 0; i < (size-1); i++) + if ((extension[i] = tolower(p[i+1])) == '\0') + break; + extension[i] = '\0'; +} + +void *memset(void *s, int c, size_t n) +{ + char *p = s; + + /* Large aligned memset? */ + size_t n32 = n & ~31; + if (n32 && !((uint32_t)p & 3)) { + memset_fast(p, c, n32); + p += n32; + n &= 31; + } + + /* Remainder/unaligned memset. */ + while (n--) + *p++ = c; + return s; +} + +void *memcpy(void *dest, const void *src, size_t n) +{ + char *p = dest; + const char *q = src; + + /* Large aligned copy? */ + size_t n32 = n & ~31; + if (n32 && !(((uint32_t)p | (uint32_t)q) & 3)) { + memcpy_fast(p, q, n32); + p += n32; + q += n32; + n &= 31; + } + + /* Remainder/unaligned copy. */ + while (n--) + *p++ = *q++; + return dest; +} + +asm ( +".global memcpy_fast, memset_fast\n" +"memcpy_fast:\n" +" push {r4-r10}\n" +"1: ldmia r1!,{r3-r10}\n" +" stmia r0!,{r3-r10}\n" +" subs r2,r2,#32\n" +" bne 1b\n" +" pop {r4-r10}\n" +" bx lr\n" +"memset_fast:\n" +" push {r4-r10}\n" +" uxtb r5, r1\n" +" mov.w r4, #0x01010101\n" +" muls r4, r5\n" +" mov r3, r4\n" +" mov r5, r4\n" +" mov r6, r4\n" +" mov r7, r4\n" +" mov r8, r4\n" +" mov r9, r4\n" +" mov r10, r4\n" +"1: stmia r0!,{r3-r10}\n" +" subs r2,r2,#32\n" +" bne 1b\n" +" pop {r4-r10}\n" +" bx lr\n" + ); + +void *memmove(void *dest, const void *src, size_t n) +{ + char *p; + const char *q; + if (dest < src) + return memcpy(dest, src, n); + p = dest; p += n; + q = src; q += n; + while (n--) + *--p = *--q; + return dest; +} + +int memcmp(const void *s1, const void *s2, size_t n) +{ + const char *_s1 = s1; + const char *_s2 = s2; + while (n--) { + int diff = *_s1++ - *_s2++; + if (diff) + return diff; + } + return 0; +} + +size_t strlen(const char *s) +{ + size_t len = 0; + while (*s++) + len++; + return len; +} + +size_t strnlen(const char *s, size_t maxlen) +{ + size_t len = 0; + while (maxlen-- && *s++) + len++; + return len; +} + +int strcmp(const char *s1, const char *s2) +{ + return strncmp(s1, s2, ~0); +} + +int strncmp(const char *s1, const char *s2, size_t n) +{ + while (n--) { + int diff = *s1 - *s2; + if (diff || !*s1) + return diff; + s1++; s2++; + } + return 0; +} + +char *strrchr(const char *s, int c) +{ + char *p = NULL; + int d; + while ((d = *s)) { + if (d == c) p = (char *)s; + s++; + } + return p; +} + +char *strcpy(char *dest, const char *src) +{ + char *p = dest; + const char *q = src; + while ((*p++ = *q++) != '\0') + continue; + return dest; +} + +int tolower(int c) +{ + if ((c >= 'A') && (c <= 'Z')) + c += 'a' - 'A'; + return c; +} + +int isspace(int c) +{ + return (c == ' ') || (c == '\t') || (c == '\n') || (c == '\r') + || (c == '\f') || (c == '\v'); +} + +long int strtol(const char *nptr, char **endptr, int base) +{ + long int val = 0; + const char *p = nptr; + bool_t is_neg = FALSE; + int c; + + /* Optional whitespace prefix. */ + while (isspace(*p)) + p++; + c = tolower(*p); + + /* Optional sign prefix: +, -. */ + if ((c == '+') || (c == '-')) { + is_neg = (c == '-'); + c = tolower(*++p); + } + + /* Optional base prefix: 0, 0x. */ + if (c == '0') { + if (base == 0) + base = 8; + c = tolower(*++p); + if (c == 'x') { + if (base == 0) + base = 16; + if (base != 16) + goto out; + c = tolower(*++p); + } + } + + /* Digits. */ + for (;;) { + /* Convert c to a digit [0123456789abcdefghijklmnopqrstuvwxyz]. */ + if ((c >= '0') && (c <= '9')) + c -= '0'; + else if ((c >= 'a') && (c <= 'z')) + c -= 'a' - 10; + else + break; + if (c >= base) + break; + val = (val * base) + c; + c = tolower(*++p); + } + +out: + if (endptr) + *endptr = (char *)p; + return is_neg ? -val : val; +} + +/* + * Local variables: + * mode: C + * c-file-style: "Linux" + * c-basic-offset: 4 + * tab-width: 4 + * indent-tabs-mode: nil + * End: + */ diff --git a/src/vectors.S b/src/vectors.S new file mode 100644 index 0000000..9ddd6cf --- /dev/null +++ b/src/vectors.S @@ -0,0 +1,45 @@ + + .syntax unified + + .section .vector_table + +.global vector_table +vector_table: + /* Top of stack */ + .word _thread_stacktop + + /* Exceptions (1-15) */ +#define E(x) .word x; .weak x; .thumb_set x, EXC_unused; + E(EXC_reset) + E(EXC_nmi) + E(EXC_hard_fault) + E(EXC_memory_management_fault) + E(EXC_bus_fault) + E(EXC_usage_fault) + E(EXC_7) + E(EXC_8) + E(EXC_9) + E(EXC_10) + E(EXC_sv_call) + E(EXC_12) + E(EXC_13) + E(EXC_pend_sv) + E(EXC_systick) + + /* Interrupts/IRQs (0-67) */ +#define I(n) E(IRQ_##n) + I( 0) I( 1) I( 2) I( 3) I( 4) I( 5) I( 6) I( 7) I( 8) I( 9) + I(10) I(11) I(12) I(13) I(14) I(15) I(16) I(17) I(18) I(19) + I(20) I(21) I(22) I(23) I(24) I(25) I(26) I(27) I(28) I(29) + I(30) I(31) I(32) I(33) I(34) I(35) I(36) I(37) I(38) I(39) + I(40) I(41) I(42) I(43) I(44) I(45) I(46) I(47) I(48) I(49) + I(50) I(51) I(52) I(53) I(54) I(55) I(56) I(57) I(58) I(59) + I(60) I(61) I(62) I(63) I(64) I(65) I(66) I(67) + + .text + .thumb_func +.global EXC_unused +EXC_unused: + push {r4, r5, r6, r7, r8, r9, r10, r11, lr} + mov r0, sp + b EXC_unexpected