diff --git a/cpu/arm/stm32f1x_cl/Makefile.stm32f1x_cl b/cpu/arm/stm32f1x_cl/Makefile.stm32f1x_cl new file mode 100644 index 000000000..c816ea40e --- /dev/null +++ b/cpu/arm/stm32f1x_cl/Makefile.stm32f1x_cl @@ -0,0 +1,169 @@ +# Adapted from Makefile.stm32f103 + +# Default to STM32F107VCT7 +ifndef SUBTARGET +$(warning SUBTARGET not defined, defaulting to STM32F107VCT) +SUBTARGET = 7VCT +endif + +### libopencm3 specifics +OPENCM3_BASE= +OPENCM3_FAMILY=STM32F1 +OPENCM3_LIB=opencm3_stm32f1 + +ifndef OPENCM3_BASE +$(error OPENCM3_BASE not defined. Please set definition to root of libopencm3) +endif +### Code common for all ARM CPUs + +CONTIKI_CPU_ARM=$(CONTIKI)/cpu/arm +CONTIKI_CPU_ARM_COMMON=$(CONTIKI_CPU_ARM)/common + +### Define the CPU directory +CONTIKI_CPU=$(CONTIKI_CPU_ARM)/stm32f1x_cl + +### Define the source files we have in the STM32F1x_cl port + +CONTIKI_CPU_DIRS = . + +STM32F1x = clock.c util.c uip-log.c symbols.c + +#Using the opencm3 usb library for now +#include $(CONTIKI_CPU_ARM_COMMON)/usb/Makefile.usb + +#No SD support for now...This is a TODO +#include $(CONTIKI_CPU_ARM_COMMON)/SD-card/Makefile.sdcard + +TARGETLIBS = random.c + +CONTIKI_TARGET_SOURCEFILES += $(STM32F1x) $(TARGETLIBS) + +CONTIKI_SOURCEFILES += $(CONTIKI_TARGET_SOURCEFILES) + +PREFIX = arm-none-eabi + +### Compiler definitions +CC = $(PREFIX)-gcc +LD = $(PREFIX)-ld +AS = $(PREFIX)-as +AR = $(PREFIX)-ar +NM = $(PREFIX)-nm +OBJCOPY = $(PREFIX)-objcopy +STRIP = $(PREFIX)-strip + +XSLTPROC=xsltproc + +PROJECT_OBJECTFILES += ${addprefix $(OBJECTDIR)/,$(CONTIKI_TARGET_MAIN:.c=.o)} + +LINKERSCRIPT = $(CONTIKI_CPU)/STM32F10$(SUBTARGET).ld + +# DFU-UTIL program upload +DFU_UTIL=dfu-util +DFU_UTIL_OFFSET=0x08000000 + +# Use dfu-util by default +PROG=dfuutil + +ARCH_FLAGS= -mcpu=cortex-m3 -mthumb + +#CONTIKI_CFLAGS = -DWITH_UIP -DWITH_ASCII + +CFLAGSNO = -I. -I$(CONTIKI)/core -I$(CONTIKI_CPU) \ + -I$(OPENCM3_BASE)/include \ + -I$(CONTIKI)/platform/$(TARGET) \ + ${addprefix -I,$(APPDIRS)} \ + -DMCK=$(MCK) -DSUBTARGET=$(SUBTARGET) \ + -Wall $(ARCH_FLAGS) -g -DSTM32F1 -MD + +CFLAGS += $(CONTIKI_CFLAGS) $(CFLAGSNO) -Os + +LDFLAGS += -L $(CONTIKI_CPU) -T $(LINKERSCRIPT) \ + -Wl,--start-group -lc -lgcc -Wl,--end-group\ + -nostartfiles -Wl,--gc-sections $(ARCH_FLAGS) + +EXTERN_LIBS += -L $(OPENCM3_BASE)/lib -l$(OPENCM3_LIB) + +CDEPFLAGS = $(CFLAGS) -D __MAKING_DEPS__ + +### Setup directory search path for source files + +CUSTOM_RULE_C_TO_OBJECTDIR_O=yes +CUSTOM_RULE_C_TO_O=yes + +%.o: %.c + $(CC) $(CFLAGS) $< -c + +$(OBJECTDIR)/%.o: %.c + $(CC) $(CFLAGS) -c $< -o $@ + + +CUSTOM_RULE_S_TO_OBJECTDIR_O = yes +%.o: %.S + $(CC) $(CFLAGS) $< -c + +$(OBJECTDIR)/%.o: %.S + $(CC) $(CFLAGS) $< -c -o $@ + + +CUSTOM_RULE_C_TO_CO=yes + +%.co: %.c + $(CC) $(CFLAGS) $< -c -o $@ + +CUSTOM_RULE_C_TO_CE=yes + +%.ce: %.o + $(LD) $(LDFLAGS) --relocatable -T $(CONTIKI_CPU)/merge-rodata.ld $< -o $@ + $(STRIP) -K _init -K _fini --strip-unneeded -g -x $@ + +CUSTOM_RULE_LINK=yes + +%-stripped.o: %.c + $(CC) $(CFLAGS) -c $< -o $@ + $(STRIP) --strip-unneeded -g -x $@ + +%-stripped.o: %.o + $(STRIP) --strip-unneeded -g -x -o $@ $< + +%.o: ${CONTIKI_TARGET}/loader/%.S + $(AS) -o $(notdir $(<:.S=.o)) $< + +%-nosyms.$(TARGET): %.co $(PROJECT_OBJECTFILES) contiki-$(TARGET).a # $(OBJECTDIR)/empty-symbols.o + $(CC) $(LDFLAGS) $(CFLAGS) -nostartfiles -o $@ $(filter-out %.a,$^) $(filter %.a,$^) -lc $(filter %.a,$^) $(EXTERN_LIBS) + + +%.ihex: %.$(TARGET) + $(OBJCOPY) $^ -O ihex $@ + +%.bin: %.$(TARGET) + $(OBJCOPY) -O binary $< $@ + +.PHONY: symbols.c +ifdef CORE +%.$(TARGET): %.co $(PROJECT_OBJECTFILES) contiki-$(TARGET).a $(STARTUP) $(OBJECTDIR)/symbols.o + $(CC) $(LDFLAGS) $(CFLAGS) -nostartfiles -o $@ $(filter-out %.a,$^) $(filter %.a,$^) -lc $(filter %.a,$^) + +symbols.c: $(CORE) + $(NM) $< | awk -f $(CONTIKI_CPU)/builtins.awk -f ../../tools/mknmlist > symbols.c + +else +%.$(TARGET): %-nosyms.$(TARGET) + ln -sf $< $@ +endif + +empty-symbols.c: + cp ${CONTIKI}/tools/empty-symbols.c symbols.c + cp ${CONTIKI}/tools/empty-symbols.h symbols.h + +# Don't use core/loader/elfloader.c, use elfloader-otf.c instead +$(OBJECTDIR)/elfloader.o: + echo -n >$@ + +clean: clean_cpu + +.PHONY: stm32test_clean + +clean_cpu: + -rm -rf $(BUILTSRCDIR) + +.PRECIOUS: %-nosyms.$(TARGET) diff --git a/cpu/arm/stm32f1x_cl/STM32F107VCT.ld b/cpu/arm/stm32f1x_cl/STM32F107VCT.ld new file mode 100644 index 000000000..40c396780 --- /dev/null +++ b/cpu/arm/stm32f1x_cl/STM32F107VCT.ld @@ -0,0 +1,54 @@ +MEMORY +{ + CODE (rx) : ORIGIN = 0x08000000, LENGTH = 256K + DATA (xrw) : ORIGIN = 0x20000000, LENGTH = 64K +} + +/* Section Definitions */ + +EXTERN (vector_table) +ENTRY(reset_handler) +SECTIONS +{ + +/* Make sure the vector table is at address 0 */ + + .text : { + *(.vectors) + *(.text*) + . = ALIGN(4); + *(.rodata*) + . = ALIGN(4); + } >CODE + + . = ALIGN(4); + _etext = . ; + PROVIDE (etext = .); + + .data : + { + _data = . ; + *(.data*) + . = ALIGN(4); + _edata = . ; + PROVIDE (_edata = .); + } >DATA AT >CODE + _data_loadaddr = LOADADDR(.data); + +/* .bss section which is used for uninitialized data */ + + .bss : + { + __bss_start = . ; + __bss_start__ = . ; + *(.bss*) + *(COMMON) + . = ALIGN(4); + _ebss = . ; + } >DATA + . = ALIGN(4); + + _end = .; + PROVIDE (_end = .); +} +PROVIDE(_stack = ORIGIN(DATA) + LENGTH(DATA)); diff --git a/cpu/arm/stm32f1x_cl/clock.c b/cpu/arm/stm32f1x_cl/clock.c new file mode 100644 index 000000000..9efaf3e05 --- /dev/null +++ b/cpu/arm/stm32f1x_cl/clock.c @@ -0,0 +1,78 @@ +#include +#include +#include +#include + +#include + +static volatile clock_time_t current_clock = 0; +static volatile unsigned long current_seconds = 0; +static unsigned int second_countdown = CLOCK_SECOND; + +void sys_tick_handler(void) __attribute__ ((interrupt)); + +void +sys_tick_handler(void) +{ + current_clock++; + if(etimer_pending() && etimer_next_expiration_time() <= current_clock) { + etimer_request_poll(); + /* printf("%d,%d\n", clock_time(),etimer_next_expiration_time ()); */ + } + + if(--second_countdown == 0) { + current_seconds++; + second_countdown = CLOCK_SECOND; + } +} + + +void +clock_init() +{ + systick_set_clocksource(STK_CTRL_CLKSOURCE_AHB_DIV8); + + /*72mhz / 8 / 1000 */ + systick_set_reload(MCK / 8 / CLOCK_SECOND); + + systick_interrupt_enable(); + systick_counter_enable(); +} + +clock_time_t +clock_time(void) +{ + return current_clock; +} + +unsigned long +clock_seconds(void) +{ + return current_seconds; +} + +/* TODO: This code needs to be evaluated for the stm32f107 and + * implemented + */ +#if 0 +/* The inner loop takes 4 cycles. The outer 5+SPIN_COUNT*4. */ + +#define SPIN_TIME 2 /* us */ +#define SPIN_COUNT (((MCK*SPIN_TIME/1000000)-5)/4) + +#ifndef __MAKING_DEPS__ + +void +clock_delay(unsigned int t) +{ +#ifdef __THUMBEL__ + asm + volatile + ("1: mov r1,%2\n2:\tsub r1,#1\n\tbne 2b\n\tsub %0,#1\n\tbne 1b\n":"=l" + (t):"0"(t), "l"(SPIN_COUNT)); +#else +#error Must be compiled in thumb mode +#endif +} +#endif +#endif /* __MAKING_DEPS__ */ diff --git a/cpu/arm/stm32f1x_cl/mtarch.h b/cpu/arm/stm32f1x_cl/mtarch.h new file mode 100644 index 000000000..a61c7be97 --- /dev/null +++ b/cpu/arm/stm32f1x_cl/mtarch.h @@ -0,0 +1,13 @@ +/* + * Implementation of multithreading in ARM Cortex-M3. To be done. + */ + + +#ifndef __MTARCH_H__ +#define __MTARCH_H__ + +struct mtarch_thread { + short mt_thread; +}; + +#endif /* __MTARCH_H__ */ \ No newline at end of file diff --git a/cpu/arm/stm32f1x_cl/rtimer-arch.c b/cpu/arm/stm32f1x_cl/rtimer-arch.c new file mode 100644 index 000000000..351ca3afc --- /dev/null +++ b/cpu/arm/stm32f1x_cl/rtimer-arch.c @@ -0,0 +1,16 @@ + +void +rtimer_arch_init(void) +{ +} + +void +rtimer_arch_set(rtimer_clock_t t) +{ +} + +rtimer_clock_t +rtimer_arch_now(void) +{ + return 0; +} diff --git a/cpu/arm/stm32f1x_cl/rtimer-arch.h b/cpu/arm/stm32f1x_cl/rtimer-arch.h new file mode 100644 index 000000000..58a9e22c5 --- /dev/null +++ b/cpu/arm/stm32f1x_cl/rtimer-arch.h @@ -0,0 +1,22 @@ +/** + * \file + * Header file for the STM32F107-specific rtimer code + * \author + * Jeff Ciesielski + * Adapted from stm32f103 example + */ + +#ifndef __RTIMER_ARCH_H__ +#define __RTIMER_ARCH_H__ + +#include "sys/rtimer.h" + +#define RTIMER_ARCH_SECOND (MCK/1024) + +void rtimer_arch_init(void); + +void rtimer_arch_set(rtimer_clock_t t); + +rtimer_clock_t rtimer_arch_now(void); + +#endif /* __RTIMER_ARCH_H__ */ diff --git a/cpu/arm/stm32f1x_cl/uip-log.c b/cpu/arm/stm32f1x_cl/uip-log.c new file mode 100644 index 000000000..52dbd1d29 --- /dev/null +++ b/cpu/arm/stm32f1x_cl/uip-log.c @@ -0,0 +1,7 @@ +#include + +void +uip_log(char *msg) +{ + printf("uip: %s\n", msg); +} diff --git a/cpu/arm/stm32f1x_cl/util.c b/cpu/arm/stm32f1x_cl/util.c new file mode 100644 index 000000000..5c4d49c25 --- /dev/null +++ b/cpu/arm/stm32f1x_cl/util.c @@ -0,0 +1,12 @@ +#include + +uint32_t retval; + +uint32_t +get_msp(void) +{ + asm("ldr r1, =retval"); + asm("mrs r0, msp"); + asm("str r0, [r1]"); + return retval; +} diff --git a/cpu/arm/stm32f1x_cl/util.h b/cpu/arm/stm32f1x_cl/util.h new file mode 100644 index 000000000..114f7c15f --- /dev/null +++ b/cpu/arm/stm32f1x_cl/util.h @@ -0,0 +1,5 @@ +#ifndef _UTIL_H_ +#define _UTIL_H_ +#include +uint32_t get_msp(void); +#endif /*_UTIL_H_*/ diff --git a/platform/stm32f107_basic/Makefile b/platform/stm32f107_basic/Makefile new file mode 100644 index 000000000..625c667f3 --- /dev/null +++ b/platform/stm32f107_basic/Makefile @@ -0,0 +1,26 @@ +TARGET=stm32f107_basic + +all: stm32f107_basic + + +CONTIKI=../.. + +CONTIKI_TARGET_MAIN=contiki-main.c + +PROJECT_SOURCEFILES = parity.c + + +randgen: randgen.c + gcc -DNDEBUG -I $(CONTIKI)/cpu/arm/stm32f107/ -I . -I $(CONTIKI)/core randgen.c -o randgen + +randcheck: randcheck.c + gcc -DNDEBUG -I $(CONTIKI)/cpu/arm/stm32f107/ -I . -I $(CONTIKI)/core randcheck.c -o randcheck + +clean: stm32test_clean + +.PHONY: stm32test_clean + +stm32test_clean: + +include $(CONTIKI)/Makefile.include + diff --git a/platform/stm32f107_basic/Makefile.stm32f107_basic b/platform/stm32f107_basic/Makefile.stm32f107_basic new file mode 100644 index 000000000..f7a9cfd8b --- /dev/null +++ b/platform/stm32f107_basic/Makefile.stm32f107_basic @@ -0,0 +1,20 @@ +CONTIKI_TARGET_DIRS = . dev +# Master clock frequency +MCK=72000000 +CFLAGS+=-DAUTOSTART_ENABLE + +ifndef CONTIKI_TARGET_MAIN +CONTIKI_TARGET_MAIN = contiki-main.c +endif + +CONTIKI_TARGET_SOURCEFILES += $(CONTIKI_TARGET_MAIN) +CONTIKI_TARGET_SOURCEFILES += debug-uart.c newlib.c gqueue.c + +include $(CONTIKI)/cpu/arm/stm32f1x_cl/Makefile.stm32f1x_cl + +contiki-$(TARGET).a: ${addprefix $(OBJECTDIR)/,symbols.o} + +ifndef BASE_IP +BASE_IP := 172.16.1.1 +endif + diff --git a/platform/stm32f107_basic/contiki-conf.h b/platform/stm32f107_basic/contiki-conf.h new file mode 100644 index 000000000..883ebf4fe --- /dev/null +++ b/platform/stm32f107_basic/contiki-conf.h @@ -0,0 +1,46 @@ +#ifndef __CONTIKI_CONF_H__CDBB4VIH3I__ +#define __CONTIKI_CONF_H__CDBB4VIH3I__ + +#include + +#define CCIF +#define CLIF + +#define WITH_UIP 1 +#define WITH_ASCII 1 + +#define CLOCK_CONF_SECOND 1000 + +/* These names are deprecated, use C99 names. */ +typedef uint8_t u8_t; +typedef uint16_t u16_t; +typedef uint32_t u32_t; +typedef int8_t s8_t; +typedef int16_t s16_t; +typedef int32_t s32_t; + +typedef unsigned int clock_time_t; +typedef unsigned int uip_stats_t; + +#ifndef BV +#define BV(x) (1<<(x)) +#endif + +/* uIP configuration */ +#define UIP_CONF_LLH_LEN 0 +#define UIP_CONF_BROADCAST 1 +#define UIP_CONF_LOGGING 1 +#define UIP_CONF_BUFFER_SIZE 116 + +#define UIP_CONF_TCP_FORWARD 1 + +/* Prefix for relocation sections in ELF files */ +#define REL_SECT_PREFIX ".rel" + +#define CC_BYTE_ALIGNED __attribute__ ((packed, aligned(1))) + +#define USB_EP1_SIZE 64 +#define USB_EP2_SIZE 64 + +#define RAND_MAX 0x7fff +#endif /* __CONTIKI_CONF_H__CDBB4VIH3I__ */ diff --git a/platform/stm32f107_basic/contiki-main.c b/platform/stm32f107_basic/contiki-main.c new file mode 100644 index 000000000..6508f42a5 --- /dev/null +++ b/platform/stm32f107_basic/contiki-main.c @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +unsigned int idle_count = 0; + +static void +configure_mcu_clocks(void) +{ + rcc_clock_setup_in_hse_25mhz_out_72mhz(); + + /* GPIO Clocks */ + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN); + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_AFIOEN); + + /* USART 1 */ + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_USART1EN); +} + +int +main() +{ + configure_mcu_clocks(); + uart_init(115200); + printf("Initialising\n"); + + clock_init(); + process_init(); + process_start(&etimer_process, NULL); + autostart_start(autostart_processes); + printf("Processes running\n"); + while(1) { + do { + } while(process_run() > 0); + idle_count++; + } + return 0; +} diff --git a/platform/stm32f107_basic/dev/debug-uart.c b/platform/stm32f107_basic/dev/debug-uart.c new file mode 100644 index 000000000..381241f29 --- /dev/null +++ b/platform/stm32f107_basic/dev/debug-uart.c @@ -0,0 +1,95 @@ +#include +#include +#include + +#include +#include +#include +#include + +DECLARE_QUEUE(char, uart_tx_buf, 256); +DECLARE_QUEUE(char, uart_rx_buf, 256); + +static void uart_init_gpio(void) +{ + gpio_set_mode(GPIOA, + GPIO_MODE_OUTPUT_50_MHZ, + GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, + GPIO9); + + gpio_set_mode(GPIOA, + GPIO_MODE_INPUT, + GPIO_CNF_INPUT_FLOAT, + GPIO10); + + /* USART lines should idle high */ + gpio_set(GPIOA, GPIO9); + gpio_set(GPIOA, GPIO10); +} + +void uart_init(int baud) +{ + uart_init_gpio(); + + nvic_enable_irq(NVIC_USART1_IRQ); + nvic_set_priority(NVIC_USART1_IRQ, 2); + + usart_set_baudrate(USART1, baud); + usart_set_databits(USART1, 8); + usart_set_parity(USART1, USART_PARITY_NONE); + usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE); + usart_set_stopbits(USART1, USART_CR2_STOPBITS_1); + usart_set_mode(USART1, USART_MODE_TX_RX); + usart_enable_rx_interrupt(USART1); + usart_enable(USART1); + + /* This ensures stdio doesn't use its own buffers */ + setvbuf(stdin, NULL, _IONBF, 0); + setvbuf(stdout, NULL, _IONBF, 0); +} + +int uart_putchar(char c) +{ + if( c == '\n') + uart_putchar('\r'); + if(!queue_is_full(&uart_tx_buf)) { + queue_enqueue(&uart_tx_buf, &c); + } else { + return -ENOMEM; + } + + usart_enable_tx_interrupt(USART1); + + return 0; +} + +int uart_getchar(char *c) +{ + if(!queue_is_empty(&uart_rx_buf)) { + queue_dequeue(&uart_rx_buf, c); + return 0; + } else { + return -ENODATA; + } +} + +void usart1_isr(void) +{ + char c; + if (usart_get_flag(USART1, USART_SR_TXE)) { + if (!queue_is_empty(&uart_tx_buf)) { + queue_dequeue(&uart_tx_buf, &c); + usart_send(USART1, (uint16_t)c); + } else { + usart_disable_tx_interrupt(USART1); + } + } + + + if (usart_get_flag(USART1, USART_SR_RXNE)) { + if (!queue_is_full(&uart_rx_buf)) { + c = usart_recv(USART1); + queue_enqueue(&uart_rx_buf, &c); + } + } +} diff --git a/platform/stm32f107_basic/dev/debug-uart.h b/platform/stm32f107_basic/dev/debug-uart.h new file mode 100644 index 000000000..1808880a9 --- /dev/null +++ b/platform/stm32f107_basic/dev/debug-uart.h @@ -0,0 +1,14 @@ +/** + * USART driver for STM32F1xx w/ libopencm3 peripherl lib + * (c) 2012 Blessed Contraptions + * Jeff Ciesielski + */ + +#ifndef _UART_H_ +#define _UART_H_ + +int uart_putchar(char c); +int uart_getchar(char *c); +void uart_init(int baud); + +#endif diff --git a/platform/stm32f107_basic/gqueue.c b/platform/stm32f107_basic/gqueue.c new file mode 100644 index 000000000..1f7e29a47 --- /dev/null +++ b/platform/stm32f107_basic/gqueue.c @@ -0,0 +1,38 @@ +#include + +void +queue_enqueue(volatile void *q, const void *elt) +{ + volatile struct generic_queue *gq = q; + + if(gq->len == 0) { + gq->head = gq->memory; + gq->tail = gq->memory; + } else { + if(gq->tail == gq->memory + (gq->max_capacity - 1) * gq->item_size) + /* FIXME: Should something be done about + * queue length? Substact one, maybe?*/ + gq->tail = gq->memory; + else + gq->tail = (uint8_t *) gq->tail + gq->item_size; + } + + memcpy((void *)gq->tail, elt, gq->item_size); + gq->len++; +} + +void +queue_dequeue(volatile void *q, void *elt) +{ + volatile struct generic_queue *gq = q; + + memcpy(elt, (void *)gq->head, gq->item_size); + + if(gq->head == gq->memory + (gq->max_capacity - 1) * gq->item_size) + /* FIXME: Should something be done about + * queue length? Substact one, maybe?*/ + gq->head = gq->memory; + else if(gq->len > 1) + gq->head = (uint8_t *) gq->head + gq->item_size; + gq->len--; +} diff --git a/platform/stm32f107_basic/gqueue.h b/platform/stm32f107_basic/gqueue.h new file mode 100644 index 000000000..8efef6c14 --- /dev/null +++ b/platform/stm32f107_basic/gqueue.h @@ -0,0 +1,73 @@ +/************************************************************************************/ +/* The MIT License (MIT) */ +/* */ +/* Copyright (c) 2012 Jeff Ciesielski */ +/* Andrey Smirnov */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining a copy of */ +/* this software and associated documentation files (the "Software"), to deal in */ +/* the Software without restriction, including without limitation the rights to */ +/* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies */ +/* of the Software, and to permit persons to whom the Software is furnished to do */ +/* so, subject to the following conditions: */ +/* */ +/* -The above copyright notice and this permission notice shall be included in all */ +/* copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR */ +/* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS */ +/* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR */ +/* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER */ +/* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/************************************************************************************/ + +#ifndef _QUEUE_H_ +#define _QUEUE_H_ + +#include +#include +#include + +struct generic_queue { + volatile void *head; + volatile void *tail; + size_t item_size; + size_t len; + size_t max_capacity; + volatile uint8_t memory[0]; +}; + +#define DECLARE_QUEUE(element_type, name, max_size) \ + struct name { \ + struct generic_queue gq; \ + element_type __elements[max_size]; \ + } name = { \ + .gq={ \ + .len = 0, \ + .item_size = sizeof(element_type), \ + .max_capacity = max_size, \ + }, \ + } + + +static inline bool queue_is_empty(volatile void *q) +{ + volatile struct generic_queue *gq = q; + + return (gq->len == 0); +} +static inline int queue_get_len(volatile void *q) +{ + volatile struct generic_queue *gq = q; + return gq->len; +} +static inline bool queue_is_full(volatile void *q) +{ + volatile struct generic_queue *gq = q; + return (gq->len >= gq->max_capacity); +} +void queue_enqueue(volatile void *q, const void *elt) __attribute__((nonnull)); +void queue_dequeue(volatile void *q, void *elt) __attribute__((nonnull)); + +#endif diff --git a/platform/stm32f107_basic/newlib.c b/platform/stm32f107_basic/newlib.c new file mode 100644 index 000000000..0edbd64c1 --- /dev/null +++ b/platform/stm32f107_basic/newlib.c @@ -0,0 +1,199 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define STDOUT_USART USART1 +#define STDERR_USART USART1 +#define STDIN_USART USART1 +#undef errno +extern int errno; + +char *__env[1] = { 0 }; +char **environ = __env; + +int _write(int file, char *ptr, int len); + +void +_exit(int status) +{ + while(1); +} + +int +_close(int file) +{ + return -1; +} + +int +_execve(char *name, char **argv, char **env) +{ + errno = ENOMEM; + return -1; +} + +int +_fork() +{ + errno = EAGAIN; + return -1; +} + +int +_fstat(int file, struct stat *st) +{ + st->st_mode = S_IFCHR; + return 0; +} + +int +_getpid() +{ + return 1; +} + +int +_gettimeofday(struct timeval *tv, struct timezone *tz) +{ + tv->tv_sec = rtc_get_counter_val(); + tv->tv_usec = 0; + return 0; +} + +int +_isatty(int file) +{ + switch (file) { + case STDOUT_FILENO: + case STDERR_FILENO: + case STDIN_FILENO: + return 1; + default: + //errno = ENOTTY; + errno = EBADF; + return 0; + } +} + +int +_kill(int pid, int sig) +{ + errno = EINVAL; + return (-1); +} + +int +_link(char *old, char *new) +{ + errno = EMLINK; + return -1; +} + +int +_lseek(int file, int ptr, int dir) +{ + return 0; +} + +caddr_t +_sbrk(int incr) +{ + extern char _ebss; + static char *heap_end; + char *prev_heap_end; + + if(heap_end == 0) { + heap_end = &_ebss; + } + prev_heap_end = heap_end; + + char *stack = (char *)get_msp(); + + if(heap_end + incr > stack) { + _write(STDERR_FILENO, "Heap and stack collision", 25); + errno = ENOMEM; + return (caddr_t) - 1; + //abort (); + } + + heap_end += incr; + return (caddr_t) prev_heap_end; + +} + +int +_read(int file, char *ptr, int len) +{ + char c = 0x00; + + switch (file) { + case STDIN_FILENO: + uart_getchar(&c); + *ptr++ = c; + return 1; + break; + default: + errno = EBADF; + return -1; + } +} + +int +_stat(const char *filepath, struct stat *st) +{ + st->st_mode = S_IFCHR; + return 0; +} + +clock_t +_times(struct tms * buf) +{ + return -1; +} + +int +_unlink(char *name) +{ + errno = ENOENT; + return -1; +} + +int +_wait(int *status) +{ + errno = ECHILD; + return -1; +} + +int +_write(int file, char *ptr, int len) +{ + int n; + char c; + + switch (file) { + case STDOUT_FILENO: /*stdout */ + for(n = 0; n < len; n++) { + c = (uint8_t) * ptr++; + uart_putchar(c); + } + break; + case STDERR_FILENO: /* stderr */ + for(n = 0; n < len; n++) { + c = (uint8_t) * ptr++; + uart_putchar(c); + } + break; + default: + errno = EBADF; + return -1; + } + return len; +} diff --git a/platform/stm32f107_basic/structgen_opts.gen.h b/platform/stm32f107_basic/structgen_opts.gen.h new file mode 100644 index 000000000..f0a459008 --- /dev/null +++ b/platform/stm32f107_basic/structgen_opts.gen.h @@ -0,0 +1,2 @@ +#pragma target_endian little +#pragma max_target_int_bytes 8