diff options
Diffstat (limited to 'src/lpc')
| -rw-r--r-- | src/lpc/Makefile.defs | 42 | ||||
| -rw-r--r-- | src/lpc/altos.ld | 74 | ||||
| -rw-r--r-- | src/lpc/ao_adc_lpc.c | 233 | ||||
| -rw-r--r-- | src/lpc/ao_arch.h | 140 | ||||
| -rw-r--r-- | src/lpc/ao_arch_funcs.h | 224 | ||||
| -rw-r--r-- | src/lpc/ao_beep_lpc.c | 87 | ||||
| -rw-r--r-- | src/lpc/ao_exti.h | 48 | ||||
| -rw-r--r-- | src/lpc/ao_exti_lpc.c | 178 | ||||
| -rw-r--r-- | src/lpc/ao_interrupt.c | 155 | ||||
| -rw-r--r-- | src/lpc/ao_led_lpc.c | 66 | ||||
| -rw-r--r-- | src/lpc/ao_romconfig.c | 25 | ||||
| -rw-r--r-- | src/lpc/ao_serial_lpc.c | 209 | ||||
| -rw-r--r-- | src/lpc/ao_spi_lpc.c | 207 | ||||
| -rw-r--r-- | src/lpc/ao_timer_lpc.c | 180 | ||||
| -rw-r--r-- | src/lpc/ao_usb_lpc.c | 1025 | ||||
| -rw-r--r-- | src/lpc/baud_rate | 131 | ||||
| -rwxr-xr-x | src/lpc/figure-checksum | 39 | ||||
| -rw-r--r-- | src/lpc/lpc.h | 1225 | ||||
| -rw-r--r-- | src/lpc/registers.ld | 19 | 
19 files changed, 4307 insertions, 0 deletions
| diff --git a/src/lpc/Makefile.defs b/src/lpc/Makefile.defs new file mode 100644 index 00000000..b63bdd12 --- /dev/null +++ b/src/lpc/Makefile.defs @@ -0,0 +1,42 @@ +vpath % ../lpc:../product:../drivers:../core:../util:../kalman:../aes:.. +vpath make-altitude ../util +vpath make-kalman ../util +vpath kalman.5c ../kalman +vpath kalman_filter.5c ../kalman +vpath load_csv.5c ../kalman +vpath matrix.5c ../kalman +vpath ao-make-product.5c ../util + +CC=/usr/bin/arm-none-eabi-gcc +SAT=/opt/cortex +SAT_CLIB=$(SAT)/lib/pdclib-cortex-m0.a +SAT_CFLAGS=-I$(SAT)/include + +ifndef VERSION +include ../Version +endif + +AO_CFLAGS=-I. -I../lpc -I../core -I../drivers -I.. +LPC_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m0 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) $(SAT_CFLAGS) + +LDFLAGS=-L../stm -Wl,-Taltos.ld + +NICKLE=nickle + +V=0 +# The user has explicitly enabled quiet compilation. +ifeq ($(V),0) +quiet = @printf "  $1 $2 $@\n"; $($1) +endif +# Otherwise, print the full command line. +quiet ?= $($1) + +.c.o: +	$(call quiet,CC) -c $(CFLAGS) -o $@ $< + +ao_serial_lpc.h: ../lpc/baud_rate ao_pins.h +	nickle ../lpc/baud_rate `awk '/AO_LPC_CLKOUT/{print $$3}' ao_pins.h` > $@ + +ao_serial_lpc.o: ao_serial_lpc.h + +.DEFAULT_GOAL=all diff --git a/src/lpc/altos.ld b/src/lpc/altos.ld new file mode 100644 index 00000000..4d6f35a8 --- /dev/null +++ b/src/lpc/altos.ld @@ -0,0 +1,74 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +MEMORY { +	rom (rx) : ORIGIN = 0x00000000, LENGTH = 32K +	ram (!w) : ORIGIN = 0x10000000, LENGTH = 4K - 128 +	usb (!x) : ORIGIN = 0x20004000 + 2K - 256, LENGTH = 256 +	stack (!w) : ORIGIN = 0x10000000 + 4K - 128, LENGTH = 128 +} + +INCLUDE registers.ld + +EXTERN (lpc_interrupt_vector) + +SECTIONS { +	/* +	 * Rom contents +	 */ + +	.text ORIGIN(rom) : { +		__text_start__ = .; +		*(.interrupt)	/* Interrupt vectors */ + +		. = ORIGIN(rom) + 0x100; + +		ao_romconfig.o(.romconfig*) +		ao_product.o(.romconfig*) + +		*(.text*)	/* Executable code */ +		*(.rodata*)	/* Constants */ + +	} > rom + +	.ARM.exidx : { +		*(.ARM.exidx* .gnu.linkonce.armexidx.*) +		__text_end__ = .; +	} > rom + +	/* Data -- relocated to RAM, but written to ROM +	 */ +	.data ORIGIN(ram) : AT (ADDR(.ARM.exidx) + SIZEOF (.ARM.exidx)) { +		__data_start__ = .; +		*(.data)	/* initialized data */ +		__data_end__ = .; +		__bss_start__ = .; +	} >ram + +	.bss : { +		*(.bss) +		*(COMMON) +		__bss_end__ = .; +	} >ram +	PROVIDE(end = .); + +	PROVIDE(__stack__ = ORIGIN(stack) + LENGTH(stack)); +} + +ENTRY(start); + + diff --git a/src/lpc/ao_adc_lpc.c b/src/lpc/ao_adc_lpc.c new file mode 100644 index 00000000..9ecc7c13 --- /dev/null +++ b/src/lpc/ao_adc_lpc.c @@ -0,0 +1,233 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_data.h> + +#ifndef AO_ADC_0 +#define AO_ADC_0	0 +#endif + +#ifndef AO_ADC_1 +#define AO_ADC_1	0 +#endif + +#ifndef AO_ADC_2 +#define AO_ADC_2	0 +#endif + +#ifndef AO_ADC_3 +#define AO_ADC_3	0 +#endif + +#ifndef AO_ADC_4 +#define AO_ADC_4	0 +#endif + +#ifndef AO_ADC_5 +#define AO_ADC_5	0 +#endif + +#ifndef AO_ADC_6 +#define AO_ADC_6	0 +#endif + +#ifndef AO_ADC_7 +#define AO_ADC_7	0 +#endif + +#if AO_ADC_7 +# define AO_ADC_LAST		7 +#else +# if AO_ADC_6 +#  define AO_ADC_LAST		6 +# else +#  if AO_ADC_5 +#   define AO_ADC_LAST		5 +#  else +#   if AO_ADC_4 +#    define AO_ADC_LAST		4 +#   else +#    if AO_ADC_3 +#     define AO_ADC_LAST	3 +#    else +#     if AO_ADC_2 +#      define AO_ADC_LAST	2 +#     else +#      if AO_ADC_1 +#       define AO_ADC_LAST	1 +#      else +#       if AO_ADC_0 +#        define AO_ADC_LAST	0 +#       endif +#      endif +#     endif +#    endif +#   endif +#  endif +# endif +#endif + +static uint8_t			ao_adc_ready; + +void  lpc_adc_isr(void) +{ +	uint32_t	stat = lpc_adc.stat; +	uint16_t	*out = (uint16_t *) &ao_data_ring[ao_data_head].adc; +	vuint32_t	*in = &lpc_adc.dr[0]; + +	lpc_adc.cr = 0; +	lpc_adc.stat = 0; + +	/* Store converted values in packet */ + +#if AO_ADC_0 +	*out++ = lpc_adc.dr[0] >> 1; +#endif +#if AO_ADC_1 +	*out++ = lpc_adc.dr[1] >> 1; +#endif +#if AO_ADC_2 +	*out++ = lpc_adc.dr[2] >> 1; +#endif +#if AO_ADC_3 +	*out++ = lpc_adc.dr[3] >> 1; +#endif +#if AO_ADC_4 +	*out++ = lpc_adc.dr[4] >> 1; +#endif +#if AO_ADC_5 +	*out++ = lpc_adc.dr[5] >> 1; +#endif +#if AO_ADC_6 +	*out++ = lpc_adc.dr[6] >> 1; +#endif +#if AO_ADC_7 +	*out++ = lpc_adc.dr[7] >> 1; +#endif + +	AO_DATA_PRESENT(AO_DATA_ADC); +	if (ao_data_present == AO_DATA_ALL) { +#if HAS_MS5607 +		ao_data_ring[ao_data_head].ms5607_raw = ao_ms5607_current; +#endif +#if HAS_MMA655X +		ao_data_ring[ao_data_head].mma655x = ao_mma655x_current; +#endif +#if HAS_HMC5883 +		ao_data_ring[ao_data_head].hmc5883 = ao_hmc5883_current; +#endif +#if HAS_MPU6000 +		ao_data_ring[ao_data_head].mpu6000 = ao_mpu6000_current; +#endif +		ao_data_ring[ao_data_head].tick = ao_tick_count; +		ao_data_head = ao_data_ring_next(ao_data_head); +		ao_wakeup((void *) &ao_data_head); +	} +	ao_adc_ready = 1; +} + +#define AO_ADC_MASK	((AO_ADC_0 << 0) |	\ +			 (AO_ADC_1 << 1) |	\ +			 (AO_ADC_2 << 2) |	\ +			 (AO_ADC_3 << 3) |	\ +			 (AO_ADC_4 << 4) |	\ +			 (AO_ADC_5 << 5) |	\ +			 (AO_ADC_6 << 6) |	\ +			 (AO_ADC_7 << 7)) + +#define AO_ADC_CLKDIV	(AO_LPC_CLKOUT / 4500000) + +/* + * Start the ADC sequence using the DMA engine + */ +void +ao_adc_poll(void) +{ +	if (!ao_adc_ready) +		return; +	ao_adc_ready = 0; + +	lpc_adc.cr = ((AO_ADC_MASK << LPC_ADC_CR_SEL) | +		      (AO_ADC_CLKDIV << LPC_ADC_CR_CLKDIV) | +		      (1 << LPC_ADC_CR_BURST) | +		      (LPC_ADC_CR_CLKS_11 << LPC_ADC_CR_CLKS)); +} + +static void +ao_adc_dump(void) __reentrant +{ +	struct ao_data	packet; +	int16_t *d; +	uint8_t i; + +	ao_data_get(&packet); +#ifdef AO_ADC_DUMP +	AO_ADC_DUMP(&packet); +#else +	printf("tick: %5u",  packet.tick); +	d = (int16_t *) (&packet.adc); +	for (i = 0; i < AO_NUM_ADC; i++) +		printf (" %2d: %5d", i, d[i]); +	printf("\n"); +#endif +} + +__code struct ao_cmds ao_adc_cmds[] = { +	{ ao_adc_dump,	"a\0Display current ADC values" }, +	{ 0, NULL }, +}; + +void +ao_adc_init(void) +{ +	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_ADC); +	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_ADC_PD); + +	/* Enable interrupt when last channel is complete */ +	lpc_adc.inten = (1 << AO_ADC_LAST); + +	lpc_nvic_set_enable(LPC_ISR_ADC_POS); +	lpc_nvic_set_priority(LPC_ISR_ADC_POS, AO_LPC_NVIC_CLOCK_PRIORITY); +#if AO_ADC_0 +	ao_enable_analog(0, 11); +#endif +#if AO_ADC_1 +	ao_enable_analog(0, 12); +#endif +#if AO_ADC_2 +	ao_enable_analog(0, 13); +#endif +#if AO_ADC_3 +	ao_enable_analog(0, 14); +#endif +#if AO_ADC_4 +	ao_enable_analog(0, 14); +#endif +#if AO_ADC_5 +	ao_enable_analog(0, 14); +#endif +#if AO_ADC_6 +	ao_enable_analog(0, 14); +#endif +#if AO_ADC_7 +	ao_enable_analog(0, 14); +#endif +	ao_cmd_register(&ao_adc_cmds[0]); + +	ao_adc_ready = 1; +} diff --git a/src/lpc/ao_arch.h b/src/lpc/ao_arch.h new file mode 100644 index 00000000..f605e3d2 --- /dev/null +++ b/src/lpc/ao_arch.h @@ -0,0 +1,140 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_ARCH_H_ +#define _AO_ARCH_H_ + +#include <lpc.h> + +/* + * LPC11U14 definitions and code fragments for AltOS + */ + +#define AO_STACK_SIZE	320 + +#define AO_LED_TYPE	uint16_t + +#define AO_PORT_TYPE	uint32_t + +#ifndef AO_TICK_TYPE +#define AO_TICK_TYPE	uint16_t +#define AO_TICK_SIGNED	int16_t +#endif + +/* Various definitions to make GCC look more like SDCC */ + +#define ao_arch_naked_declare	__attribute__((naked)) +#define ao_arch_naked_define +#define __pdata +#define __data +#define __xdata +#define __code const +#define __reentrant +#define __interrupt(n) +#define __at(n) + +#define ao_arch_reboot() arm_scb.aircr = ((0x05fa << 16) |	\ +					  (0 << 15) |		\ +					  (1 << 2) |		\ +					  (0 << 1)) + +#define ao_arch_nop()		asm("nop") + +#define ao_arch_interrupt(n)	/* nothing */ + +#undef putchar +#undef getchar +#define putchar(c)	ao_putchar(c) +#define getchar		ao_getchar + +extern void putchar(char c); +extern char getchar(void); + +/* + * ao_romconfig.c + */ + +#define AO_ROMCONFIG_VERSION	2 + +#define AO_ROMCONFIG_SYMBOL(a) __attribute__((section(".romconfig"))) const + +extern const uint16_t ao_romconfig_version; +extern const uint16_t ao_romconfig_check; +extern const uint16_t ao_serial_number; +extern const uint32_t ao_radio_cal; + +#define ao_arch_task_members\ +	uint32_t *sp;			/* saved stack pointer */ + +#define ao_arch_block_interrupts()	asm("cpsid i") +#define ao_arch_release_interrupts()	asm("cpsie i") + +/* + * For now, we're running at a weird frequency + */ + +#if AO_HSE +#define AO_PLLSRC	AO_HSE +#else +#define AO_PLLSRC	STM_HSI_FREQ +#endif + +#define AO_PLLVCO	(AO_PLLSRC * AO_PLLMUL) +#define AO_SYSCLK	(AO_PLLVCO / AO_PLLDIV) +#define AO_HCLK		(AO_SYSCLK / AO_AHB_PRESCALER) +#define AO_PCLK1	(AO_HCLK / AO_APB1_PRESCALER) +#define AO_PCLK2	(AO_HCLK / AO_APB2_PRESCALER) + +#if AO_APB1_PRESCALER == 1 +#define AO_TIM23467_CLK		AO_PCLK1 +#else +#define AO_TIM23467_CLK		(2 * AO_PCLK1) +#endif + +#if AO_APB2_PRESCALER == 1 +#define AO_TIM91011_CLK		AO_PCLK2 +#else +#define AO_TIM91011_CLK		(2 * AO_PCLK2) +#endif + +#define AO_LPC_NVIC_HIGH_PRIORITY	0 +#define AO_LPC_NVIC_CLOCK_PRIORITY	1 +#define AO_LPC_NVIC_MED_PRIORITY	2 +#define AO_LPC_NVIC_LOW_PRIORITY	3 + +void +ao_adc_init(void); + +#define AO_USB_OUT_EP	2 +#define AO_USB_IN_EP	3 + +void +ao_serial_init(void); + +/* SPI definitions */ + +#define AO_SPI_SPEED_12MHz		4 +#define AO_SPI_SPEED_6MHz		8 +#define AO_SPI_SPEED_4MHz		12 +#define AO_SPI_SPEED_2MHz		24 +#define AO_SPI_SPEED_1MHz		48 +#define AO_SPI_SPEED_500kHz		96 +#define AO_SPI_SPEED_250kHz		192 + +#define AO_SPI_SPEED_FAST	AO_SPI_SPEED_12MHz + +#endif /* _AO_ARCH_H_ */ diff --git a/src/lpc/ao_arch_funcs.h b/src/lpc/ao_arch_funcs.h new file mode 100644 index 00000000..204d1227 --- /dev/null +++ b/src/lpc/ao_arch_funcs.h @@ -0,0 +1,224 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_ARCH_FUNCS_H_ +#define _AO_ARCH_FUNCS_H_ + +#define ao_spi_get_bit(reg,bit,pin,bus,speed) ao_spi_get_mask(reg,(1<<bit),bus,speed) +#define ao_spi_put_bit(reg,bit,pin,bus) ao_spi_put_mask(reg,(1<<bit),bus) + +#define ao_enable_port(port) (lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_GPIO)) + +#define lpc_all_bit(port,bit)	(((port) << 5) | (bit)) + +#define ao_gpio_set(port, bit, pin, v)	(lpc_gpio.byte[lpc_all_bit(port,bit)] = (v)) + +#define ao_gpio_get(port, bit, pin) 	(lpc_gpio_byte[lpc_all_bit(port,bit)]) + +#define ao_enable_output(port,bit,pin,v) do {			\ +		ao_enable_port(port);				\ +		ao_gpio_set(port, bit, pin, v);			\ +		lpc_gpio.dir[port] |= (1 << bit);		\ +	} while (0) + +#define ao_enable_input(port,bit,mode) do {				\ +		vuint32_t *_ioconf = &lpc_ioconf.pio0_0 + ((port)*24+(bit)); \ +		vuint32_t _mode;					\ +		ao_enable_port(port);					\ +		lpc_gpio.dir[port] &= ~(1 << bit);			\ +		if (mode == AO_EXTI_MODE_PULL_UP)			\ +			_mode = LPC_IOCONF_MODE_PULL_UP << LPC_IOCONF_MODE; \ +		else if (mode == AO_EXTI_MODE_PULL_DOWN)		\ +			_mode = LPC_IOCONF_MODE_PULL_UP << LPC_IOCONF_MODE; \ +		else							\ +			_mode = LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE; \ +		*_ioconf = ((*_ioconf & ~(LPC_IOCONF_MODE_MASK << LPC_IOCONF_MODE)) | \ +			    _mode |					\ +			    (1 << LPC_IOCONF_ADMODE));			\ +	} while (0) + +#define ao_enable_analog(port,bit) do {					\ +		vuint32_t *_ioconf = &lpc_ioconf.pio0_0 + ((port)*24+(bit)); \ +		ao_enable_port(port);					\ +		lpc_gpio.dir[port] &= ~(1 << bit);			\ +		*_ioconf = *_ioconf & ~((1 << LPC_IOCONF_ADMODE) |	\ +					(LPC_IOCONF_MODE_MASK << LPC_IOCONF_MODE)); \ +	} while (0) +	 +#define ARM_PUSH32(stack, val)	(*(--(stack)) = (val)) + +static inline uint32_t +ao_arch_irqsave(void) { +	uint32_t	primask; +	asm("mrs %0,primask" : "=&r" (primask)); +	ao_arch_block_interrupts(); +	return primask; +} + +static inline void +ao_arch_irqrestore(uint32_t primask) { +	asm("msr primask,%0" : : "r" (primask)); +} + +static inline void +ao_arch_memory_barrier() { +	asm volatile("" ::: "memory"); +} + +static inline void +ao_arch_init_stack(struct ao_task *task, void *start) +{ +	uint32_t	*sp = (uint32_t *) (task->stack + AO_STACK_SIZE); +	uint32_t	a = (uint32_t) start; +	int		i; + +	/* Return address (goes into LR) */ +	ARM_PUSH32(sp, a); + +	/* Clear register values r0-r7 */ +	i = 8; +	while (i--) +		ARM_PUSH32(sp, 0); + +	/* APSR */ +	ARM_PUSH32(sp, 0); + +	/* PRIMASK with interrupts enabled */ +	ARM_PUSH32(sp, 0); + +	task->sp = sp; +} + +static inline void ao_arch_save_regs(void) { +	/* Save general registers */ +	asm("push {r0-r7,lr}\n"); + +	/* Save APSR */ +	asm("mrs r0,apsr"); +	asm("push {r0}"); + +	/* Save PRIMASK */ +	asm("mrs r0,primask"); +	asm("push {r0}"); +} + +static inline void ao_arch_save_stack(void) { +	uint32_t	*sp; +	asm("mov %0,sp" : "=&r" (sp) ); +	ao_cur_task->sp = (sp); +	if ((uint8_t *) sp < &ao_cur_task->stack[0]) +		ao_panic (AO_PANIC_STACK); +} + +static inline void ao_arch_restore_stack(void) { +	uint32_t	sp; +	sp = (uint32_t) ao_cur_task->sp; + +	/* Switch stacks */ +	asm("mov sp, %0" : : "r" (sp) ); + +	/* Restore PRIMASK */ +	asm("pop {r0}"); +	asm("msr primask,r0"); + +	/* Restore APSR */ +	asm("pop {r0}"); +	asm("msr apsr_nczvq,r0"); + +	/* Restore general registers and return */ +	asm("pop {r0-r7,pc}\n"); +} + +#define ao_arch_isr_stack() + +#define ao_arch_wait_interrupt() do {			\ +		asm(".global ao_idle_loc\n\twfi\nao_idle_loc:");	\ +		ao_arch_release_interrupts();				\ +		ao_arch_block_interrupts();				\ +	} while (0) + +#define ao_arch_critical(b) do {				\ +		ao_arch_block_interrupts();			\ +		do { b } while (0);				\ +		ao_arch_release_interrupts();			\ +	} while (0) + +/* + * SPI + */ + +#define ao_spi_set_cs(port,mask) (lpc_gpio.clr[port] = (mask)) +#define ao_spi_clr_cs(port,mask) (lpc_gpio.set[port] = (mask)) + +#define ao_spi_get_mask(port,mask,bus,speed) do {	\ +		ao_spi_get(bus, speed);			\ +		ao_spi_set_cs(port, mask);		\ +	} while (0) + +#define ao_spi_put_mask(reg,mask,bus) do {	\ +		ao_spi_clr_cs(reg,mask);	\ +		ao_spi_put(bus);		\ +	} while (0) + +#define ao_spi_get_bit(reg,bit,pin,bus,speed) ao_spi_get_mask(reg,(1<<bit),bus,speed) +#define ao_spi_put_bit(reg,bit,pin,bus) ao_spi_put_mask(reg,(1<<bit),bus) + +void +ao_spi_get(uint8_t spi_index, uint32_t speed); + +void +ao_spi_put(uint8_t spi_index); + +void +ao_spi_send(void *block, uint16_t len, uint8_t spi_index); + +void +ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index); + +void +ao_spi_recv(void *block, uint16_t len, uint8_t spi_index); + +void +ao_spi_duplex(void *out, void *in, uint16_t len, uint8_t spi_index); + +extern uint16_t	ao_spi_speed[LPC_NUM_SPI]; + +void +ao_spi_init(void); + +#define ao_spi_init_cs(port, mask) do {					\ +		uint8_t __bit__;					\ +		for (__bit__ = 0; __bit__ < 32; __bit__++) {		\ +			if (mask & (1 << __bit__))			\ +				ao_enable_output(port, __bit__, PIN, 1); \ +		}							\ +	} while (0) + +#define HAS_ARCH_START_SCHEDULER	1 + +static inline void ao_arch_start_scheduler(void) { +	uint32_t	sp; +	uint32_t	control; + +	asm("mrs %0,msp" : "=&r" (sp)); +	asm("msr psp,%0" : : "r" (sp)); +	asm("mrs %0,control" : "=&r" (control)); +	control |= (1 << 1); +	asm("msr control,%0" : : "r" (control)); +} + +#endif /* _AO_ARCH_FUNCS_H_ */ diff --git a/src/lpc/ao_beep_lpc.c b/src/lpc/ao_beep_lpc.c new file mode 100644 index 00000000..eb9132b8 --- /dev/null +++ b/src/lpc/ao_beep_lpc.c @@ -0,0 +1,87 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +void +ao_beep(uint8_t beep) +{ +	if (beep == 0) { +		lpc_ct32b1.tcr = ((0 << LPC_CT32B_TCR_CEN) | +				  (1 << LPC_CT32B_TCR_CRST)); +		lpc_scb.sysahbclkctrl &= ~(1 << LPC_SCB_SYSAHBCLKCTRL_CT32B1); +	} else { +		lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_CT32B1); + +		/* Set prescaler to match cc1111 clocks +		 */ +		lpc_ct32b1.pr = AO_LPC_SYSCLK / 750000 - 1; + +		/* Write the desired data in the match registers */ + +		/* Reset after two time units */ +		lpc_ct32b1.mr[0] = beep << 1; + +		/* PWM width is half of that */ +		lpc_ct32b1.mr[1] = beep; + +		/* Flip output 1 on PWM match */ +		lpc_ct32b1.emr = (LPC_CT32B_EMR_EMC_TOGGLE << LPC_CT32B_EMR_EMC1); + +		/* Reset on match 0 */ +		lpc_ct32b1.mcr = (1 << LPC_CT32B_MCR_MR0R); + +		/* PWM on match 1 */ +		lpc_ct32b1.pwmc = (1 << LPC_CT32B_PWMC_PWMEN1); +		 +		/* timer mode */ +		lpc_ct32b1.ctcr = 0; + +		/* And turn the timer on */ +		lpc_ct32b1.tcr = ((1 << LPC_CT32B_TCR_CEN) | +				  (0 << LPC_CT32B_TCR_CRST)); +	} +} + +void +ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant +{ +	ao_beep(beep); +	ao_delay(ticks); +	ao_beep(0); +} + +void +ao_beep_init(void) +{ +	/* Our beeper is on c32b1_mat1 +	 * which is on pin pio0_14 +	 */ + +	lpc_ioconf.pio0_14 = ((LPC_IOCONF_FUNC_PIO0_14_CT32B1_MAT1 << LPC_IOCONF_FUNC) | +			      (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | +			      (0 << LPC_IOCONF_HYS) | +			      (0 << LPC_IOCONF_INV) | +			      (1 << LPC_IOCONF_ADMODE) | +			      (0 << LPC_IOCONF_OD)); + +	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_CT32B1); + +	/* Disable the counter and reset the value */ +	lpc_ct32b1.tcr = ((0 << LPC_CT32B_TCR_CEN) | +			  (1 << LPC_CT32B_TCR_CRST)); +} diff --git a/src/lpc/ao_exti.h b/src/lpc/ao_exti.h new file mode 100644 index 00000000..e8599eb4 --- /dev/null +++ b/src/lpc/ao_exti.h @@ -0,0 +1,48 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_EXTI_H_ +#define _AO_EXTI_H_ + +#define AO_EXTI_MODE_RISING	1 +#define AO_EXTI_MODE_FALLING	2 +#define AO_EXTI_MODE_PULL_UP	4 +#define AO_EXTI_MODE_PULL_DOWN	8 +#define AO_EXTI_PRIORITY_LOW	16 +#define AO_EXTI_PRIORITY_MED	0 +#define AO_EXTI_PRIORITY_HIGH	32 +#define AO_EXTI_PIN_NOCONFIGURE	64 + +void +ao_exti_setup(uint8_t gpio, uint8_t pin, uint8_t mode, void (*callback)()); + +void +ao_exti_set_mode(uint8_t gpio, uint8_t pin, uint8_t mode); + +void +ao_exti_set_callback(uint8_t gpio, uint8_t pin, void (*callback)()); + +void +ao_exti_enable(uint8_t gpio, uint8_t pin); + +void +ao_exti_disable(uint8_t gpio, uint8_t pin); + +void +ao_exti_init(void); + +#endif /* _AO_EXTI_H_ */ diff --git a/src/lpc/ao_exti_lpc.c b/src/lpc/ao_exti_lpc.c new file mode 100644 index 00000000..588cf58c --- /dev/null +++ b/src/lpc/ao_exti_lpc.c @@ -0,0 +1,178 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_exti.h> + +#define LPC_NUM_PINS	56 +#define LPC_NUM_PINT	8 + +static void	(*ao_exti_callback[LPC_NUM_PINT])(void); + +static uint8_t	ao_pint_map[LPC_NUM_PINS]; +static uint8_t	ao_pint_mode[LPC_NUM_PINS]; +static uint8_t	ao_pint_inuse; +static uint8_t	ao_pint_enabled; + +static void +ao_exti_isr(uint8_t pint) +{ +	uint8_t	mask = 1 << pint; + +	if (lpc_gpio_pin.ist & mask) { +		lpc_gpio_pin.ist = mask; +		lpc_gpio_pin.rise = mask; +		lpc_gpio_pin.fall = mask; + +		(*ao_exti_callback[pint]) (); +	} +} + +#define pin_isr(n)	void lpc_pin_int ## n ## _isr(void) { ao_exti_isr(n); } +pin_isr(0) +pin_isr(1) +pin_isr(2) +pin_isr(3) +pin_isr(4) +pin_isr(5) +pin_isr(6) +pin_isr(7) + +#define pin_id(port,pin)	((port) * 24 + (pin)); + +static void +_ao_exti_set_enable(uint8_t pint) +{ +	uint8_t		mask = 1 << pint; +	uint8_t		mode; + +	if (ao_pint_enabled & mask) +		mode = ao_pint_mode[pint]; +	else +		mode = 0; + +	if (mode & AO_EXTI_MODE_RISING) +		lpc_gpio_pin.sienr = mask; +	else +		lpc_gpio_pin.cienr = mask; +		 +	if (mode & AO_EXTI_MODE_FALLING) +		lpc_gpio_pin.sienf = mask; +	else +		lpc_gpio_pin.cienf = mask; +	lpc_gpio_pin.rise = mask; +	lpc_gpio_pin.fall = mask; +} + +void +ao_exti_setup (uint8_t port, uint8_t pin, uint8_t mode, void (*callback)(void)) { +	uint8_t		id = pin_id(port,pin); +	uint8_t	       	pint; +	uint32_t	mask; +	uint8_t		prio; + +	for (pint = 0; pint < LPC_NUM_PINT; pint++) +		if ((ao_pint_inuse & (1 << pint)) == 0) +			break; +	if (pint == LPC_NUM_PINT) +		ao_panic(AO_PANIC_EXTI); + +	if (!mode & AO_EXTI_PIN_NOCONFIGURE) +		ao_enable_input(port, pin, mode); + +	ao_arch_block_interrupts(); +	mask = (1 << pint); +	ao_pint_inuse |= mask; +	ao_pint_enabled &= ~mask; +	 +	ao_pint_map[id] = pint; +	ao_exti_callback[pint] = callback; + +	/* configure gpio to interrupt routing */ +	lpc_scb.pintsel[pint] = id; + +	/* Set edge triggered */ +	lpc_gpio_pin.isel &= ~mask; + +	ao_pint_enabled &= ~mask; +	ao_pint_mode[pint] = mode; +	_ao_exti_set_enable(pint); + +	/* Set interrupt mask and rising/falling mode */ + +	prio = AO_LPC_NVIC_MED_PRIORITY; +	if (mode & AO_EXTI_PRIORITY_LOW) +		prio = AO_LPC_NVIC_LOW_PRIORITY; +	else if (mode & AO_EXTI_PRIORITY_HIGH) +		prio = AO_LPC_NVIC_HIGH_PRIORITY; + +	/* Set priority and enable */ +	lpc_nvic_set_priority(LPC_ISR_PIN_INT0_POS + pint, prio); +	lpc_nvic_set_enable(LPC_ISR_PIN_INT0_POS + pint); +	ao_arch_release_interrupts(); +} + +void +ao_exti_set_mode(uint8_t port, uint8_t pin, uint8_t mode) +{ +	uint8_t		id = pin_id(port,pin); +	uint8_t		pint = ao_pint_map[id]; + +	ao_arch_block_interrupts(); +	ao_pint_mode[pint] = mode; +	_ao_exti_set_enable(pint); +	ao_arch_release_interrupts(); +} + +void +ao_exti_set_callback(uint8_t port, uint8_t pin, void (*callback)()) { +	uint8_t		id = pin_id(port,pin); +	uint8_t		pint = ao_pint_map[id]; + +	ao_exti_callback[pint] = callback; +} + +void +ao_exti_enable(uint8_t port, uint8_t pin) +{ +	uint8_t		id = pin_id(port,pin); +	uint8_t		pint = ao_pint_map[id]; +	uint8_t		mask = 1 << pint; + +	ao_arch_block_interrupts(); +	ao_pint_enabled |= mask; +	_ao_exti_set_enable(pint); +	ao_arch_release_interrupts(); +} + +void +ao_exti_disable(uint8_t port, uint8_t pin) { +	uint8_t		id = pin_id(port,pin); +	uint8_t		pint = ao_pint_map[id]; +	uint8_t		mask = 1 << pint; + +	ao_arch_block_interrupts(); +	ao_pint_enabled &= ~mask; +	_ao_exti_set_enable(pint); +	ao_arch_release_interrupts(); +} + +void +ao_exti_init(void) +{ +	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_PINT); +} diff --git a/src/lpc/ao_interrupt.c b/src/lpc/ao_interrupt.c new file mode 100644 index 00000000..b5e67007 --- /dev/null +++ b/src/lpc/ao_interrupt.c @@ -0,0 +1,155 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <string.h> + +extern void main(void); +extern char __stack__; +extern char __text_start__, __text_end__; +extern char __data_start__, __data_end__; +extern char __bss_start__, __bss_end__; + +/* Interrupt functions */ + +void lpc_halt_isr(void) +{ +	ao_panic(AO_PANIC_CRASH); +} + +void lpc_ignore_isr(void) +{ +} + +int x; + +void start(void) { +	x = 0; +	memcpy(&__data_start__, &__text_end__, &__data_end__ - &__data_start__); +	memset(&__bss_start__, '\0', &__bss_end__ - &__bss_start__); +	main(); +} + +#define STRINGIFY(x) #x + +#define isr(name) \ +	void __attribute__ ((weak)) lpc_ ## name ## _isr(void); \ +	_Pragma(STRINGIFY(weak lpc_ ## name ## _isr = lpc_ignore_isr)) + +#define isr_halt(name) \ +	void __attribute__ ((weak)) lpc_ ## name ## _isr(void); \ +	_Pragma(STRINGIFY(weak lpc_ ## name ## _isr = lpc_halt_isr)) + +isr(nmi) +isr_halt(hardfault) +isr_halt(memmanage) +isr_halt(busfault) +isr_halt(usagefault) +isr(svc) +isr(debugmon) +isr(pendsv) +isr(systick) + +isr(pin_int0)	/* IRQ0 */ +isr(pin_int1) +isr(pin_int2) +isr(pin_int3) +isr(pin_int4)	/* IRQ4 */ +isr(pin_int5) +isr(pin_int6) +isr(pin_int7) + +isr(gint0)	/* IRQ8 */ +isr(gint1) +isr(ssp1) +isr(i2c) + +isr(ct16b0)	/* IRQ16 */ +isr(ct16b1) +isr(ct32b0) +isr(ct32b1) +isr(ssp0)	/* IRQ20 */ +isr(usart) +isr(usb_irq) +isr(usb_fiq) + +isr(adc)	/* IRQ24 */ +isr(wwdt) +isr(bod) +isr(flash) + +isr(usb_wakeup) + +#define i(addr,name)	[(addr)/4] = lpc_ ## name ## _isr +#define c(addr,value)	[(addr)/4] = (value) + +__attribute__ ((section(".interrupt"))) +const void *lpc_interrupt_vector[] = { +	[0] = &__stack__, +	[1] = start, +	i(0x08, nmi), +	i(0x0c, hardfault), +	c(0x10, 0), +	c(0x14, 0), +	c(0x18, 0), +	c(0x1c, 0), +	c(0x20, 0), +	c(0x24, 0), +	c(0x28, 0), +	i(0x2c, svc), +	i(0x30, hardfault), +	i(0x34, hardfault), +	i(0x38, pendsv), +	i(0x3c, systick), + +	i(0x40, pin_int0),	/* IRQ0 */ +	i(0x44, pin_int1), +	i(0x48, pin_int2), +	i(0x4c, pin_int3), +	i(0x50, pin_int4),	/* IRQ4 */ +	i(0x54, pin_int5), +	i(0x58, pin_int6), +	i(0x5c, pin_int7), + +	i(0x60, gint0),		/* IRQ8 */ +	i(0x64, gint1), +	i(0x68, hardfault), +	i(0x6c, hardfault), +	i(0x70, hardfault),	/* IRQ12 */ +	i(0x74, hardfault), +	i(0x78, ssp1), +	i(0x7c, i2c), + +	i(0x80, ct16b0),	/* IRQ16 */ +	i(0x84, ct16b1), +	i(0x88, ct32b0), +	i(0x8c, ct32b1), +	i(0x90, ssp0),		/* IRQ20 */ +	i(0x94, usart), +	i(0x98, usb_irq), +	i(0x9c, usb_fiq), + +	i(0xa0, adc),		/* IRQ24 */ +	i(0xa4, wwdt), +	i(0xa8, bod), +	i(0xac, flash), + +	i(0xb0, hardfault),	/* IRQ28 */ +	i(0xb4, hardfault), +	i(0xb8, usb_wakeup), +	i(0xbc, hardfault),	 +}; diff --git a/src/lpc/ao_led_lpc.c b/src/lpc/ao_led_lpc.c new file mode 100644 index 00000000..7bef51ba --- /dev/null +++ b/src/lpc/ao_led_lpc.c @@ -0,0 +1,66 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> + +__pdata uint16_t ao_led_enable; + +void +ao_led_on(uint16_t colors) +{ +	lpc_gpio.pin[LED_PORT] |= colors; +} + +void +ao_led_off(uint16_t colors) +{ +	lpc_gpio.pin[LED_PORT] &= ~colors; +} + +void +ao_led_set(uint16_t colors) +{ +	uint16_t	on = colors & ao_led_enable; +	uint16_t	off = ~colors & ao_led_enable; + +	ao_led_off(off); +	ao_led_on(on); +} + +void +ao_led_toggle(uint16_t colors) +{ +	lpc_gpio.pin[LED_PORT] ^= colors; +} + +void +ao_led_for(uint16_t colors, uint16_t ticks) __reentrant +{ +	ao_led_on(colors); +	ao_delay(ticks); +	ao_led_off(colors); +} + +void +ao_led_init(uint16_t enable) +{ +	int	bit; + +	ao_led_enable = enable; +	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_GPIO); +	lpc_gpio.dir[LED_PORT] |= enable; +} diff --git a/src/lpc/ao_romconfig.c b/src/lpc/ao_romconfig.c new file mode 100644 index 00000000..cbb922ec --- /dev/null +++ b/src/lpc/ao_romconfig.c @@ -0,0 +1,25 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +AO_ROMCONFIG_SYMBOL (0) uint16_t ao_romconfig_version = AO_ROMCONFIG_VERSION; +AO_ROMCONFIG_SYMBOL (0) uint16_t ao_romconfig_check = ~AO_ROMCONFIG_VERSION; +AO_ROMCONFIG_SYMBOL (0) uint16_t ao_serial_number = 0; +#ifdef AO_RADIO_CAL_DEFAULT +AO_ROMCONFIG_SYMBOL (0) uint32_t ao_radio_cal = AO_RADIO_CAL_DEFAULT; +#endif diff --git a/src/lpc/ao_serial_lpc.c b/src/lpc/ao_serial_lpc.c new file mode 100644 index 00000000..c0424699 --- /dev/null +++ b/src/lpc/ao_serial_lpc.c @@ -0,0 +1,209 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_serial.h> + +struct ao_fifo	ao_usart_rx_fifo; +struct ao_fifo	ao_usart_tx_fifo; +uint8_t		ao_usart_tx_avail; +uint8_t		ao_usart_tx_avail_min; + +#define LPC_USART_TX_FIFO_SIZE	16 + +void +ao_debug_out(char c) +{ +	if (c == '\n') +		ao_debug_out('\r'); +	while (!(lpc_usart.lsr & (1 << LPC_USART_LSR_TEMT))) +		; +	lpc_usart.rbr_thr = c; +} + +static void +_ao_serial_tx_start(void) +{ +	if (!ao_fifo_empty(ao_usart_tx_fifo) && ao_usart_tx_avail) { +		ao_usart_tx_avail--; +		if (ao_usart_tx_avail < ao_usart_tx_avail_min) +			ao_usart_tx_avail_min = ao_usart_tx_avail; +		ao_fifo_remove(ao_usart_tx_fifo, lpc_usart.rbr_thr); +	} +} + +void +lpc_usart_isr(void) +{ +	(void) lpc_usart.iir_fcr; + +	while (lpc_usart.lsr & (1 << LPC_USART_LSR_RDR)) { +		char c = lpc_usart.rbr_thr; +		if (!ao_fifo_full(ao_usart_rx_fifo)) +			ao_fifo_insert(ao_usart_rx_fifo, c); +		ao_wakeup(&ao_usart_rx_fifo); +		if (stdin) +			ao_wakeup(&ao_stdin_ready); +	} +	if (lpc_usart.lsr & (1 << LPC_USART_LSR_THRE)) { +		ao_usart_tx_avail = LPC_USART_TX_FIFO_SIZE; +		_ao_serial_tx_start(); +		ao_wakeup(&ao_usart_tx_fifo); +	} +} + +int +_ao_serial_pollchar(void) +{ +	int	c; +	 +	if (ao_fifo_empty(ao_usart_rx_fifo)) +		c = AO_READ_AGAIN; +	else { +		uint8_t	u; +		ao_fifo_remove(ao_usart_rx_fifo,u); +		c = u; +	} +	return c; +} + +char +ao_serial_getchar(void) +{ +	int c; +	ao_arch_block_interrupts(); +	while ((c = _ao_serial_pollchar()) == AO_READ_AGAIN) +		ao_sleep(&ao_usart_rx_fifo); +	ao_arch_release_interrupts(); +	return (char) c; +} + +void +ao_serial_putchar(char c) +{ +	ao_arch_block_interrupts(); +	while (ao_fifo_full(ao_usart_tx_fifo)) +		ao_sleep(&ao_usart_tx_fifo); +	ao_fifo_insert(ao_usart_tx_fifo, c); +	_ao_serial_tx_start(); +	ao_arch_release_interrupts(); +} + +void +ao_serial_drain(void) +{ +	ao_arch_block_interrupts(); +	while (!ao_fifo_empty(ao_usart_tx_fifo)) +		ao_sleep(&ao_usart_tx_fifo); +	ao_arch_release_interrupts(); +} + +#include "ao_serial_lpc.h" + +void +ao_serial_set_speed(uint8_t speed) +{ +	if (speed > AO_SERIAL_SPEED_115200) +		return; + +	/* Flip to allow access to divisor latches */ +	lpc_usart.lcr |= (1 << LPC_USART_LCR_DLAB); + +	/* DL LSB */ +	lpc_usart.rbr_thr = ao_usart_speeds[speed].dl & 0xff; +	 +	/* DL MSB */ +	lpc_usart.ier = (ao_usart_speeds[speed].dl >> 8) & 0xff; + +	lpc_usart.fdr = ((ao_usart_speeds[speed].divaddval << LPC_USART_FDR_DIVADDVAL) | +			 (ao_usart_speeds[speed].mulval << LPC_USART_FDR_MULVAL)); + +	/* Turn access to divisor latches back off */ +	lpc_usart.lcr &= ~(1 << LPC_USART_LCR_DLAB); +} + +void +ao_serial_init(void) +{ +#if SERIAL_0_18_19 +	lpc_ioconf.pio0_18 = ((LPC_IOCONF_FUNC_PIO0_18_RXD << LPC_IOCONF_FUNC) | +			      (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | +			      (0 << LPC_IOCONF_HYS) | +			      (0 << LPC_IOCONF_INV) | +			      (0 << LPC_IOCONF_OD)); +	lpc_ioconf.pio0_19 = ((LPC_IOCONF_FUNC_PIO0_19_TXD << LPC_IOCONF_FUNC) | +			      (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | +			      (0 << LPC_IOCONF_HYS) | +			      (0 << LPC_IOCONF_INV) | +			      (0 << LPC_IOCONF_OD)); +#endif + +	/* Turn on the USART */ +	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_USART); + +	/* Turn on the USART clock */ +	lpc_scb.uartclkdiv = AO_LPC_CLKOUT / AO_LPC_USARTCLK; + +	/* Configure USART */ + +	/* Enable FIFOs, reset fifo contents, interrupt on 1 received char */ +	lpc_usart.iir_fcr = ((1 << LPC_USART_FCR_FIFOEN) | +			 (1 << LPC_USART_FCR_RXFIFORES) | +			 (1 << LPC_USART_FCR_TXFIFORES) | +			 (LPC_USART_FCR_RXTL_1 << LPC_USART_FCR_RXTL)); + +	ao_usart_tx_avail = LPC_USART_TX_FIFO_SIZE; +	ao_usart_tx_avail_min = LPC_USART_TX_FIFO_SIZE; + +	/* 8 n 1 */ +	lpc_usart.lcr = ((LPC_USART_LCR_WLS_8 << LPC_USART_LCR_WLS) | +			 (LPC_USART_LCR_SBS_1 << LPC_USART_LCR_SBS) | +			 (0 << LPC_USART_LCR_PE) | +			 (LPC_USART_LCR_PS_ODD << LPC_USART_LCR_PS) | +			 (0 << LPC_USART_LCR_BC) | +			 (0 << LPC_USART_LCR_DLAB)); + +	/* Disable flow control */ +	lpc_usart.mcr = ((0 << LPC_USART_MCR_DTRCTRL) | +			 (0 << LPC_USART_MCR_RTSCTRL) | +			 (0 << LPC_USART_MCR_LMS) | +			 (0 << LPC_USART_MCR_RTSEN) | +			 (0 << LPC_USART_MCR_CTSEN)); + +	/* 16x oversampling */ +	lpc_usart.osr = ((0 << LPC_USART_OSR_OSFRAC) | +			 ((16 - 1) << LPC_USART_OSR_OSINT) | +			 (0 << LPC_USART_OSR_FDINT)); + +	/* Full duplex */ +	lpc_usart.hden = ((0 << LPC_USART_HDEN_HDEN)); + +	/* Set baud rate */ +	ao_serial_set_speed(AO_SERIAL_SPEED_9600); + +	/* Enable interrupts */ +	lpc_usart.ier = ((1 << LPC_USART_IER_RBRINTEN) | +			 (1 << LPC_USART_IER_THREINTEN)); + +	lpc_nvic_set_enable(LPC_ISR_USART_POS); +	lpc_nvic_set_priority(LPC_ISR_USART_POS, 0); +#if USE_SERIAL_0_STDIN +	ao_add_stdio(_ao_serial_pollchar, +		     ao_serial_putchar, +		     NULL); +#endif +} diff --git a/src/lpc/ao_spi_lpc.c b/src/lpc/ao_spi_lpc.c new file mode 100644 index 00000000..c3587698 --- /dev/null +++ b/src/lpc/ao_spi_lpc.c @@ -0,0 +1,207 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> + +static uint8_t		ao_spi_mutex[LPC_NUM_SPI]; + +static struct lpc_ssp * const ao_lpc_ssp[LPC_NUM_SPI] = { &lpc_ssp0, &lpc_ssp1 }; + +static uint8_t	spi_dev_null; + +#define tx_busy(lpc_ssp) (lpc_ssp->sr & ((1 << LPC_SSP_SR_BSY) | (1 << LPC_SSP_SR_TNF))) != (1 << LPC_SSP_SR_TNF) +#define rx_busy(lpc_ssp) (lpc_ssp->sr & ((1 << LPC_SSP_SR_BSY) | (1 << LPC_SSP_SR_RNE))) != (1 << LPC_SSP_SR_RNE) + +#define spi_loop(len, put, get) do {					\ +		while (len--) {						\ +			/* Wait for space in the fifo */		\ +			while (tx_busy(lpc_ssp))			\ +				;					\ +									\ +			/* send a byte */				\ +			lpc_ssp->dr = put;				\ +									\ +			/* Wait for byte to appear in the fifo */	\ +			while (rx_busy(lpc_ssp))			\ +				;					\ +									\ +			/* recv a byte */				\ +			get lpc_ssp->dr;				\ +		}							\ +	} while (0); + +void +ao_spi_send(void *block, uint16_t len, uint8_t id) +{ +	uint8_t	*b = block; +	struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + +	spi_loop(len, *b++, (void)); +} + +void +ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t id) +{ +	struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + +	spi_loop(len, value, (void)); +} + +void +ao_spi_recv(void *block, uint16_t len, uint8_t id) +{ +	uint8_t	*b = block; +	struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + +	spi_loop(len, 0xff, *b++ =); +} + +void +ao_spi_duplex(void *out, void *in, uint16_t len, uint8_t id) +{ +	uint8_t	*o = out; +	uint8_t	*i = in; +	struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + +	spi_loop(len, *o++, *i++ =); +} + +void +ao_spi_get(uint8_t id, uint32_t speed) +{ +	struct lpc_ssp	*lpc_ssp = ao_lpc_ssp[id]; + +	ao_mutex_get(&ao_spi_mutex[id]); +	 +	/* Set the clock prescale */ +	lpc_ssp->cpsr = speed; +} + +void +ao_spi_put(uint8_t id) +{ +	ao_mutex_put(&ao_spi_mutex[id]); +} + +static void +ao_spi_channel_init(uint8_t id) +{ +	struct lpc_ssp	*lpc_ssp = ao_lpc_ssp[id]; +	uint8_t	d; + +	lpc_ssp->cr0 = ((LPC_SSP_CR0_DSS_8 << LPC_SSP_CR0_DSS) | +			(LPC_SSP_CR0_FRF_SPI << LPC_SSP_CR0_FRF) | +			(0 << LPC_SSP_CR0_CPOL) | +			(0 << LPC_SSP_CR0_CPHA) | +			(0 << LPC_SSP_CR0_SCR)); + +	/* Enable the device */ +	lpc_ssp->cr1 = ((0 << LPC_SSP_CR1_LBM) | +			(1 << LPC_SSP_CR1_SSE) | +			(LPC_SSP_CR1_MS_MASTER << LPC_SSP_CR1_MS) | +			(0 << LPC_SSP_CR1_SOD)); + +	/* Drain the receive fifo */ +	for (d = 0; d < LPC_SSP_FIFOSIZE; d++) +		(void) lpc_ssp->dr; +} + +void +ao_spi_init(void) +{ +#if HAS_SPI_0 +	/* Configure pins */ +#if SPI_SCK0_P0_6 +	lpc_ioconf.pio0_6 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_6_SCK0); +#define HAS_SCK0 +#endif +#if SPI_SCK0_P0_10 +	lpc_ioconf.pio0_10 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_10_SCK0); +#define HAS_SCK0 +#endif +#if SPI_SCK0_P1_29 +	lpc_ioconf.pio1_29 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_29_SCK0); +#define HAS_SCK0 +#endif +#ifndef HAS_SCK0 +#error "No pin specified for SCK0" +#endif +	lpc_ioconf.pio0_8 = ao_lpc_alternate(LPC_IOCONF_FUNC_MISO0); +	lpc_ioconf.pio0_9 = ao_lpc_alternate(LPC_IOCONF_FUNC_MOSI0); + +	/* Enable the device */ +	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_SSP0); + +	/* Turn on the clock */ +	lpc_scb.ssp0clkdiv = 1; + +	/* Reset the device */ +	lpc_scb.presetctrl &= ~(1 << LPC_SCB_PRESETCTRL_SSP0_RST_N); +	lpc_scb.presetctrl |= (1 << LPC_SCB_PRESETCTRL_SSP0_RST_N); +	ao_spi_channel_init(0); +#endif			    + +#if HAS_SPI_1 + +#if SPI_SCK1_P1_15 +	lpc_ioconf.pio1_15 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_15_SCK1); +#define HAS_SCK1 +#endif +#if SPI_SCK1_P1_20 +	lpc_ioconf.pio1_20 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_20_SCK1); +#define HAS_SCK1 +#endif +#ifndef HAS_SCK1 +#error "No pin specified for SCK1" +#endif + +#if SPI_MISO1_P0_22 +	lpc_ioconf.pio0_22 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_22_MISO1); +#define HAS_MISO1 +#endif +#if SPI_MISO1_P1_21 +	lpc_ioconf.pio1_21 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_21_MISO1); +#define HAS_MISO1 +#endif +#ifndef HAS_MISO1 +#error "No pin specified for MISO1" +#endif + +#if SPI_MOSI1_P0_21 +	lpc_ioconf.pio0_21 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_21_MOSI1); +#define HAS_MOSI1 +#endif +#if SPI_MOSI1_P1_22 +	lpc_ioconf.pio1_22 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_22_MOSI1); +#define HAS_MOSI1 +#endif +#ifndef HAS_MOSI1 +#error "No pin specified for MOSI1" +#endif +		 +	/* Enable the device */ +	lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_SSP1); + +	/* Turn on the clock */ +	lpc_scb.ssp1clkdiv = 1; + +	/* Reset the device */ +	lpc_scb.presetctrl &= ~(1 << LPC_SCB_PRESETCTRL_SSP1_RST_N); +	lpc_scb.presetctrl |= (1 << LPC_SCB_PRESETCTRL_SSP1_RST_N); +	ao_spi_channel_init(1); +#endif /* HAS_SPI_1 */ +} diff --git a/src/lpc/ao_timer_lpc.c b/src/lpc/ao_timer_lpc.c new file mode 100644 index 00000000..51835baa --- /dev/null +++ b/src/lpc/ao_timer_lpc.c @@ -0,0 +1,180 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> + +volatile __data AO_TICK_TYPE ao_tick_count; + +uint16_t +ao_time(void) +{ +	return ao_tick_count; +} + +#if AO_DATA_ALL +volatile __data uint8_t	ao_data_interval = 1; +volatile __data uint8_t	ao_data_count; +#endif + +void lpc_systick_isr(void) +{ +	if (lpc_systick.csr & (1 << LPC_SYSTICK_CSR_COUNTFLAG)) { +		++ao_tick_count; +#if HAS_TASK_QUEUE +		if (ao_task_alarm_tick && (int16_t) (ao_tick_count - ao_task_alarm_tick) >= 0) +			ao_task_check_alarm((uint16_t) ao_tick_count); +#endif +#if AO_DATA_ALL +		if (++ao_data_count == ao_data_interval) { +			ao_data_count = 0; +			ao_adc_poll(); +#if (AO_DATA_ALL & ~(AO_DATA_ADC)) +			ao_wakeup((void *) &ao_data_count); +#endif +		} +#endif +	} +} + +#if HAS_ADC +void +ao_timer_set_adc_interval(uint8_t interval) +{ +	ao_arch_critical( +		ao_data_interval = interval; +		ao_data_count = 0; +		); +} +#endif + +#define SYSTICK_RELOAD ((AO_LPC_SYSCLK / 2) / 100 - 1) + +/* Initialize our 100Hz clock */ +void +ao_timer_init(void) +{ +	lpc_systick.rvr = SYSTICK_RELOAD; +	lpc_systick.cvr = 0; +	lpc_systick.csr = ((1 << LPC_SYSTICK_CSR_ENABLE) | +			   (1 << LPC_SYSTICK_CSR_TICKINT) | +			   (LPC_SYSTICK_CSR_CLKSOURCE_CPU_OVER_2 << LPC_SYSTICK_CSR_CLKSOURCE)); +} + +#define AO_LPC_M	((AO_LPC_CLKOUT / AO_LPC_CLKIN) - 1) + +#define AO_LPC_FCCO_MIN	156000000 + +static void +ao_clock_delay(void) +{ +	uint32_t	i; +	for (i = 0; i < 200; i++) +		ao_arch_nop(); +} + +void +ao_clock_init(void) +{ +	uint8_t		p; +	uint32_t	i; + +	/* Turn off all perhipherals except for GPIO configuration */ +	lpc_scb.sysahbclkctrl = ((1 << LPC_SCB_SYSAHBCLKCTRL_SYS) | +				 (1 << LPC_SCB_SYSAHBCLKCTRL_ROM) | +				 (1 << LPC_SCB_SYSAHBCLKCTRL_RAM0) | +				 (1 << LPC_SCB_SYSAHBCLKCTRL_FLASHARRAY) | +				 (1 << LPC_SCB_SYSAHBCLKCTRL_GPIO) | +				 (1 << LPC_SCB_SYSAHBCLKCTRL_IOCON)); +				  +	/* Turn the IRC clock back on */ +	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_IRC_PD); +	ao_clock_delay(); +	 +	/* Switch to the IRC clock */ +	lpc_scb.mainclksel = LPC_SCB_MAINCLKSEL_SEL_IRC << LPC_SCB_MAINCLKSEL_SEL; +	lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA); +	lpc_scb.mainclkuen = (0 << LPC_SCB_MAINCLKUEN_ENA); +	lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA); +	while (!(lpc_scb.mainclkuen & (1 << LPC_SCB_MAINCLKUEN_ENA))) +		; +	 +	/* Find a PLL post divider ratio that gets the FCCO in range */ +	for (p = 0; p < 4; p++) +		if (AO_LPC_CLKOUT << (1 + p) >= AO_LPC_FCCO_MIN) +			break; + +	if (p == 4) +		ao_panic(AO_PANIC_CRASH); + +	/* Power down the PLL before touching the registers */ +	lpc_scb.pdruncfg |= (1 << LPC_SCB_PDRUNCFG_SYSPLL_PD); +	ao_clock_delay(); + +	/* Set PLL divider values */ +	lpc_scb.syspllctrl = ((AO_LPC_M << LPC_SCB_SYSPLLCTRL_MSEL) | +			      (p << LPC_SCB_SYSPLLCTRL_PSEL)); + +	/* Turn off the external crystal clock */ +	lpc_scb.pdruncfg |= (1 << LPC_SCB_PDRUNCFG_SYSOSC_PD); +	ao_clock_delay(); + +	/* Configure the crystal clock */ +	lpc_scb.sysoscctrl = ((0 << LPC_SCB_SYSOSCCTRL_BYPASS) |			   /* using a crystal */ +			      ((AO_LPC_CLKIN > 15000000) << LPC_SCB_SYSOSCCTRL_FREQRANGE));/* set range */ + +	/* Turn on the external crystal clock */ +	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_SYSOSC_PD); +	ao_clock_delay(); + +	/* Select crystal as PLL input */ + +	lpc_scb.syspllclksel = (LPC_SCB_SYSPLLCLKSEL_SEL_SYSOSC << LPC_SCB_SYSPLLCLKSEL_SEL); +	lpc_scb.syspllclkuen = (1 << LPC_SCB_SYSPLLCLKUEN_ENA); +	lpc_scb.syspllclkuen = (0 << LPC_SCB_SYSPLLCLKUEN_ENA); +	lpc_scb.syspllclkuen = (1 << LPC_SCB_SYSPLLCLKUEN_ENA); +	while (!(lpc_scb.syspllclkuen & (1 << LPC_SCB_SYSPLLCLKUEN_ENA))) +		; +	 +	/* Turn on the PLL */ +	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_SYSPLL_PD); + +	/* Wait for it to lock */ +	 +	for (i = 0; i < 20000; i++) +		if (lpc_scb.syspllstat & (1 << LPC_SCB_SYSPLLSTAT_LOCK)) +			break; +	if (i == 20000) +		ao_panic(AO_PANIC_CRASH); + +	/* Switch to the PLL */ +	lpc_scb.mainclksel = LPC_SCB_MAINCLKSEL_SEL_PLL_OUTPUT << LPC_SCB_MAINCLKSEL_SEL; +	lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA); +	lpc_scb.mainclkuen = (0 << LPC_SCB_MAINCLKUEN_ENA); +	lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA); +	while (!(lpc_scb.mainclkuen & (1 << LPC_SCB_MAINCLKUEN_ENA))) +		; + +	/* Set system clock divider */ +	lpc_scb.sysahbclkdiv = AO_LPC_CLKOUT / AO_LPC_SYSCLK; + +	/* Shut down perhipheral clocks (enabled as needed) */ +	lpc_scb.ssp0clkdiv = 0; +	lpc_scb.uartclkdiv = 0; +	lpc_scb.ssp1clkdiv = 0; +	lpc_scb.usbclkdiv = 0; +	lpc_scb.clkoutdiv = 0; +} diff --git a/src/lpc/ao_usb_lpc.c b/src/lpc/ao_usb_lpc.c new file mode 100644 index 00000000..aac0df04 --- /dev/null +++ b/src/lpc/ao_usb_lpc.c @@ -0,0 +1,1025 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_usb.h" +#include "ao_product.h" + +#define USB_DEBUG 	0 +#define USB_DEBUG_DATA	0 +#define USB_ECHO	0 + +#if USB_DEBUG +#define debug(format, args...)	printf(format, ## args); +#else +#define debug(format, args...) +#endif + +#if USB_DEBUG_DATA +#define debug_data(format, args...)	printf(format, ## args); +#else +#define debug_data(format, args...) +#endif + +struct ao_task ao_usb_task; + +struct ao_usb_setup { +	uint8_t		dir_type_recip; +	uint8_t		request; +	uint16_t	value; +	uint16_t	index; +	uint16_t	length; +} ao_usb_setup; + +static uint8_t 	ao_usb_ep0_state; + +/* Pending EP0 IN data */ +static const uint8_t	*ao_usb_ep0_in_data;	/* Remaining data */ +static uint8_t 		ao_usb_ep0_in_len;	/* Remaining amount */ + +/* Temp buffer for smaller EP0 in data */ +static uint8_t	ao_usb_ep0_in_buf[2]; + +/* Pending EP0 OUT data */ +static uint8_t *ao_usb_ep0_out_data; +static uint8_t 	ao_usb_ep0_out_len; + +/* + * Objects allocated in special USB memory + */ + +/* USB address of end of allocated storage */ +static uint8_t	*ao_usb_sram; + +/* Pointer to ep0 tx/rx buffers in USB memory */ +static uint8_t	*ao_usb_ep0_tx_buffer; +static uint8_t	*ao_usb_ep0_setup_buffer; +static uint8_t	*ao_usb_ep0_rx_buffer; + +/* Pointer to bulk data tx/rx buffers in USB memory */ +static uint8_t	*ao_usb_in_tx_buffer; +static uint8_t	*ao_usb_out_rx_buffer; + +/* Our data buffers */ +static uint8_t	ao_usb_tx_buffer[AO_USB_IN_SIZE]; +static uint8_t	ao_usb_tx_count; + +static uint8_t	ao_usb_rx_buffer[AO_USB_OUT_SIZE]; +static uint8_t	ao_usb_rx_count, ao_usb_rx_pos; + +extern struct lpc_usb_endpoint lpc_usb_endpoint; + +/* Marks when we don't need to send an IN packet. + * This happens only when the last IN packet is not full, + * otherwise the host will expect to keep seeing packets. + * Send a zero-length packet as required + */ +static uint8_t	ao_usb_in_flushed; + +/* Marks when we have delivered an IN packet to the hardware + * and it has not been received yet. ao_sleep on this address + * to wait for it to be delivered. + */ +static uint8_t	ao_usb_in_pending; + +/* Marks when an OUT packet has been received by the hardware + * but not pulled to the shadow buffer. + */ +static uint8_t	ao_usb_out_avail; +static uint8_t	ao_usb_running; +static uint8_t	ao_usb_configuration; +static uint8_t	ueienx_0; + +#define AO_USB_EP0_GOT_RESET	1 +#define AO_USB_EP0_GOT_SETUP	2 +#define AO_USB_EP0_GOT_RX_DATA	4 +#define AO_USB_EP0_GOT_TX_ACK	8 + +static uint8_t	ao_usb_ep0_receive; +static uint8_t	ao_usb_address; +static uint8_t	ao_usb_address_pending; + +static inline uint32_t set_toggle(uint32_t 	current_value, +				   uint32_t	mask, +				   uint32_t	desired_value) +{ +	return (current_value ^ desired_value) & mask; +} + +/* + * Set current device address and mark the + * interface as active + */ +void +ao_usb_set_address(uint8_t address) +{ +	debug("ao_usb_set_address %02x\n", address); +	lpc_usb.devcmdstat = ((address << LPC_USB_DEVCMDSTAT_DEV_ADDR) | +			      (1 << LPC_USB_DEVCMDSTAT_DEV_EN) | +			      (0 << LPC_USB_DEVCMDSTAT_SETUP) | +			      (0 << LPC_USB_DEVCMDSTAT_PLL_ON) | +			      (0 << LPC_USB_DEVCMDSTAT_LPM_SUP) | +			      (0 << LPC_USB_DEVCMDSTAT_INTONNAK_AO) | +			      (0 << LPC_USB_DEVCMDSTAT_INTONNAK_AI) | +			      (0 << LPC_USB_DEVCMDSTAT_INTONNAK_CO) | +			      (0 << LPC_USB_DEVCMDSTAT_INTONNAK_CI) | +			      (1 << LPC_USB_DEVCMDSTAT_DCON) | +			      (0 << LPC_USB_DEVCMDSTAT_DSUS) | +			      (0 << LPC_USB_DEVCMDSTAT_DCON_C) | +			      (0 << LPC_USB_DEVCMDSTAT_DSUS_C) | +			      (0 << LPC_USB_DEVCMDSTAT_DRES_C) | +			      (0 << LPC_USB_DEVCMDSTAT_VBUSDEBOUNCED)); +	ao_usb_address_pending = 0; +} + +#define TX_DBG 0 +#define RX_DBG 0 + +#if TX_DBG +#define _tx_dbg0(msg) _dbg(__LINE__,msg,0) +#define _tx_dbg1(msg,value) _dbg(__LINE__,msg,value) +#else +#define _tx_dbg0(msg) +#define _tx_dbg1(msg,value) +#endif + +#if RX_DBG +#define _rx_dbg0(msg) _dbg(__LINE__,msg,0) +#define _rx_dbg1(msg,value) _dbg(__LINE__,msg,value) +#else +#define _rx_dbg0(msg) +#define _rx_dbg1(msg,value) +#endif + +#if TX_DBG || RX_DBG +static void _dbg(int line, char *msg, uint32_t value); +#endif + +/* + * Set just endpoint 0, for use during startup + */ + +static uint8_t * +ao_usb_alloc_sram(uint16_t size) +{ +	uint8_t	*addr = ao_usb_sram; + +	ao_usb_sram += (size + 63) & ~63; +	return addr; +} + +static uint16_t +ao_usb_sram_offset(uint8_t *addr) +{ +	return (uint16_t) ((intptr_t) addr >> 6); +} + +static void +ao_usb_set_ep(vuint32_t *ep, uint8_t *addr, uint16_t nbytes) +{ +	*ep = ((ao_usb_sram_offset(addr) << LPC_USB_EP_OFFSET) | +	       (nbytes << LPC_USB_EP_NBYTES) | +	       (0 << LPC_USB_EP_ENDPOINT_ISO) | +	       (0 << LPC_USB_EP_RATE_FEEDBACK) | +	       (0 << LPC_USB_EP_TOGGLE_RESET) | +	       (0 << LPC_USB_EP_STALL) | +	       (0 << LPC_USB_EP_DISABLED) | +	       (1 << LPC_USB_EP_ACTIVE)); +} + +static inline uint16_t +ao_usb_ep_count(vuint32_t *ep) +{ +	return (*ep >> LPC_USB_EP_NBYTES) & LPC_USB_EP_NBYTES_MASK; +} + +static inline uint8_t +ao_usb_ep_stall(vuint32_t *ep) +{ +	return (*ep >> LPC_USB_EP_STALL) & 1; +} + +static inline vuint32_t * +ao_usb_ep0_out(void) +{ +	return &lpc_usb_endpoint.ep0_out; +} + +static inline vuint32_t * +ao_usb_ep0_in(void) +{ +	return &lpc_usb_endpoint.ep0_in; +} + +static inline vuint32_t * +ao_usb_epn_out(uint8_t n) +{ +	return &lpc_usb_endpoint.epn[n-1].out[0]; +} + +static inline vuint32_t * +ao_usb_epn_in(uint8_t n) +{ +	return &lpc_usb_endpoint.epn[n-1].in[0]; +} + +static void +ao_usb_set_epn_in(uint8_t n, uint8_t *addr, uint16_t nbytes) +{ +	ao_usb_set_ep(ao_usb_epn_in(n), addr, nbytes); +} + +static void +ao_usb_set_epn_out(uint8_t n, uint8_t *addr, uint16_t nbytes) +{ +	ao_usb_set_ep(ao_usb_epn_out(n), addr, nbytes); +} + +static inline uint16_t +ao_usb_epn_out_count(uint8_t n) +{ +	return ao_usb_ep_count(ao_usb_epn_out(n)); +} + +static inline uint16_t +ao_usb_epn_in_count(uint8_t n) +{ +	return ao_usb_ep_count(ao_usb_epn_in(n)); +} + +static uint8_t * +ao_usb_enable_ep(vuint32_t *ep, uint16_t nbytes, uint16_t set_nbytes) +{ +	uint8_t	*addr = ao_usb_alloc_sram(nbytes); +	 +	ao_usb_set_ep(ep, addr, set_nbytes); +	return addr; +} + +static void +ao_usb_disable_ep(vuint32_t *ep) +{ +	*ep = ((0 << LPC_USB_EP_OFFSET) | +	       (0 << LPC_USB_EP_NBYTES) | +	       (0 << LPC_USB_EP_ENDPOINT_ISO) | +	       (0 << LPC_USB_EP_RATE_FEEDBACK) | +	       (0 << LPC_USB_EP_TOGGLE_RESET) | +	       (0 << LPC_USB_EP_STALL) | +	       (1 << LPC_USB_EP_DISABLED) | +	       (0 << LPC_USB_EP_ACTIVE)); +} + +static void +ao_usb_enable_epn(uint8_t n, uint16_t out_bytes, uint8_t **out_addr, uint16_t in_bytes, uint8_t **in_addr) +{ +	uint8_t	*addr; + +	addr = ao_usb_enable_ep(ao_usb_epn_out(n), out_bytes, out_bytes); +	if (out_addr) +		*out_addr = addr; +	ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].out[1]); + +	addr = ao_usb_enable_ep(ao_usb_epn_in(n), in_bytes, 0); +	if (in_addr) +		*in_addr = addr; +	ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].in[1]); +} + +static void +ao_usb_disable_epn(uint8_t n) +{ +	ao_usb_disable_ep(ao_usb_epn_out(n)); +	ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].out[1]); +	ao_usb_disable_ep(ao_usb_epn_in(n)); +	ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].in[1]); +} + +static void +ao_usb_set_ep0(void) +{ +	int			e; + +	/* Everything is single buffered for now */ +	lpc_usb.epbufcfg = 0; +	lpc_usb.epinuse = 0; +	lpc_usb.epskip = 0xffffffff; + +	ao_usb_set_address(0); +	lpc_usb.intstat = 0xc00003ff; + +	ao_usb_configuration = 0; + +	ao_usb_sram = lpc_usb_sram; + +	lpc_usb.epliststart = (uint32_t) (intptr_t) &lpc_usb_endpoint; +	lpc_usb.databufstart = ((uint32_t) (intptr_t) ao_usb_sram) & 0xffc00000; + +	/* Set up EP 0 - a Control end point with 32 bytes of in and out buffers */ + +	ao_usb_ep0_rx_buffer = ao_usb_enable_ep(ao_usb_ep0_out(), AO_USB_CONTROL_SIZE, AO_USB_CONTROL_SIZE); +	ao_usb_ep0_setup_buffer = ao_usb_alloc_sram(AO_USB_CONTROL_SIZE); +	lpc_usb_endpoint.setup = ao_usb_sram_offset(ao_usb_ep0_setup_buffer); +	ao_usb_ep0_tx_buffer = ao_usb_enable_ep(ao_usb_ep0_in(), AO_USB_CONTROL_SIZE, 0); + +	/* Clear all of the other endpoints */ +	for (e = 1; e <= 4; e++) +		ao_usb_disable_epn(e); + +} + +static void +ao_usb_set_configuration(void) +{ +	debug ("ao_usb_set_configuration\n"); + +	/* Set up the INT end point */ +	ao_usb_enable_epn(AO_USB_INT_EP, 0, NULL, 0, NULL); +	 +	/* Set up the OUT end point */ +	ao_usb_enable_epn(AO_USB_OUT_EP, AO_USB_OUT_SIZE, &ao_usb_out_rx_buffer, 0, NULL); + +	/* Set up the IN end point */ +	ao_usb_enable_epn(AO_USB_IN_EP, 0, NULL, AO_USB_IN_SIZE, &ao_usb_in_tx_buffer); + +	ao_usb_running = 1; +} + +/* Send an IN data packet */ +static void +ao_usb_ep0_flush(void) +{ +	uint8_t this_len; + +	this_len = ao_usb_ep0_in_len; +	if (this_len > AO_USB_CONTROL_SIZE) +		this_len = AO_USB_CONTROL_SIZE; + +	if (this_len < AO_USB_CONTROL_SIZE) +		ao_usb_ep0_state = AO_USB_EP0_IDLE; + +	ao_usb_ep0_in_len -= this_len; + +	debug_data ("Flush EP0 len %d:", this_len); +	memcpy(ao_usb_ep0_tx_buffer, ao_usb_ep0_in_data, this_len); +	debug_data ("\n"); +	ao_usb_ep0_in_data += this_len; + +	/* Mark the endpoint as TX valid to send the packet */ +	ao_usb_set_ep(ao_usb_ep0_in(), ao_usb_ep0_tx_buffer, this_len); +	debug ("queue tx.  0 now %08x\n", *ao_usb_ep0_in()); +} + +/* Read data from the ep0 OUT fifo */ +static void +ao_usb_ep0_fill(void) +{ +	uint16_t	len; +	uint8_t		*rx_buffer; + +	/* Pull all of the data out of the packet */ +	if (lpc_usb.devcmdstat & (1 << LPC_USB_DEVCMDSTAT_SETUP)) { +		rx_buffer = ao_usb_ep0_setup_buffer; +		len = 8; +	} else { +		rx_buffer = ao_usb_ep0_rx_buffer; +		len = AO_USB_CONTROL_SIZE - ao_usb_ep_count(ao_usb_ep0_out()); +	} + +	if (len > ao_usb_ep0_out_len) +		len = ao_usb_ep0_out_len; +	ao_usb_ep0_out_len -= len; + +	debug_data ("Fill EP0 len %d:", len); +	memcpy(ao_usb_ep0_out_data, rx_buffer, len); +	debug_data ("\n"); +	ao_usb_ep0_out_data += len; + +	/* ACK the packet */ +	ao_usb_set_ep(ao_usb_ep0_out(), ao_usb_ep0_rx_buffer, AO_USB_CONTROL_SIZE); +	lpc_usb.devcmdstat |= (1 << LPC_USB_DEVCMDSTAT_SETUP); +} + +static void +ao_usb_ep0_in_reset(void) +{ +	ao_usb_ep0_in_data = ao_usb_ep0_in_buf; +	ao_usb_ep0_in_len = 0; +} + +static void +ao_usb_ep0_in_queue_byte(uint8_t a) +{ +	if (ao_usb_ep0_in_len < sizeof (ao_usb_ep0_in_buf)) +		ao_usb_ep0_in_buf[ao_usb_ep0_in_len++] = a; +} + +static void +ao_usb_ep0_in_set(const uint8_t *data, uint8_t len) +{ +	ao_usb_ep0_in_data = data; +	ao_usb_ep0_in_len = len; +} + +static void +ao_usb_ep0_out_set(uint8_t *data, uint8_t len) +{ +	ao_usb_ep0_out_data = data; +	ao_usb_ep0_out_len = len; +} + +static void +ao_usb_ep0_in_start(uint8_t max) +{ +	/* Don't send more than asked for */ +	if (ao_usb_ep0_in_len > max) +		ao_usb_ep0_in_len = max; +	ao_usb_ep0_flush(); +} + +static struct ao_usb_line_coding ao_usb_line_coding = {115200, 0, 0, 8}; + +/* Walk through the list of descriptors and find a match + */ +static void +ao_usb_get_descriptor(uint16_t value) +{ +	const uint8_t		*descriptor; +	uint8_t		type = value >> 8; +	uint8_t		index = value; + +	descriptor = ao_usb_descriptors; +	while (descriptor[0] != 0) { +		if (descriptor[1] == type && index-- == 0) { +			uint8_t	len; +			if (type == AO_USB_DESC_CONFIGURATION) +				len = descriptor[2]; +			else +				len = descriptor[0]; +			ao_usb_ep0_in_set(descriptor, len); +			break; +		} +		descriptor += descriptor[0]; +	} +} + +static void +ao_usb_ep0_setup(void) +{ +	/* Pull the setup packet out of the fifo */ +	ao_usb_ep0_out_set((uint8_t *) &ao_usb_setup, 8); +	ao_usb_ep0_fill(); +	if (ao_usb_ep0_out_len != 0) { +		debug ("invalid setup packet length\n"); +		return; +	} + +	if ((ao_usb_setup.dir_type_recip & AO_USB_DIR_IN) || ao_usb_setup.length == 0) +		ao_usb_ep0_state = AO_USB_EP0_DATA_IN; +	else +		ao_usb_ep0_state = AO_USB_EP0_DATA_OUT; + +	ao_usb_ep0_in_reset(); + +	switch(ao_usb_setup.dir_type_recip & AO_USB_SETUP_TYPE_MASK) { +	case AO_USB_TYPE_STANDARD: +		debug ("Standard setup packet\n"); +		switch(ao_usb_setup.dir_type_recip & AO_USB_SETUP_RECIP_MASK) { +		case AO_USB_RECIP_DEVICE: +			debug ("Device setup packet\n"); +			switch(ao_usb_setup.request) { +			case AO_USB_REQ_GET_STATUS: +				debug ("get status\n"); +				ao_usb_ep0_in_queue_byte(0); +				ao_usb_ep0_in_queue_byte(0); +				break; +			case AO_USB_REQ_SET_ADDRESS: +				debug ("set address %d\n", ao_usb_setup.value); +				ao_usb_address = ao_usb_setup.value; +				ao_usb_address_pending = 1; +				break; +			case AO_USB_REQ_GET_DESCRIPTOR: +				debug ("get descriptor %d\n", ao_usb_setup.value); +				ao_usb_get_descriptor(ao_usb_setup.value); +				break; +			case AO_USB_REQ_GET_CONFIGURATION: +				debug ("get configuration %d\n", ao_usb_configuration); +				ao_usb_ep0_in_queue_byte(ao_usb_configuration); +				break; +			case AO_USB_REQ_SET_CONFIGURATION: +				ao_usb_configuration = ao_usb_setup.value; +				debug ("set configuration %d\n", ao_usb_configuration); +				ao_usb_set_configuration(); +				break; +			} +			break; +		case AO_USB_RECIP_INTERFACE: +			debug ("Interface setup packet\n"); +			switch(ao_usb_setup.request) { +			case AO_USB_REQ_GET_STATUS: +				ao_usb_ep0_in_queue_byte(0); +				ao_usb_ep0_in_queue_byte(0); +				break; +			case AO_USB_REQ_GET_INTERFACE: +				ao_usb_ep0_in_queue_byte(0); +				break; +			case AO_USB_REQ_SET_INTERFACE: +				break; +			} +			break; +		case AO_USB_RECIP_ENDPOINT: +			debug ("Endpoint setup packet\n"); +			switch(ao_usb_setup.request) { +			case AO_USB_REQ_GET_STATUS: +				ao_usb_ep0_in_queue_byte(0); +				ao_usb_ep0_in_queue_byte(0); +				break; +			} +			break; +		} +		break; +	case AO_USB_TYPE_CLASS: +		debug ("Class setup packet\n"); +		switch (ao_usb_setup.request) { +		case AO_USB_SET_LINE_CODING: +			debug ("set line coding\n"); +			ao_usb_ep0_out_set((uint8_t *) &ao_usb_line_coding, 7); +			break; +		case AO_USB_GET_LINE_CODING: +			debug ("get line coding\n"); +			ao_usb_ep0_in_set((const uint8_t *) &ao_usb_line_coding, 7); +			break; +		case AO_USB_SET_CONTROL_LINE_STATE: +			break; +		} +		break; +	} + +	/* If we're not waiting to receive data from the host, +	 * queue an IN response +	 */ +	if (ao_usb_ep0_state == AO_USB_EP0_DATA_IN) +		ao_usb_ep0_in_start(ao_usb_setup.length); +} + +static void +ao_usb_ep0_handle(uint8_t receive) +{ +	ao_usb_ep0_receive = 0; + +	if (receive & AO_USB_EP0_GOT_RESET) { +		debug ("\treset\n"); +		ao_usb_set_ep0(); +		return; +	} +	if (receive & AO_USB_EP0_GOT_SETUP) { +		debug ("\tsetup\n"); +		ao_usb_ep0_setup(); +	} +	if (receive & AO_USB_EP0_GOT_RX_DATA) { +		debug ("\tgot rx data\n"); +		if (ao_usb_ep0_state == AO_USB_EP0_DATA_OUT) { +			ao_usb_ep0_fill(); +			if (ao_usb_ep0_out_len == 0) { +				ao_usb_ep0_state = AO_USB_EP0_DATA_IN; +				ao_usb_ep0_in_start(0); +			} +		} +	} +	if (receive & AO_USB_EP0_GOT_TX_ACK) { +		debug ("\tgot tx ack\n"); + +		/* Wait until the IN packet is received from addr 0 +		 * before assigning our local address +		 */ +		if (ao_usb_address_pending) { +#if HAS_FLIGHT +			/* Go to idle mode if USB is connected +			 */ +			ao_flight_force_idle = 1; +#endif +			ao_usb_set_address(ao_usb_address); +		} +		if (ao_usb_ep0_state == AO_USB_EP0_DATA_IN) +			ao_usb_ep0_flush(); +	} +} + +static uint16_t	control_count; +static uint16_t int_count; +static uint16_t	in_count; +static uint16_t	out_count; +static uint16_t	reset_count; + +void +lpc_usb_irq_isr(void) +{ +	uint32_t	intstat = lpc_usb.intstat & lpc_usb.inten; + +	lpc_usb.intstat = intstat; +	/* Handle EP0 OUT packets */ +	if (intstat & (1 << LPC_USB_INT_EPOUT(0))) { +		if (lpc_usb.devcmdstat & (1 << LPC_USB_DEVCMDSTAT_SETUP)) +			ao_usb_ep0_receive |= AO_USB_EP0_GOT_SETUP; +		else +			ao_usb_ep0_receive |= AO_USB_EP0_GOT_RX_DATA; + +		ao_usb_ep0_handle(ao_usb_ep0_receive); +	} + +	/* Handle EP0 IN packets */ +	if (intstat & (1 << LPC_USB_INT_EPIN(0))) { +		ao_usb_ep0_receive |= AO_USB_EP0_GOT_TX_ACK; + +		ao_usb_ep0_handle(ao_usb_ep0_receive); +	} + + +	/* Handle OUT packets */ +	if (intstat & (1 << LPC_USB_INT_EPOUT(AO_USB_OUT_EP))) { +		++out_count; +		_rx_dbg1("RX ISR", *ao_usb_epn_out(AO_USB_OUT_EP)); +		ao_usb_out_avail = 1; +		_rx_dbg0("out avail set"); +		ao_wakeup(&ao_stdin_ready); +		_rx_dbg0("stdin awoken"); +	} + +	/* Handle IN packets */ +	if (intstat & (1 << LPC_USB_INT_EPIN(AO_USB_IN_EP))) { +		++in_count; +		_tx_dbg1("TX ISR", *ao_usb_epn_in(AO_USB_IN_EP)); +		ao_usb_in_pending = 0; +		ao_wakeup(&ao_usb_in_pending); +	} + +	/* NAK all INT EP IN packets */ +	if (intstat & (1 << LPC_USB_INT_EPIN(AO_USB_INT_EP))) { +		; +	} + +	/* Check for reset */ +	if (intstat & (1 << LPC_USB_INT_DEV)) { +		if (lpc_usb.devcmdstat & (1 << LPC_USB_DEVCMDSTAT_DRES_C)) +		{ +			lpc_usb.devcmdstat |= (1 << LPC_USB_DEVCMDSTAT_DRES_C); +			ao_usb_ep0_receive |= AO_USB_EP0_GOT_RESET; +			ao_usb_ep0_handle(ao_usb_ep0_receive); +		} +	} +} + + + +/* Queue the current IN buffer for transmission */ +static void +_ao_usb_in_send(void) +{ +	_tx_dbg0("in_send start"); +	debug ("send %d\n", ao_usb_tx_count); +	while (ao_usb_in_pending) +		ao_sleep(&ao_usb_in_pending); +	ao_usb_in_pending = 1; +	if (ao_usb_tx_count != AO_USB_IN_SIZE) +		ao_usb_in_flushed = 1; +	memcpy(ao_usb_in_tx_buffer, ao_usb_tx_buffer, ao_usb_tx_count); +	ao_usb_set_ep(ao_usb_epn_in(AO_USB_IN_EP), ao_usb_in_tx_buffer, ao_usb_tx_count); +	ao_usb_tx_count = 0; +	_tx_dbg0("in_send end"); +} + +/* Wait for a free IN buffer. Interrupts are blocked */ +static void +_ao_usb_in_wait(void) +{ +	for (;;) { +		/* Check if the current buffer is writable */ +		if (ao_usb_tx_count < AO_USB_IN_SIZE) +			break; + +		_tx_dbg0("in_wait top"); +		/* Wait for an IN buffer to be ready */ +		while (ao_usb_in_pending) +			ao_sleep(&ao_usb_in_pending); +		_tx_dbg0("in_wait bottom"); +	} +} + +void +ao_usb_flush(void) +{ +	if (!ao_usb_running) +		return; + +	/* Anytime we've sent a character since +	 * the last time we flushed, we'll need +	 * to send a packet -- the only other time +	 * we would send a packet is when that +	 * packet was full, in which case we now +	 * want to send an empty packet +	 */ +	ao_arch_block_interrupts(); +	while (!ao_usb_in_flushed) { +		_tx_dbg0("flush top"); +		_ao_usb_in_send(); +		_tx_dbg0("flush end"); +	} +	ao_arch_release_interrupts(); +} + +void +ao_usb_putchar(char c) +{ +	if (!ao_usb_running) +		return; + +	ao_arch_block_interrupts(); +	_ao_usb_in_wait(); + +	ao_usb_in_flushed = 0; +	ao_usb_tx_buffer[ao_usb_tx_count++] = (uint8_t) c; + +	/* Send the packet when full */ +	if (ao_usb_tx_count == AO_USB_IN_SIZE) { +		_tx_dbg0("putchar full"); +		_ao_usb_in_send(); +		_tx_dbg0("putchar flushed"); +	} +	ao_arch_release_interrupts(); +} + +static void +_ao_usb_out_recv(void) +{ +	_rx_dbg0("out_recv top"); +	ao_usb_out_avail = 0; + +	ao_usb_rx_count = AO_USB_OUT_SIZE - ao_usb_epn_out_count(AO_USB_OUT_EP); + +	_rx_dbg1("out_recv count", ao_usb_rx_count); +	debug ("recv %d\n", ao_usb_rx_count); +	debug_data("Fill OUT len %d:", ao_usb_rx_count); +	memcpy(ao_usb_rx_buffer, ao_usb_out_rx_buffer, ao_usb_rx_count); +	debug_data("\n"); +	ao_usb_rx_pos = 0; + +	/* ACK the packet */ +	ao_usb_set_epn_out(AO_USB_OUT_EP, ao_usb_out_rx_buffer, AO_USB_OUT_SIZE); +} + +int +_ao_usb_pollchar(void) +{ +	uint8_t c; + +	if (!ao_usb_running) +		return AO_READ_AGAIN; + +	for (;;) { +		if (ao_usb_rx_pos != ao_usb_rx_count) +			break; + +		_rx_dbg0("poll check"); +		/* Check to see if a packet has arrived */ +		if (!ao_usb_out_avail) { +			_rx_dbg0("poll none"); +			return AO_READ_AGAIN; +		} +		_ao_usb_out_recv(); +	} + +	/* Pull a character out of the fifo */ +	c = ao_usb_rx_buffer[ao_usb_rx_pos++]; +	return c; +} + +char +ao_usb_getchar(void) +{ +	int	c; + +	ao_arch_block_interrupts(); +	while ((c = _ao_usb_pollchar()) == AO_READ_AGAIN) +		ao_sleep(&ao_stdin_ready); +	ao_arch_release_interrupts(); +	return c; +} + +void +ao_usb_disable(void) +{ +	ao_arch_block_interrupts(); + +	/* Disable interrupts */ +	lpc_usb.inten = 0; + +	lpc_nvic_clear_enable(LPC_ISR_USB_IRQ_POS); + +	/* Disable the device */ +	lpc_usb.devcmdstat = 0; + +	/* Turn off USB clock */ +	lpc_scb.usbclkdiv = 0; + +	/* Disable USB PHY */ +	lpc_scb.pdruncfg |= (1 << LPC_SCB_PDRUNCFG_USBPAD_PD); + +	/* Disable USB registers and RAM */ +	lpc_scb.sysahbclkctrl &= ~((1 << LPC_SCB_SYSAHBCLKCTRL_USB) | +				   (1 << LPC_SCB_SYSAHBCLKCTRL_USBRAM)); + +	ao_arch_release_interrupts(); +} + +void +ao_usb_enable(void) +{ +	int	t; + +	/* Enable USB pins */ +#if HAS_USB_CONNECT +	lpc_ioconf.pio0_6 = ((LPC_IOCONF_FUNC_USB_CONNECT << LPC_IOCONF_FUNC) | +			     (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | +			     (0 << LPC_IOCONF_HYS) | +			     (0 << LPC_IOCONF_INV) | +			     (0 << LPC_IOCONF_OD) | +			     0x80); +#endif +#if HAS_USB_VBUS +	lpc_ioconf.pio0_3 = ((LPC_IOCONF_FUNC_USB_VBUS << LPC_IOCONF_FUNC) | +			     (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | +			     (0 << LPC_IOCONF_HYS) | +			     (0 << LPC_IOCONF_INV) | +			     (0 << LPC_IOCONF_OD) | +			     0x80); +#endif +	/* Enable USB registers and RAM */ +	lpc_scb.sysahbclkctrl |= ((1 << LPC_SCB_SYSAHBCLKCTRL_USB) | +				  (1 << LPC_SCB_SYSAHBCLKCTRL_USBRAM)); + +	/* Enable USB PHY */ +	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_USBPAD_PD); +	 +	/* Turn on USB PLL */ +	lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_USBPLL_PD); + +	lpc_scb.usbpllclksel = (LPC_SCB_SYSPLLCLKSEL_SEL_SYSOSC << LPC_SCB_SYSPLLCLKSEL_SEL); +	lpc_scb.usbpllclkuen = (1 << LPC_SCB_USBPLLCLKUEN_ENA); +	lpc_scb.usbpllclkuen = (0 << LPC_SCB_USBPLLCLKUEN_ENA); +	lpc_scb.usbpllclkuen = (1 << LPC_SCB_USBPLLCLKUEN_ENA); +	while (!(lpc_scb.usbpllclkuen & (1 << LPC_SCB_USBPLLCLKUEN_ENA))) +		; +	lpc_scb.usbpllctrl = 0x23; +	while (!(lpc_scb.usbpllstat & 1)) +		; + +	lpc_scb.usbclksel = 0; + +	/* Turn on USB clock, use 48MHz clock unchanged */ +	lpc_scb.usbclkdiv = 1; + +	/* Configure interrupts */ +	ao_arch_block_interrupts(); + +	/* Route all interrupts to the main isr */ +	lpc_usb.introuting = 0; + +	/* Configure NVIC */ + +	lpc_nvic_set_enable(LPC_ISR_USB_IRQ_POS); +	lpc_nvic_set_priority(LPC_ISR_USB_IRQ_POS, 0); + +	/* Clear any spurious interrupts */ +	lpc_usb.intstat = 0xffffffff; + +	debug ("ao_usb_enable\n"); + +	/* Enable interrupts */ +	lpc_usb.inten = ((1 << LPC_USB_INT_EPOUT(0)) | +			 (1 << LPC_USB_INT_EPIN(0)) | +			 (1 << LPC_USB_INT_EPIN(AO_USB_INT_EP)) | +			 (1 << LPC_USB_INT_EPOUT(AO_USB_OUT_EP)) | +			 (1 << LPC_USB_INT_EPIN(AO_USB_IN_EP)) | +			 (1 << LPC_USB_INT_DEV)); + +	ao_arch_release_interrupts(); + +	lpc_usb.devcmdstat = 0; +	for (t = 0; t < 1000; t++) +		ao_arch_nop(); + +	ao_usb_set_ep0(); +} + +#if USB_ECHO +struct ao_task ao_usb_echo_task; + +static void +ao_usb_echo(void) +{ +	char	c; + +	for (;;) { +		c = ao_usb_getchar(); +		ao_usb_putchar(c); +		ao_usb_flush(); +	} +} +#endif + +#if USB_DEBUG +static void +ao_usb_irq(void) +{ +	printf ("control: %d out: %d in: %d int: %d reset: %d\n", +		control_count, out_count, in_count, int_count, reset_count); +} + +__code struct ao_cmds ao_usb_cmds[] = { +	{ ao_usb_irq, "I\0Show USB interrupt counts" }, +	{ 0, NULL } +}; +#endif + +void +ao_usb_init(void) +{ +	ao_usb_enable(); + +	debug ("ao_usb_init\n"); +#if USB_ECHO +	ao_add_task(&ao_usb_echo_task, ao_usb_echo, "usb echo"); +#endif +#if USB_DEBUG +	ao_cmd_register(&ao_usb_cmds[0]); +#endif +#if !USB_ECHO +	ao_add_stdio(_ao_usb_pollchar, ao_usb_putchar, ao_usb_flush); +#endif +} + +#if TX_DBG || RX_DBG + +struct ao_usb_dbg { +	int		line; +	char		*msg; +	uint32_t	value; +	uint32_t	primask; +#if TX_DBG +	uint16_t	in_count; +	uint32_t	in_ep; +	uint32_t	in_pending; +	uint32_t	tx_count; +	uint32_t	in_flushed; +#endif +#if RX_DBG +	uint8_t		rx_count; +	uint8_t		rx_pos; +	uint8_t		out_avail; +	uint32_t	out_ep; +#endif +}; + +#define NUM_USB_DBG	8 + +static struct ao_usb_dbg dbg[NUM_USB_DBG]; +static int dbg_i; + +static void _dbg(int line, char *msg, uint32_t value) +{ +	uint32_t	primask; +	dbg[dbg_i].line = line; +	dbg[dbg_i].msg = msg; +	dbg[dbg_i].value = value; +	asm("mrs %0,primask" : "=&r" (primask)); +	dbg[dbg_i].primask = primask; +#if TX_DBG +	dbg[dbg_i].in_count = in_count; +	dbg[dbg_i].in_ep = *ao_usb_epn_in(AO_USB_IN_EP); +	dbg[dbg_i].in_pending = ao_usb_in_pending; +	dbg[dbg_i].tx_count = ao_usb_tx_count; +	dbg[dbg_i].in_flushed = ao_usb_in_flushed; +#endif +#if RX_DBG +	dbg[dbg_i].rx_count = ao_usb_rx_count; +	dbg[dbg_i].rx_pos = ao_usb_rx_pos; +	dbg[dbg_i].out_avail = ao_usb_out_avail; +	dbg[dbg_i].out_ep = *ao_usb_epn_out(AO_USB_OUT_EP); +#endif +	if (++dbg_i == NUM_USB_DBG) +		dbg_i = 0; +} +#endif diff --git a/src/lpc/baud_rate b/src/lpc/baud_rate new file mode 100644 index 00000000..2bfbf309 --- /dev/null +++ b/src/lpc/baud_rate @@ -0,0 +1,131 @@ +#!/usr/bin/env nickle + +/* + * Given a main clock frequency, + * compute USART clock freq and a table + * of USART config parameters for our target baud rates + */ + +real main_clock = 0; +real usart_clock = 0; + +real[] baud_rates = { 4800, 9600, 19200, 57600, 115200 }; + +void +compute_baud_rate(real rate) { +	int	divaddval; +	int	mulval; + +	real 	dl_est = usart_clock / (16 * rate); + +	if (dl_est == floor(dl_est)) { +		divaddval = 0; +		mulval = 1; +	} else { +		if (false) { + +			/* This is how the docs suggest doing it; this +			 * generates a rate which is reasonably close +			 */ + +			real fr_est = 1.5; + +			/* Compute fractional estimate */ +			do { +				dl_est = floor(usart_clock / (16 * rate * fr_est) + 0.5); +				fr_est = usart_clock / (16 * rate * dl_est); +			} while (fr_est <= 1.1 || 1.9 <= fr_est); + +			/* Given fractional estimate, compute divaddval/mulvals that work best */ + +			real best_dist = 1000; +			for (int tmp_divaddval = 1; tmp_divaddval < 15; tmp_divaddval++) { +				for (int tmp_mulval = 1; tmp_mulval < 16; tmp_mulval++) { +					real fr = 1 + tmp_divaddval / tmp_mulval; +					real dist = abs(fr - fr_est); +					if (dist < best_dist) { +						divaddval = tmp_divaddval; +						mulval = tmp_mulval; +						best_dist = dist; +					} +				} +			} +		} else { + +			/* This exhaustively searches for the best match */ + +			real my_best_dist = 1e20; +			int my_best_dl; +			int my_best_divaddval; +			int my_best_mulval; +			for (int my_dl = 1; my_dl < 1024; my_dl++) { +				for (int my_mulval = 1; my_mulval < 16; my_mulval++) { +					for (int my_divaddval = 0; my_divaddval < my_mulval; my_divaddval++) { +						real my_rate = usart_clock / ((16 * my_dl) * (1 + my_divaddval/my_mulval)); + +						real my_dist = abs(rate - my_rate); + +						if (my_dist == 0 && my_divaddval == 0) { +							my_dist = -1; +						} + +						if (my_dist < my_best_dist) { +							my_best_dl = my_dl; +							my_best_divaddval = my_divaddval; +							my_best_mulval = my_mulval; +							my_best_dist = my_dist; +						} +					} +				} +			} + +			dl_est = my_best_dl; +			divaddval = my_best_divaddval; +			mulval = my_best_mulval; +		} +	} + +	int dl = floor (dl_est);	 + +	real actual = usart_clock / ((16 * dl) * (1 + divaddval/mulval)); + +	printf("\t[AO_SERIAL_SPEED_%d] = { /* actual = %8.2f */\n", floor(rate), actual); +	printf("\t\t.dl = %d,\n", dl); +	printf("\t\t.divaddval = %d,\n", divaddval); +	printf("\t\t.mulval = %d\n", mulval); +	printf("\t},\n"); +} + +void +main() { +	if (dim(argv) < 2) { +		printf ("usage: %s <main-clock>\n", argv[0]); +		exit(1); +	} +	main_clock = string_to_real(argv[1]); + +	for (int div = 0; div < 4; div++) { +		if (main_clock / (1 << div) <= 12000000) { +			usart_clock = main_clock / (1 << div); +			break; +		} +	} + +	if (usart_clock == 0) { +		printf ("can't get usart clock in range\n"); +		exit(1); +	} + +	printf ("#define AO_LPC_USARTCLK %d\n\n", floor(usart_clock)); +	printf("static const struct {\n"); +	printf("\tuint16_t dl;\n"); +	printf("\tuint8_t divaddval;\n"); +	printf("\tuint8_t mulval;\n"); +	printf("} ao_usart_speeds[] = {\n"); +	for (int i = 0; i < dim(baud_rates); i++) { +		compute_baud_rate(baud_rates[i]); +	} +	printf ("};\n"); +} + +main(); diff --git a/src/lpc/figure-checksum b/src/lpc/figure-checksum new file mode 100755 index 00000000..0b1de578 --- /dev/null +++ b/src/lpc/figure-checksum @@ -0,0 +1,39 @@ +#!/usr/bin/env nickle + +autoimport Process; + +int byteflip(int x) { +	return ((x >> 24) & 0xff) | ((x >> 8) & 0xff00) | ((x << 8) & 0xff0000) | ((x << 24) & 0xff000000); +} + +void main () { +	file	input = popen(popen_direction.read, true, "objdump", +			      "objdump", "-j", ".text", +			      "--start-address=0", +			      "--stop-address=0x20", +			      "-s", argv[1]); +	int sum = 0; + +	void add_in(int addr, int value) { +		if (addr < 0x1c) { +			sum += value; +		} else if (addr == 0x1c) { +			printf ("-DCKSUM=0x%08x\n", -sum & 0xffffffff); +			exit(0); +		} +	} +	while (!File::end(input)) { +		string line = File::fgets(input); +		string[] words = String::wordsplit(line, " "); + +		if (dim(words) < 5) +		    continue; +		if (words[0] == "0000" || words[0] == "0010") { +			int addr = string_to_integer(words[0], 16); +			for (int i = 0; i < 4; i++) +				add_in(addr + i * 4, byteflip(string_to_integer(words[i+1], 16))); +		} +	} +} + +main(); diff --git a/src/lpc/lpc.h b/src/lpc/lpc.h new file mode 100644 index 00000000..49034c1c --- /dev/null +++ b/src/lpc/lpc.h @@ -0,0 +1,1225 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _LPC_H_ +#define _LPC_H_ + +#include <stdint.h> + +typedef volatile uint32_t	vuint32_t; +typedef volatile uint16_t	vuint16_t; +typedef volatile uint8_t	vuint8_t; +typedef volatile void *		vvoid_t; + +struct lpc_ioconf { +	vuint32_t	pio0_0; +	vuint32_t	pio0_1; +	vuint32_t	pio0_2; +	vuint32_t	pio0_3; + +	vuint32_t	pio0_4; +	vuint32_t	pio0_5; +	vuint32_t	pio0_6; +	vuint32_t	pio0_7; + +	vuint32_t	pio0_8; +	vuint32_t	pio0_9; +	vuint32_t	pio0_10; +	vuint32_t	pio0_11; + +	vuint32_t	pio0_12; +	vuint32_t	pio0_13; +	vuint32_t	pio0_14; +	vuint32_t	pio0_15; + +	vuint32_t	pio0_16; +	vuint32_t	pio0_17; +	vuint32_t	pio0_18; +	vuint32_t	pio0_19; + +	vuint32_t	pio0_20; +	vuint32_t	pio0_21; +	vuint32_t	pio0_22; +	vuint32_t	pio0_23; + +	vuint32_t	pio1_0;		/* 0x60 */ +	vuint32_t	pio1_1; +	vuint32_t	pio1_2; +	vuint32_t	pio1_3; + +	vuint32_t	pio1_4; +	vuint32_t	pio1_5; +	vuint32_t	pio1_6; +	vuint32_t	pio1_7; + +	vuint32_t	pio1_8;		/* 0x80 */ +	vuint32_t	pio1_9; +	vuint32_t	pio1_10; +	vuint32_t	pio1_11; + +	vuint32_t	pio1_12; +	vuint32_t	pio1_13; +	vuint32_t	pio1_14; +	vuint32_t	pio1_15; + +	vuint32_t	pio1_16;	/* 0xa0 */ +	vuint32_t	pio1_17; +	vuint32_t	pio1_18; +	vuint32_t	pio1_19; + +	vuint32_t	pio1_20; +	vuint32_t	pio1_21; +	vuint32_t	pio1_22; +	vuint32_t	pio1_23; + +	vuint32_t	pio1_24;	/* 0xc0 */ +	vuint32_t	pio1_25; +	vuint32_t	pio1_26; +	vuint32_t	pio1_27; + +	vuint32_t	pio1_28; +	vuint32_t	pio1_29; +	vuint32_t	pio1_30; +	vuint32_t	pio1_31; +}; + +extern struct lpc_ioconf lpc_ioconf; + +#define LPC_IOCONF_FUNC		0 + +/* PIO0_0 */ +#define  LPC_IOCONF_FUNC_RESET		0 +#define  LPC_IOCONF_FUNC_PIO0_0		1 + +/* PIO0_1 */ +#define  LPC_IOCONF_FUNC_PIO0_1		0 +#define  LPC_IOCONF_FUNC_CLKOUT		1 +#define  LPC_IOCONF_FUNC_CT32B0_MAT2	2 +#define  LPC_IOCONF_FUNC_USB_FTOGGLE	3 + +/* PIO0_2 */ +#define  LPC_IOCONF_FUNC_PIO0_2		0 +#define  LPC_IOCONF_FUNC_SSEL0		1 +#define  LPC_IOCONF_FUNC_CT16B0_CAP0	2 + +/* PIO0_3 */ +#define  LPC_IOCONF_FUNC_PIO0_3		0 +#define  LPC_IOCONF_FUNC_USB_VBUS	1 + +/* PIO0_4 +#define  LPC_IOCONF_FUNC_PIO0_4		0 +#define  LPC_IOCONF_FUNC_I2C_SCL	1 + +/* PIO0_5 */ +#define  LPC_IOCONF_FUNC_PIO0_5		0 +#define  LPC_IOCONF_FUNC_I2C_SDA	1 + +/* PIO0_6 */ +#define  LPC_IOCONF_FUNC_PIO0_6		0 +#define  LPC_IOCONF_FUNC_USB_CONNECT	1 +#define  LPC_IOCONF_FUNC_PIO0_6_SCK0	2 + +/* PIO0_7 */ +#define  LPC_IOCONF_FUNC_PIO0_7		0 +#define  LPC_IOCONF_FUNC_CTS		1 + +/* PIO0_8 */ +#define  LPC_IOCONF_FUNC_PIO0_8		0 +#define  LPC_IOCONF_FUNC_MISO0		1 +#define  LPC_IOCONF_FUNC_CT16B0_MAT0	2 + +/* PIO0_9 */ +#define  LPC_IOCONF_FUNC_PIO0_9		0 +#define  LPC_IOCONF_FUNC_MOSI0		1 +#define  LPC_IOCONF_FUNC_CT16B0_MAT1	2 + +/* PIO0_10 */ +#define  LPC_IOCONF_FUNC_SWCLK		0 +#define  LPC_IOCONF_FUNC_PIO0_10	1 +#define  LPC_IOCONF_FUNC_PIO0_10_SCK0	2 +#define  LPC_IOCONF_FUNC_CT16B0_MAT2	3 + +/* PIO0_11 */ +#define  LPC_IOCONF_FUNC_TDI		0 +#define  LPC_IOCONF_FUNC_PIO0_11	1 +#define  LPC_IOCONF_FUNC_AD0		2 +#define  LPC_IOCONF_FUNC_CT32B0_MAT3	3 + +/* PIO0_12 */ +#define  LPC_IOCONF_FUNC_TMS		0 +#define  LPC_IOCONF_FUNC_PIO0_12	1 +#define  LPC_IOCONF_FUNC_AD1		2 +#define  LPC_IOCONF_FUNC_CT32B1_CAP0	3 + +/* PIO0_13 */ +#define  LPC_IOCONF_FUNC_TD0		0 +#define  LPC_IOCONF_FUNC_PIO0_13	1 +#define  LPC_IOCONF_FUNC_AD2		2 +#define  LPC_IOCONF_FUNC_CT32B1_MAT0	3 + +/* PIO0_14 */ +#define  LPC_IOCONF_FUNC_TRST		0 +#define  LPC_IOCONF_FUNC_PIO0_14	1 +#define  LPC_IOCONF_FUNC_AD3		2 +#define  LPC_IOCONF_FUNC_PIO0_14_CT32B1_MAT1	3 + +/* PIO0_15 */ +#define  LPC_IOCONF_FUNC_SWDIO		0 +#define  LPC_IOCONF_FUNC_PIO0_15	1 +#define  LPC_IOCONF_FUNC_AD4		2 +#define  LPC_IOCONF_FUNC_CT32B1_MAT2	3 + +/* PIO0_16 */ +#define  LPC_IOCONF_FUNC_PIO0_16	0 +#define  LPC_IOCONF_FUNC_AD5		1 +#define  LPC_IOCONF_FUNC_CT32B1_MAT3	2 + +/* PIO0_17 */ +#define  LPC_IOCONF_FUNC_PIO0_17	0 +#define  LPC_IOCONF_FUNC_RTS		1 +#define  LPC_IOCONF_FUNC_CT32B0_CAP0	2 +#define  LPC_IOCONF_FUNC_SCLK		3 + +/* PIO0_18 */ +#define  LPC_IOCONF_FUNC_PIO0_18		0 +#define  LPC_IOCONF_FUNC_PIO0_18_RXD		1 +#define  LPC_IOCONF_FUNC_PIO0_18_CT32B0_MAT0	2 + +/* PIO0_19 */ +#define  LPC_IOCONF_FUNC_PIO0_19		0 +#define  LPC_IOCONF_FUNC_PIO0_19_TXD		1 +#define  LPC_IOCONF_FUNC_PIO0_19_CT32B0_MAT1	2 + +/* PIO0_20 */ +#define  LPC_IOCONF_FUNC_PIO0_20	0 +#define  LPC_IOCONF_FUNC_CT16B1_CAP0	1 + +/* PIO0_21 */ +#define  LPC_IOCONF_FUNC_PIO0_21	0 +#define  LPC_IOCONF_FUNC_CT16B1_MAT0	1 +#define  LPC_IOCONF_FUNC_PIO0_21_MOSI1	2 + +/* PIO0_22 */ +#define  LPC_IOCONF_FUNC_PIO0_22	0 +#define  LPC_IOCONF_FUNC_AD6		1 +#define  LPC_IOCONF_FUNC_CT16B1_MAT1	2 +#define  LPC_IOCONF_FUNC_PIO0_22_MISO1	3 + +/* PIO0_23 */ +#define  LPC_IOCONF_FUNC_PIO0_23	0 +#define  LPC_IOCONF_FUNC_AD7		1 + +/* PIO1_0 */ +#define  LPC_IOCONF_FUNC_PIO1_0		0 +#define  LPC_IOCONF_FUNC_CT32B1_MAT1	1 + +/* PIO1_1 */ +#define  LPC_IOCONF_FUNC_PIO1_1		0 +#define  LPC_IOCONF_FUNC_CT32B1_MAT1	1 + +/* PIO1_2 */ +#define  LPC_IOCONF_FUNC_PIO1_2		0 +#define  LPC_IOCONF_FUNC_PIO1_2_CT32B1_MAT2	1 + +/* PIO1_3*/ +#define  LPC_IOCONF_FUNC_PIO1_3		0 +#define  LPC_IOCONF_FUNC_PIO1_3_CT32B1_MAT3	1 + +/* PIO1_4 */ +#define  LPC_IOCONF_FUNC_PIO1_4		0 +#define  LPC_IOCONF_FUNC_PIO1_4_CT32B1_CAP0	1 + +/* PIO1_5 */ +#define  LPC_IOCONF_FUNC_PIO1_5		0 +#define  LPC_IOCONF_FUNC_CT32B1_CAP1	1 + +/* PIO1_6 */ +#define  LPC_IOCONF_FUNC_PIO1_6		0 + +/* PIO1_7 */ +#define  LPC_IOCONF_FUNC_PIO1_7		0 + +/* PIO1_8 */ +#define  LPC_IOCONF_FUNC_PIO1_8		0 + +/* PIO1_9 */ +#define  LPC_IOCONF_FUNC_PIO1_9		0 + +/* PIO1_10 */ +#define  LPC_IOCONF_FUNC_PIO1_10	0 + +/* PIO1_11 */ +#define  LPC_IOCONF_FUNC_PIO1_11	0 + +/* PIO1_12 */ +#define  LPC_IOCONF_FUNC_PIO1_12	0 + +/* PIO1_13 */ +#define  LPC_IOCONF_FUNC_PIO1_13	0 +#define  LPC_IOCONF_FUNC_DTR		1 +#define  LPC_IOCONF_FUNC_CT16B0_MAT0	2 +#define  LPC_IOCONF_FUNC_PIO1_13_TXD		3 + +/* PIO1_14 */ +#define  LPC_IOCONF_FUNC_PIO1_14	0 +#define  LPC_IOCONF_FUNC_DSR		1 +#define  LPC_IOCONF_FUNC_CT16B0_MAT1	2 +#define  LPC_IOCONF_FUNC_PIO1_13_RXD		3 + +/* PIO1_15 */ +#define  LPC_IOCONF_FUNC_PIO1_15	0 +#define  LPC_IOCONF_FUNC_DCD		1 +#define  LPC_IOCONF_FUNC_PIO1_15_CT16B0_MAT2	2 +#define  LPC_IOCONF_FUNC_PIO1_15_SCK1	3 + +/* PIO1_16 */ +#define  LPC_IOCONF_FUNC_PIO1_16	0 +#define  LPC_IOCONF_FUNC_RI		1 +#define  LPC_IOCONF_FUNC_CT16B0_CAP0	2 + +/* PIO1_17 */ +#define  LPC_IOCONF_FUNC_PIO1_17	0 +#define  LPC_IOCONF_FUNC_CT16B0_CAP1	1 +#define  LPC_IOCONF_FUNC_PIO1_17_RXD		2 + +/* PIO1_18 */ +#define  LPC_IOCONF_FUNC_PIO1_18	0 +#define  LPC_IOCONF_FUNC_CT16B1_CAP1	1 +#define  LPC_IOCONF_FUNC_PIO1_18_TXD		2 + +/* PIO1_19 */ +#define  LPC_IOCONF_FUNC_PIO1_19	0 +#define  LPC_IOCONF_FUNC_DTR		1 +#define  LPC_IOCONF_FUNC_SSEL1		2 + +/* PIO1_20 */ +#define  LPC_IOCONF_FUNC_PIO1_20	0 +#define  LPC_IOCONF_FUNC_DSR		1 +#define  LPC_IOCONF_FUNC_PIO1_20_SCK1		2 + +/* PIO1_21 */ +#define  LPC_IOCONF_FUNC_PIO1_21	0 +#define  LPC_IOCONF_FUNC_DCD		1 +#define  LPC_IOCONF_FUNC_PIO1_21_MISO1		2 + +/* PIO1_22 */ +#define  LPC_IOCONF_FUNC_PIO1_22	0 +#define  LPC_IOCONF_FUNC_RI		1 +#define  LPC_IOCONF_FUNC_PIO1_22_MOSI1	2 + +/* PIO1_23 */ +#define  LPC_IOCONF_FUNC_PIO1_23	0 +#define  LPC_IOCONF_FUNC_PIO1_23_CT16B1_MAT1	1 +#define  LPC_IOCONF_FUNC_SSEL1		2 + +/* PIO1_24 */ +#define  LPC_IOCONF_FUNC_PIO1_24	0 +#define  LPC_IOCONF_FUNC_PIO1_24_CT32B0_MAT0	1 + +/* PIO1_25 */ +#define  LPC_IOCONF_FUNC_PIO1_25	0 +#define  LPC_IOCONF_FUNC_PIO1_25_CT32B0_MAT1	1 + +/* PIO1_26 */ +#define  LPC_IOCONF_FUNC_PIO1_26	0 +#define  LPC_IOCONF_FUNC_PIO1_26_CT32B0_MAT2	1 +#define  LPC_IOCONF_FUNC_PIO1_26_RXD		2 + +/* PIO1_27 */ +#define  LPC_IOCONF_FUNC_PIO1_27	0 +#define  LPC_IOCONF_FUNC_PIO1_27_CT32B0_MAT3	1 +#define  LPC_IOCONF_FUNC_PIO1_27_TXD		2 + +/* PIO1_28 */ +#define  LPC_IOCONF_FUNC_PIO1_28	0 +#define  LPC_IOCONF_FUNC_PIO1_28_CT32B0_CAP0	1 +#define  LPC_IOCONF_FUNC_PIO1_28_SCLK		2 + +/* PIO1_29 */ +#define  LPC_IOCONF_FUNC_PIO1_29		0 +#define  LPC_IOCONF_FUNC_PIO1_29_SCK0		1 +#define  LPC_IOCONF_FUNC_PIO1_29_CT32B0_CAP1	2 + +/* PIO1_31 */ +#define  LPC_IOCONF_FUNC_PIO1_31	0 + +#define  LPC_IOCONF_FUNC_MASK		0x7 + +#define ao_lpc_alternate(func) (((func) << LPC_IOCONF_FUNC) | \ +				(LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | \ +				(0 << LPC_IOCONF_HYS) |			\ +				(0 << LPC_IOCONF_INV) |			\ +				(0 << LPC_IOCONF_OD) |			\ +				0x80) + +#define LPC_IOCONF_MODE			3 +#define  LPC_IOCONF_MODE_INACTIVE		0 +#define  LPC_IOCONF_MODE_PULL_DOWN		1 +#define  LPC_IOCONF_MODE_PULL_UP		2 +#define  LPC_IOCONF_MODE_REPEATER		3 +#define  LPC_IOCONF_MODE_MASK			3 + +#define LPC_IOCONF_HYS			5 + +#define LPC_IOCONF_INV			6 +#define LPC_IOCONF_ADMODE		7 +#define LPC_IOCONF_FILTR		8 +#define LPC_IOCONF_OD			10 + +struct lpc_scb { +	vuint32_t	sysmemremap;	/* 0x00 */ +	vuint32_t	presetctrl; +	vuint32_t	syspllctrl; +	vuint32_t	syspllstat; + +	vuint32_t	usbpllctrl;	/* 0x10 */ +	vuint32_t	usbpllstat; +	uint32_t	r18; +	uint32_t	r1c; + +	vuint32_t	sysoscctrl;	/* 0x20 */ +	vuint32_t	wdtoscctrl; +	uint32_t	r28; +	uint32_t	r2c; + +	vuint32_t	sysrststat;	/* 0x30 */ +	uint32_t	r34; +	uint32_t	r38; +	uint32_t	r3c; + +	vuint32_t	syspllclksel;	/* 0x40 */ +	vuint32_t	syspllclkuen; +	vuint32_t	usbpllclksel; +	vuint32_t	usbpllclkuen; + +	uint32_t	r50[8]; + +	vuint32_t	mainclksel;	/* 0x70 */ +	vuint32_t	mainclkuen; +	vuint32_t	sysahbclkdiv; +	uint32_t	r7c;		 + +	vuint32_t	sysahbclkctrl;	/* 0x80 */ +	uint32_t	r84[3]; + +	uint32_t	r90;		/* 0x90 */ +	vuint32_t	ssp0clkdiv; +	vuint32_t	uartclkdiv; +	vuint32_t	ssp1clkdiv; + +	uint32_t	ra0[8]; + +	vuint32_t	usbclksel;	/* 0xc0 */ +	vuint32_t	usbclkuen; +	vuint32_t	usbclkdiv; +	uint32_t	rcc; + +	uint32_t	rd0[4]; +	 +	vuint32_t	clkoutsel;	/* 0xe0 */ +	vuint32_t	clkoutuen; +	vuint32_t	clkoutdiv; +	uint32_t	rec; +	 +	uint32_t	rf0[4];		/* 0xf0 */ +	 +	vuint32_t	pioporcap0;	/* 0x100 */ +	vuint32_t	pioporcap1; +	uint32_t	r102[2]; + +	uint32_t	r110[4];	/* 0x110 */ +	uint32_t	r120[4];	/* 0x120 */ +	uint32_t	r130[4];	/* 0x130 */ +	uint32_t	r140[4];	/* 0x140 */ +	 +	vuint32_t	bodctrl;	/* 0x150 */ +	vuint32_t	systckcal; +	uint32_t	r158[2]; + +	uint32_t	r160[4];	/* 0x160 */ + +	vuint32_t	irqlatency;	/* 0x170 */ +	vuint32_t	nmisrc; +	vuint32_t	pintsel[8]; + +	vuint32_t	usbclkctrl;	/* 0x198 */ +	vuint32_t	usbclkst; + +	uint32_t	r1a0[6*4];	/* 0x1a0 */ + +	uint32_t	r200;		/* 0x200 */ +	vuint32_t	starterp0; +	uint32_t	r208[2]; + +	uint32_t	r210;		/* 0x210 */ +	vuint32_t	starterp1; +	uint32_t	r218[2]; + +	uint32_t	r220[4];	/* 0x220 */ + +	vuint32_t	pdsleepcfg;	/* 0x230 */ +	vuint32_t	pdawakecfg; +	vuint32_t	pdruncfg; +	uint32_t	r23c; + +	uint32_t	r240[12 * 4];	/* 0x240 */ + +	uint32_t	r300[15 * 4];	/* 0x300 */ +			      +	uint32_t	r3f0;		/* 0x3f0 */ +	vuint32_t	device_id; +}; + +extern struct lpc_scb lpc_scb; + +#define LPC_SCB_PRESETCTRL_SSP0_RST_N	0 +#define LPC_SCB_PRESETCTRL_I2C_RST_N	1 +#define LPC_SCB_PRESETCTRL_SSP1_RST_N	2 + +#define LPC_SCB_SYSPLLCTRL_MSEL		0 +#define LPC_SCB_SYSPLLCTRL_PSEL		5 +#define  LPC_SCB_SYSPLLCTRL_PSEL_1		0 +#define  LPC_SCB_SYSPLLCTRL_PSEL_2		1 +#define  LPC_SCB_SYSPLLCTRL_PSEL_4		2 +#define  LPC_SCB_SYSPLLCTRL_PSEL_8		3 +#define  LPC_SCB_SYSPLLCTRL_PSEL_MASK		3 + +#define LPC_SCB_SYSPLLSTAT_LOCK		0 + +#define LPC_SCB_USBPLLCTRL_MSEL		0 +#define LPC_SCB_USBPLLCTRL_PSEL		5 +#define  LPC_SCB_USBPLLCTRL_PSEL_1		0 +#define  LPC_SCB_USBPLLCTRL_PSEL_2		1 +#define  LPC_SCB_USBPLLCTRL_PSEL_4		2 +#define  LPC_SCB_USBPLLCTRL_PSEL_8		3 +#define  LPC_SCB_USBPLLCTRL_PSEL_MASK		3 + +#define LPC_SCB_USBPLLSTAT_LOCK		0 + +#define LPC_SCB_SYSOSCCTRL_BYPASS	0 +#define LPC_SCB_SYSOSCCTRL_FREQRANGE	1 +#define  LPC_SCB_SYSOSCCTRL_FREQRANGE_1_20	0 +#define  LPC_SCB_SYSOSCCTRL_FREQRANGE_15_25	1 + +#define LPC_SCB_WDTOSCCTRL_DIVSEL		0 +#define  LPC_SCB_WDTOSCCTRL_DIVSEL_MASK			0x1f +#define LPC_SCB_WDTOSCCTRL_FREQSEL		5 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_0_6			1 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_1_05		2 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_1_4			3 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_1_75		4 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_2_1			5 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_2_4			6 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_2_7			7 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_3_0			8 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_3_25		9 +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_3_5			0x0a +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_3_75		0x0b +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_4_0			0x0c +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_4_2			0x0d +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_4_4			0x0e +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_4_6			0x0f +#define  LPC_SCB_WDTOSCCTRL_FREQSEL_MASK		0x0f + +#define LPC_SCB_SYSRSTSTAT_POR		0 +#define LPC_SCB_SYSRSTSTAT_EXTRST	1 +#define LPC_SCB_SYSRSTSTAT_WDT		2 +#define LPC_SCB_SYSRSTSTAT_BOD		3 +#define LPC_SCB_SYSRSTSTAT_SYSRST	4 + +#define LPC_SCB_SYSPLLCLKSEL_SEL	0 +#define  LPC_SCB_SYSPLLCLKSEL_SEL_IRC		0 +#define  LPC_SCB_SYSPLLCLKSEL_SEL_SYSOSC	1 +#define  LPC_SCB_SYSPLLCLKSEL_SEL_MASK		3 + +#define LPC_SCB_SYSPLLCLKUEN_ENA	0 + +#define LPC_SCB_USBPLLCLKSEL_SEL	0 +#define  LPC_SCB_USBPLLCLKSEL_SEL_IRC		0 +#define  LPC_SCB_USBPLLCLKSEL_SEL_SYSOSC	1 +#define  LPC_SCB_USBPLLCLKSEL_SEL_MASK		3 + +#define LPC_SCB_USBPLLCLKUEN_ENA	0 + +#define LPC_SCB_MAINCLKSEL_SEL		0 +#define  LPC_SCB_MAINCLKSEL_SEL_IRC		0 +#define  LPC_SCB_MAINCLKSEL_SEL_PLL_INPUT	1 +#define  LPC_SCB_MAINCLKSEL_SEL_WATCHDOG	2 +#define  LPC_SCB_MAINCLKSEL_SEL_PLL_OUTPUT	3 +#define  LPC_SCB_MAINCLKSEL_SEL_MASK		3 + +#define LPC_SCB_MAINCLKUEN_ENA		0 + +#define LPC_SCB_SYSAHBCLKDIV_DIV	0 + +#define LPC_SCB_SYSAHBCLKCTRL_SYS	0 +#define LPC_SCB_SYSAHBCLKCTRL_ROM	1 +#define LPC_SCB_SYSAHBCLKCTRL_RAM0	2 +#define LPC_SCB_SYSAHBCLKCTRL_FLASHREG	3 +#define LPC_SCB_SYSAHBCLKCTRL_FLASHARRAY	4 +#define LPC_SCB_SYSAHBCLKCTRL_I2C	5 +#define LPC_SCB_SYSAHBCLKCTRL_GPIO	6 +#define LPC_SCB_SYSAHBCLKCTRL_CT16B0	7 +#define LPC_SCB_SYSAHBCLKCTRL_CT16B1	8 +#define LPC_SCB_SYSAHBCLKCTRL_CT32B0	9 +#define LPC_SCB_SYSAHBCLKCTRL_CT32B1	10 +#define LPC_SCB_SYSAHBCLKCTRL_SSP0	11 +#define LPC_SCB_SYSAHBCLKCTRL_USART	12 +#define LPC_SCB_SYSAHBCLKCTRL_ADC	13 +#define LPC_SCB_SYSAHBCLKCTRL_USB	14 +#define LPC_SCB_SYSAHBCLKCTRL_WWDT	15 +#define LPC_SCB_SYSAHBCLKCTRL_IOCON	16 +#define LPC_SCB_SYSAHBCLKCTRL_SSP1	18 +#define LPC_SCB_SYSAHBCLKCTRL_PINT	19 +#define LPC_SCB_SYSAHBCLKCTRL_GROUP0INT	23 +#define LPC_SCB_SYSAHBCLKCTRL_GROUP1INT	24 +#define LPC_SCB_SYSAHBCLKCTRL_RAM1	26 +#define LPC_SCB_SYSAHBCLKCTRL_USBRAM	27 + +#define LPC_SCB_SSP0CLKDIV_ +#define LPC_SCB_UARTCLKDIV_ +#define LPC_SCB_SSP1CLKDIV_ + +#define LPC_SCB_USBCLKSEL_SEL		0 +#define LPC_SCB_USBCLKSEL_SEL_USB_PLL		0 +#define LPC_SCB_USBCLKSEL_SEL_MAIN_CLOCK	1 + +#define LPC_SCB_USBCLKUEN_ENA		0 +#define LPC_SCB_USBCLKDIV_DIV		0 + +#define LPC_SCB_CLKOUTSEL_SEL		0 +#define  LPC_SCB_CLKOUTSEL_SEL_IRC		0 +#define  LPC_SCB_CLKOUTSEL_SEL_SYSOSC		1 +#define  LPC_SCB_CLKOUTSEL_SEL_LF		2 +#define  LPC_SCB_CLKOUTSEL_SEL_MAIN_CLOCK	3 + +#define LPC_SCB_CLKOUTUEN_ENA		0 + +#define LPC_SCB_PDRUNCFG_IRCOUT_PD	0 +#define LPC_SCB_PDRUNCFG_IRC_PD		1 +#define LPC_SCB_PDRUNCFG_FLASH_PD	2 +#define LPC_SCB_PDRUNCFG_BOD_PD		3 +#define LPC_SCB_PDRUNCFG_ADC_PD		4 +#define LPC_SCB_PDRUNCFG_SYSOSC_PD	5 +#define LPC_SCB_PDRUNCFG_WDTOSC_PD	6 +#define LPC_SCB_PDRUNCFG_SYSPLL_PD	7 +#define LPC_SCB_PDRUNCFG_USBPLL_PD	8 +#define LPC_SCB_PDRUNCFG_USBPAD_PD	10 + +struct lpc_flash { +	uint32_t	r0[4];		/* 0x0 */ + +	vuint32_t	flashcfg;	/* 0x10 */ +}; + +extern struct lpc_flash lpc_flash; + +struct lpc_gpio_pin { +	vuint32_t	isel;		/* 0x00 */ +	vuint32_t	ienr; +	vuint32_t	sienr; +	vuint32_t	cienr; + +	vuint32_t	ienf;		/* 0x10 */ +	vuint32_t	sienf; +	vuint32_t	cienf; +	vuint32_t	rise; + +	vuint32_t	fall;		/* 0x20 */ +	vuint32_t	ist; +}; + +extern struct lpc_gpio_pin lpc_gpio_pin; + +struct lpc_gpio_group0 { +}; + +extern struct lpc_gpio_group0 lpc_gpio_group0; + +struct lpc_gpio_group1 { +}; + +extern struct lpc_gpio_group1 lpc_gpio_group1; + +struct lpc_gpio { +	vuint8_t	byte[0x40];	/* 0x0000 */ + +	uint8_t		r0030[0x1000 - 0x40]; + +	vuint32_t	word[0x40];	/* 0x1000 */ + +	uint8_t		r1100[0x2000 - 0x1100]; +	 +	vuint32_t	dir[2];		/* 0x2000 */ + +	uint8_t		r2008[0x2080 - 0x2008]; + +	vuint32_t	mask[2];	/* 0x2080 */ + +	uint8_t		r2088[0x2100 - 0x2088]; + +	vuint32_t	pin[2];		/* 0x2100 */ + +	uint8_t		r2108[0x2200 - 0x2108]; + +	vuint32_t	set[2];		/* 0x2200 */ + +	uint8_t		r2208[0x2280 - 0x2208]; + +	vuint32_t	clr[2];		/* 0x2280 */ + +	uint8_t		r2288[0x2300 - 0x2288]; + +	vuint32_t	not[2];		/* 0x2300 */ +}; + +extern struct lpc_gpio lpc_gpio; + +struct lpc_systick { +	uint8_t		r0000[0x10];	/* 0x0000 */ + +	vuint32_t	csr;		/* 0x0010 */ +	vuint32_t	rvr; +	vuint32_t	cvr; +	vuint32_t	calib; +}; + +extern struct lpc_systick lpc_systick; + +#define LPC_SYSTICK_CSR_ENABLE		0 +#define LPC_SYSTICK_CSR_TICKINT		1 +#define LPC_SYSTICK_CSR_CLKSOURCE	2 +#define  LPC_SYSTICK_CSR_CLKSOURCE_CPU_OVER_2		0 +#define  LPC_SYSTICK_CSR_CLKSOURCE_CPU			1 +#define LPC_SYSTICK_CSR_COUNTFLAG	16 + +struct lpc_usart { +	vuint32_t	rbr_thr;	/* 0x0000 */ +	vuint32_t	ier; +	vuint32_t	iir_fcr; +	vuint32_t	lcr; + +	vuint32_t	mcr;		/* 0x0010 */ +	vuint32_t	lsr; +	vuint32_t	msr; +	vuint32_t	scr; + +	vuint32_t	acr;		/* 0x0020 */ +	vuint32_t	icr; +	vuint32_t	fdr; +	vuint32_t	osr; + +	vuint32_t	ter;		/* 0x0030 */ +	uint32_t	r34[3]; + +	vuint32_t	hden;		/* 0x0040 */ +	uint32_t	r44; +	vuint32_t	scictrl; +	vuint32_t	rs485ctrl; + +	vuint32_t	rs485addrmatch;	/* 0x0050 */ +	vuint32_t	rs485dly; +	vuint32_t	syncctrl; +}; + +extern struct lpc_usart lpc_usart; + +#define LPC_USART_IER_RBRINTEN	0 +#define LPC_USART_IER_THREINTEN	1 +#define LPC_USART_IER_RSLINTEN	2 +#define LPC_USART_IER_MSINTEN	3 +#define LPC_USART_IER_ABEOINTEN	8 +#define LPC_USART_IER_ABTOINTEN	9 + +#define LPC_USART_IIR_INTSTATUS		0 +#define LPC_USART_IIR_INTID		1 +#define LPC_USART_IIR_INTID_RLS			3 +#define LPC_USART_IIR_INTID_RDA			2 +#define LPC_USART_IIR_INTID_CTI			6 +#define LPC_USART_IIR_INTID_THRE		1 +#define LPC_USART_IIR_INTID_MS			0 +#define LPC_USART_IIR_INTID_MASK		7 +#define LPC_USART_IIR_FIFOEN		6 +#define LPC_USART_IIR_ABEOINT		8 +#define LPC_USART_IIR_ABTOINT		9 + +#define LPC_USART_FCR_FIFOEN		0 +#define LPC_USART_FCR_RXFIFORES		1 +#define LPC_USART_FCR_TXFIFORES		2 +#define LPC_USART_FCR_RXTL		6 +#define LPC_USART_FCR_RXTL_1			0 +#define LPC_USART_FCR_RXTL_4			1 +#define LPC_USART_FCR_RXTL_8			2 +#define LPC_USART_FCR_RXTL_14			3 + +#define LPC_USART_LCR_WLS	0 +#define LPC_USART_LCR_WLS_5		0 +#define LPC_USART_LCR_WLS_6		1 +#define LPC_USART_LCR_WLS_7		2 +#define LPC_USART_LCR_WLS_8		3 +#define LPC_USART_LCR_WLS_MASK		3 +#define LPC_USART_LCR_SBS	2 +#define LPC_USART_LCR_SBS_1		0 +#define LPC_USART_LCR_SBS_2		1 +#define LPC_USART_LCR_SBS_MASK		1 +#define LPC_USART_LCR_PE	3 +#define LPC_USART_LCR_PS	4 +#define LPC_USART_LCR_PS_ODD		0 +#define LPC_USART_LCR_PS_EVEN		1 +#define LPC_USART_LCR_PS_ONE		2 +#define LPC_USART_LCR_PS_ZERO		3 +#define LPC_USART_LCR_PS_MASK		3 +#define LPC_USART_LCR_BC	6 +#define LPC_USART_LCR_DLAB	7 + +#define LPC_USART_MCR_DTRCTRL	0 +#define LPC_USART_MCR_RTSCTRL	1 +#define LPC_USART_MCR_LMS	4 +#define LPC_USART_MCR_RTSEN	6 +#define LPC_USART_MCR_CTSEN	7 + +#define LPC_USART_LSR_RDR	0 +#define LPC_USART_LSR_OE	1 +#define LPC_USART_LSR_PE	2 +#define LPC_USART_LSR_FE	3 +#define LPC_USART_LSR_BI	4 +#define LPC_USART_LSR_THRE	5 +#define LPC_USART_LSR_TEMT	6 +#define LPC_USART_LSR_RXFE	7 +#define LPC_USART_LSR_TXERR	8 + +#define LPC_USART_MSR_DCTS	0 +#define LPC_USART_MSR_DDSR	1 +#define LPC_USART_MSR_TERI	2 +#define LPC_USART_MSR_DDCD	3 +#define LPC_USART_MSR_CTS	4 +#define LPC_USART_MSR_DSR	5 +#define LPC_USART_MSR_RI	6 +#define LPC_USART_MSR_DCD	7 + +#define LPC_USART_ACR_START	0 +#define LPC_USART_ACR_MODE	1 +#define LPC_USART_ACR_AUTORESTART	2 +#define LPC_USART_ACR_ABEOINTCLR	8 +#define LPC_USART_ACR_ABTOINTCLR	9 + +#define LPC_USART_FDR_DIVADDVAL	0 +#define LPC_USART_FDR_MULVAL	4 + +#define LPC_USART_OSR_OSFRAC	1 +#define LPC_USART_OSR_OSINT	4 +#define LPC_USART_OSR_FDINT	8 + +#define LPC_USART_TER_TXEN	7 + +#define LPC_USART_HDEN_HDEN	0 + +struct lpc_usb { +	vuint32_t	devcmdstat; +	vuint32_t	info; +	vuint32_t	epliststart; +	vuint32_t	databufstart; +	vuint32_t	lpm; +	vuint32_t	epskip; +	vuint32_t	epinuse; +	vuint32_t	epbufcfg; +	vuint32_t	intstat; +	vuint32_t	inten; +	vuint32_t	intsetstat; +	vuint32_t	introuting; +	uint32_t	r30; +	vuint32_t	eptoggle; +} lpc_usb; + +extern struct lpc_usb lpc_usb; + +#define LPC_USB_DEVCMDSTAT_DEV_ADDR	0 +#define LPC_USB_DEVCMDSTAT_DEV_ADDR_MASK	0x7f +#define LPC_USB_DEVCMDSTAT_DEV_EN	7 +#define LPC_USB_DEVCMDSTAT_SETUP	8 +#define LPC_USB_DEVCMDSTAT_PLL_ON	9 +#define LPC_USB_DEVCMDSTAT_LPM_SUP	11 +#define LPC_USB_DEVCMDSTAT_INTONNAK_AO	12 +#define LPC_USB_DEVCMDSTAT_INTONNAK_AI	13 +#define LPC_USB_DEVCMDSTAT_INTONNAK_CO	14 +#define LPC_USB_DEVCMDSTAT_INTONNAK_CI	15 +#define LPC_USB_DEVCMDSTAT_DCON		16 +#define LPC_USB_DEVCMDSTAT_DSUS		17 +#define LPC_USB_DEVCMDSTAT_LPM_SUS	19 +#define LPC_USB_DEVCMDSTAT_LPM_REWP	20 +#define LPC_USB_DEVCMDSTAT_DCON_C	24 +#define LPC_USB_DEVCMDSTAT_DSUS_C	25 +#define LPC_USB_DEVCMDSTAT_DRES_C	26 +#define LPC_USB_DEVCMDSTAT_VBUSDEBOUNCED	28 + +#define LPC_USB_INFO_FRAME_NR		0 +#define LPC_USB_INFO_FRAME_NR_MASK	0x3ff +#define LPC_USB_INFO_ERR_CODE		11 +#define LPC_USB_INFO_ERR_CODE_NO_ERROR			0 +#define LPC_USB_INFO_ERR_CODE_PID_ENCODING_ERROR	1 +#define LPC_USB_INFO_ERR_CODE_PID_UNKNOWN		2 +#define LPC_USB_INFO_ERR_CODE_PACKET_UNEXPECTED		3 +#define LPC_USB_INFO_ERR_CODE_TOKEN_CRC_ERROR		4 +#define LPC_USB_INFO_ERR_CODE_DATA_CRC_ERROR		5 +#define LPC_USB_INFO_ERR_CODE_TIME_OUT			6 +#define LPC_USB_INFO_ERR_CODE_BABBLE			7 +#define LPC_USB_INFO_ERR_CODE_TRUNCATED_EOP		8 +#define LPC_USB_INFO_ERR_CODE_SENT_RECEIVED_NAK		9 +#define LPC_USB_INFO_ERR_CODE_SENT_STALL		0xa +#define LPC_USB_INFO_ERR_CODE_OVERRUN			0xb +#define LPC_USB_INFO_ERR_CODE_SENT_EMPTY_PACKET		0xc +#define LPC_USB_INFO_ERR_CODE_BITSTUFF_ERROR		0xd +#define LPC_USB_INFO_ERR_CODE_SYNC_ERROR		0xe +#define LPC_USB_INFO_ERR_CODE_WRONG_DATA_TOGGLE		0xf +#define LPC_USB_INFO_ERR_CODE_MASK			0xf + +#define LPC_USB_EPLISTSTART_EP_LIST			0 + +#define LPC_USB_DATABUFSTART_DA_BUF			0 + +#define LPC_USB_LPM_HIRD_HW		0 +#define LPC_USB_LPM_HIRD_HW_MASK		0xf +#define LPC_USB_LPM_HIRD_SW		4 +#define LPC_USB_LPM_HIRD_SW_MASK		0xf +#define LPC_USB_LPM_DATA_PENDING	8 + +#define LPC_USB_EPSKIP_SKIP		0 + +#define LPC_USB_EPINUSE_BUF(ep)		(ep) + +#define LPC_USB_EPBUFCFG_BUF_SB(ep)	(ep) + +#define LPC_USB_INT_EPOUT(ep)		((ep) << 1) +#define LPC_USB_INT_EPIN(ep)		(((ep) << 1) + 1) + +#define LPC_USB_INT_FRAME		30 +#define LPC_USB_INT_DEV			31 + +#define LPC_USB_INTIN_EP_INT_EN(ep)	(ep) +#define LPC_USB_INTIN_FRAME_INT_EN	30 +#define LPC_USB_INTIN_DEV_INT_EN	31 + +#define LPC_USB_INTSETSTAT_EP_SET_INT(ep)	(ep) +#define LPC_USB_INTSETSTAT_FRAME_SET_INT	30 +#define LPC_USB_INTSETSTAT_DEV_SET_INT		31 + +#define LPC_USB_INTROUTING_ROUTE_INT(ep)	(ep) +#define LPC_USB_INTROUTING_INT30		30 +#define LPC_USB_INTROUTING_INT31		31 + +#define LPC_USB_EPTOGGLE_TOGGLE(ep)		(ep) + +struct lpc_usb_epn { +	vuint32_t		out[2]; +	vuint32_t		in[2]; +}; + +struct lpc_usb_endpoint { +	vuint32_t		ep0_out; +	vuint32_t		setup; +	vuint32_t		ep0_in; +	vuint32_t		reserved_0c; +	struct lpc_usb_epn	epn[4]; +}; + +/* Assigned in registers.ld to point at the base + * of USB ram + */ + +extern uint8_t lpc_usb_sram[]; + +#define LPC_USB_EP_ACTIVE		31 +#define LPC_USB_EP_DISABLED		30 +#define LPC_USB_EP_STALL		29 +#define LPC_USB_EP_TOGGLE_RESET		28 +#define LPC_USB_EP_RATE_FEEDBACK	27 +#define LPC_USB_EP_ENDPOINT_ISO		26 +#define LPC_USB_EP_NBYTES		16 +#define  LPC_USB_EP_NBYTES_MASK			0x3ff +#define LPC_USB_EP_OFFSET		0 + +#define LPC_ISR_PIN_INT0_POS	0 +#define LPC_ISR_PIN_INT1_POS	1 +#define LPC_ISR_PIN_INT2_POS	2 +#define LPC_ISR_PIN_INT3_POS	3 +#define LPC_ISR_PIN_INT4_POS	4 +#define LPC_ISR_PIN_INT5_POS	5 +#define LPC_ISR_PIN_INT6_POS	6 +#define LPC_ISR_PIN_INT7_POS	7 +#define LPC_ISR_GINT0_POS	8 +#define LPC_ISR_GINT1_POS	9 +#define LPC_ISR_SSP1_POS	14 +#define LPC_ISR_I2C_POS		15 +#define LPC_ISR_CT16B0_POS	16 +#define LPC_ISR_CT16B1_POS	17 +#define LPC_ISR_CT32B0_POS	18 +#define LPC_ISR_CT32B1_POS	19 +#define LPC_ISR_SSP0_POS	20 +#define LPC_ISR_USART_POS	21 +#define LPC_ISR_USB_IRQ_POS	22 +#define LPC_ISR_USB_FIQ_POS	23 +#define LPC_ISR_ADC_POS		24 +#define LPC_ISR_WWDT_POS	25 +#define LPC_ISR_BOD_POS		26 +#define LPC_ISR_FLASH_POS	27 +#define LPC_ISR_USB_WAKEUP_POS	30 + +struct lpc_nvic { +	vuint32_t	iser;		/* 0x000 0xe000e100 Set Enable Register */ + +	uint8_t		_unused020[0x080 - 0x004]; + +	vuint32_t	icer;		/* 0x080 0xe000e180 Clear Enable Register */ + +	uint8_t		_unused0a0[0x100 - 0x084]; + +	vuint32_t	ispr;		/* 0x100 0xe000e200 Set Pending Register */ + +	uint8_t		_unused120[0x180 - 0x104]; + +	vuint32_t	icpr;		/* 0x180 0xe000e280 Clear Pending Register */ + +	uint8_t		_unused1a0[0x300 - 0x184]; + +	vuint32_t	ipr[8];		/* 0x300 0xe000e400 Priority Register */ +}; + +extern struct lpc_nvic lpc_nvic; + +static inline void +lpc_nvic_set_enable(int irq) { +	lpc_nvic.iser |= (1 << irq); +} + +static inline void +lpc_nvic_clear_enable(int irq) { +	lpc_nvic.icer |= (1 << irq); +} + +static inline int +lpc_nvic_enabled(int irq) { +	return (lpc_nvic.iser >> irq) & 1; +} + +	 +static inline void +lpc_nvic_set_pending(int irq) { +	lpc_nvic.ispr = (1 << irq); +} + +static inline void +lpc_nvic_clear_pending(int irq) { +	lpc_nvic.icpr = (1 << irq); +} + +static inline int +lpc_nvic_pending(int irq) { +	return (lpc_nvic.ispr >> irq) & 1; +} + +#define IRQ_PRIO_REG(irq)	((irq) >> 2) +#define IRQ_PRIO_BIT(irq)	(((irq) & 3) << 3) +#define IRQ_PRIO_MASK(irq)	(0xff << IRQ_PRIO_BIT(irq)) + +static inline void +lpc_nvic_set_priority(int irq, uint8_t prio) { +	int		n = IRQ_PRIO_REG(irq); +	uint32_t	v; + +	v = lpc_nvic.ipr[n]; +	v &= ~IRQ_PRIO_MASK(irq); +	v |= (prio) << IRQ_PRIO_BIT(irq); +	lpc_nvic.ipr[n] = v; +} + +static inline uint8_t +lpc_nvic_get_priority(int irq) { +	return (lpc_nvic.ipr[IRQ_PRIO_REG(irq)] >> IRQ_PRIO_BIT(irq)) & IRQ_PRIO_MASK(0); +} + +struct arm_scb { +	vuint32_t	cpuid; +	vuint32_t	icsr; +	uint32_t	reserved08; +	vuint32_t	aircr; + +	vuint32_t	scr; +	vuint32_t	ccr; +	uint32_t	reserved18; +	vuint32_t	shpr2; + +	vuint32_t	shpr3; +}; + +extern struct arm_scb arm_scb; + +struct lpc_ssp { +	vuint32_t	cr0;	/* 0x00 */ +	vuint32_t	cr1; +	vuint32_t	dr; +	vuint32_t	sr; + +	vuint32_t	cpsr;	/* 0x10 */ +	vuint32_t	imsc; +	vuint32_t	ris; +	vuint32_t	mis; + +	vuint32_t	icr;	/* 0x20 */ +}; + +extern struct lpc_ssp lpc_ssp0, lpc_ssp1; + +#define LPC_NUM_SPI		2 + +#define LPC_SSP_FIFOSIZE	8 + +#define LPC_SSP_CR0_DSS		0 +#define  LPC_SSP_CR0_DSS_4		0x3 +#define  LPC_SSP_CR0_DSS_5		0x4 +#define  LPC_SSP_CR0_DSS_6		0x5 +#define  LPC_SSP_CR0_DSS_7		0x6 +#define  LPC_SSP_CR0_DSS_8		0x7 +#define  LPC_SSP_CR0_DSS_9		0x8 +#define  LPC_SSP_CR0_DSS_10		0x9 +#define  LPC_SSP_CR0_DSS_11		0xa +#define  LPC_SSP_CR0_DSS_12		0xb +#define  LPC_SSP_CR0_DSS_13		0xc +#define  LPC_SSP_CR0_DSS_14		0xd +#define  LPC_SSP_CR0_DSS_15		0xe +#define  LPC_SSP_CR0_DSS_16		0xf +#define LPC_SSP_CR0_FRF		4 +#define  LPC_SSP_CR0_FRF_SPI		0 +#define  LPC_SSP_CR0_FRF_TI		1 +#define  LPC_SSP_CR0_FRF_MICROWIRE	2 +#define LPC_SSP_CR0_CPOL	6 +#define  LPC_SSP_CR0_CPOL_LOW		0 +#define  LPC_SSP_CR0_CPOL_HIGH		1 +#define LPC_SSP_CR0_CPHA	7 +#define  LPC_SSP_CR0_CPHA_FIRST		0 +#define  LPC_SSP_CR0_CPHA_SECOND	1 +#define LPC_SSP_CR0_SCR		8 + +#define LPC_SSP_CR1_LBM		0 +#define LPC_SSP_CR1_SSE		1 +#define LPC_SSP_CR1_MS		2 +#define  LPC_SSP_CR1_MS_MASTER		0 +#define  LPC_SSP_CR1_MS_SLAVE		1 +#define LPC_SSP_CR1_SOD		3 + +#define LPC_SSP_SR_TFE		0 +#define LPC_SSP_SR_TNF		1 +#define LPC_SSP_SR_RNE		2 +#define LPC_SSP_SR_RFF		3 +#define LPC_SSP_SR_BSY		4 + +#define LPC_SSP_IMSC_RORIM	0 +#define LPC_SSP_IMSC_RTIM	1 +#define LPC_SSP_IMSC_RXIM	2 +#define LPC_SSP_IMSC_TXIM	3 + +#define LPC_SSP_RIS_RORRIS	0 +#define LPC_SSP_RIS_RTRIS	1 +#define LPC_SSP_RIS_RXRIS	2 +#define LPC_SSP_RIS_TXRIS	3 + +#define LPC_SSP_MIS_RORMIS	0 +#define LPC_SSP_MIS_RTMIS	1 +#define LPC_SSP_MIS_RXMIS	2 +#define LPC_SSP_MIS_TXMIS	3 + +#define LPC_SSP_ICR_RORIC	0 +#define LPC_SSP_ICR_RTIC	1 + +struct lpc_adc { +	vuint32_t	cr;	/* 0x00 */ +	vuint32_t	gdr; +	uint32_t	r08; +	vuint32_t	inten; + +	vuint32_t	dr[8];	/* 0x10 */ + +	vuint32_t	stat;	/* 0x30 */ +}; + +extern struct lpc_adc lpc_adc; + +#define LPC_ADC_CR_SEL		0 +#define LPC_ADC_CR_CLKDIV	8 +#define LPC_ADC_CR_BURST	16 +#define LPC_ADC_CR_CLKS		17 +#define  LPC_ADC_CR_CLKS_11		0 +#define  LPC_ADC_CR_CLKS_10		1 +#define  LPC_ADC_CR_CLKS_9		2 +#define  LPC_ADC_CR_CLKS_8		3 +#define  LPC_ADC_CR_CLKS_7		4 +#define  LPC_ADC_CR_CLKS_6		5 +#define  LPC_ADC_CR_CLKS_5		6 +#define  LPC_ADC_CR_CLKS_4		7 + +#define LPC_ADC_INTEN_ADINTEN	0 +#define LPC_ADC_INTEN_ADGINTEN	8 + +#define LPC_ADC_STAT_DONE	0 +#define LPC_ADC_STAT_OVERRUN	8 +#define LPC_ADC_STAT_ADINT	16 + +struct lpc_ct32b { +	vuint32_t	ir;	/* 0x00 */ +	vuint32_t	tcr; +	vuint32_t	tc; +	vuint32_t	pr; +	 +	vuint32_t	pc;	/* 0x10 */ +	vuint32_t	mcr; +	vuint32_t	mr[4];	/* 0x18 */ +	vuint32_t	ccr;	/* 0x28 */ +	vuint32_t	cr0; +	 +	vuint32_t	cr1_0;	/* 0x30 (only for ct32b0 */ +	vuint32_t	cr1_1;	/* 0x34 (only for ct32b1 */ +	uint32_t	r38; +	vuint32_t	emr; + +	uint32_t	r40[12]; + +	vuint32_t	ctcr;	/* 0x70 */ +	vuint32_t	pwmc; +}; + +extern struct lpc_ct32b lpc_ct32b0, lpc_ct32b1; + +#define LPC_CT32B_TCR_CEN	0 +#define LPC_CT32B_TCR_CRST	1 + +#define LPC_CT32B_MCR_MR0R	1 + +#define LPC_CT32B_PWMC_PWMEN0	0 +#define LPC_CT32B_PWMC_PWMEN1	1 +#define LPC_CT32B_PWMC_PWMEN2	2 +#define LPC_CT32B_PWMC_PWMEN3	3 + +#define LPC_CT32B_EMR_EMC0	4 +#define LPC_CT32B_EMR_EMC1	6 +#define LPC_CT32B_EMR_EMC2	8 +#define LPC_CT32B_EMR_EMC3	10 + +#define LPC_CT32B_EMR_EMC_NOTHING	0 +#define LPC_CT32B_EMR_EMC_CLEAR		1 +#define LPC_CT32B_EMR_EMC_SET		2 +#define LPC_CT32B_EMR_EMC_TOGGLE	3 + +#endif /* _LPC_H_ */ diff --git a/src/lpc/registers.ld b/src/lpc/registers.ld new file mode 100644 index 00000000..a523c39c --- /dev/null +++ b/src/lpc/registers.ld @@ -0,0 +1,19 @@ +lpc_usb_sram	= 0x20004000; +lpc_usb_endpoint = 0x20004700; +lpc_usart	= 0x40008000; +lpc_ct32b0	= 0x40014000; +lpc_ct32b1	= 0x40018000; +lpc_adc		= 0x4001c000; +lpc_flash	= 0x4003c000; +lpc_ssp0	= 0x40040000; +lpc_ioconf	= 0x40044000; +lpc_scb		= 0x40048000; +lpc_gpio_pin	= 0x4004c000; +lpc_ssp1	= 0x40058000; +lpc_gpio_group0 = 0x4005c000; +lpc_gpio_group1 = 0x40060000; +lpc_usb		= 0x40080000; +lpc_gpio	= 0x50000000; +lpc_systick	= 0xe000e000; +lpc_nvic	= 0xe000e100; +arm_scb		= 0xe000ed00; | 
