From 13cd5cc76d6db6358f7834ba6b46ea7ab424c887 Mon Sep 17 00:00:00 2001 From: Eyck Jentzsch Date: Sat, 13 Jan 2024 23:06:01 +0100 Subject: [PATCH] adds ehrenberg platform --- env/common-gcc.mk | 22 ++-- env/ehrenberg/.gitignore | 1 + env/ehrenberg/init.c | 140 +++++++++++++++++++++ env/ehrenberg/link.lds | 173 ++++++++++++++++++++++++++ env/ehrenberg/platform.h | 119 ++++++++++++++++++ include/ehrenberg/const.h | 18 +++ include/ehrenberg/devices.h | 37 ++++++ include/ehrenberg/devices/gpio.h | 17 +++ include/ehrenberg/devices/interrupt.h | 16 +++ include/ehrenberg/devices/qspi.h | 112 +++++++++++++++++ include/ehrenberg/devices/timer.h | 52 ++++++++ include/ehrenberg/devices/uart.h | 67 ++++++++++ libwrap/sys/printf.c | 64 ++++------ libwrap/sys/puts.c | 6 +- libwrap/sys/read.c | 20 ++- libwrap/sys/sbrk.c | 2 +- libwrap/sys/write.c | 34 +++-- 17 files changed, 829 insertions(+), 71 deletions(-) create mode 100644 env/ehrenberg/.gitignore create mode 100644 env/ehrenberg/init.c create mode 100644 env/ehrenberg/link.lds create mode 100644 env/ehrenberg/platform.h create mode 100644 include/ehrenberg/const.h create mode 100644 include/ehrenberg/devices.h create mode 100644 include/ehrenberg/devices/gpio.h create mode 100644 include/ehrenberg/devices/interrupt.h create mode 100644 include/ehrenberg/devices/qspi.h create mode 100644 include/ehrenberg/devices/timer.h create mode 100644 include/ehrenberg/devices/uart.h diff --git a/env/common-gcc.mk b/env/common-gcc.mk index 9e724c5..a74fb24 100644 --- a/env/common-gcc.mk +++ b/env/common-gcc.mk @@ -1,8 +1,10 @@ ifndef _MK_COMMON _MK_COMMON := # defined -.PHONY: all -all: $(TARGET) +TL_TARGET?=all + +.PHONY: $(TL_TARGET) +$(TL_TARGET): $(TARGET) include $(BSP_BASE)/libwrap/libwrap.mk @@ -14,17 +16,17 @@ ASM_SRCS += $(ENV_DIR)/start.S ASM_SRCS += $(ENV_DIR)/entry.S C_SRCS += $(PLATFORM_DIR)/init.c -LINKER_SCRIPT := $(PLATFORM_DIR)/$(LINK_TARGET).lds +LINKER_SCRIPT ?= $(PLATFORM_DIR)/$(LINK_TARGET).lds INCLUDES += -I$(BSP_BASE)/include INCLUDES += -I$(BSP_BASE)/drivers/ INCLUDES += -I$(ENV_DIR) INCLUDES += -I$(PLATFORM_DIR) -TOOL_DIR ?= $(BSP_BASE)/../toolchain/bin/ - LDFLAGS += -march=$(RISCV_ARCH) -mabi=$(RISCV_ABI) +ifndef NO_DEFAULT_LINK LDFLAGS += -T $(LINKER_SCRIPT) -Wl,--no-warn-rwx-segments -Wl,-Map=$(TARGET).map -nostartfiles +endif LDFLAGS += -L$(ENV_DIR) # --specs=nano.specs @@ -53,12 +55,14 @@ CC=$(TOOL_DIR)$(TRIPLET)-gcc LD=$(TOOL_DIR)$(TRIPLET)-gcc AR=$(TOOL_DIR)$(TRIPLET)-ar OBJDUMP := $(TOOL_DIR)$(TRIPLET)-objdump +OBJCOPY := $(TOOL_DIR)$(TRIPLET)-objcopy - -$(TARGET): $(LINK_OBJS) $(LINK_DEPS) +ifndef NO_DEFAULT_LINK +$(TARGET).elf: $(LINK_OBJS) $(LINK_DEPS) $(LD) $(LINK_OBJS) $(LDFLAGS) $(LIBWRAP) -o $@ - $(OBJDUMP) -d -S $(TARGET) > $(TARGET).dis - + $(OBJDUMP) -d -S $@ > $(TARGET).dis +endif + $(ASM_OBJS): %.o: %.S $(HEADERS) $(CC) $(CFLAGS) $(INCLUDES) -c -o $@ $< diff --git a/env/ehrenberg/.gitignore b/env/ehrenberg/.gitignore new file mode 100644 index 0000000..cc5bb74 --- /dev/null +++ b/env/ehrenberg/.gitignore @@ -0,0 +1 @@ +/*.o diff --git a/env/ehrenberg/init.c b/env/ehrenberg/init.c new file mode 100644 index 0000000..e1fc54e --- /dev/null +++ b/env/ehrenberg/init.c @@ -0,0 +1,140 @@ +#include +#include +#include + +#include "platform.h" +#include "encoding.h" +#include + +#if __riscv_xlen == 32 +#define MCAUSE_INT 0x80000000UL +#define MCAUSE_CAUSE 0x000003FFUL +#else +#define MCAUSE_INT 0x8000000000000000UL +#define MCAUSE_CAUSE 0x00000000000003FFUL +#endif + +extern int main(int argc, char** argv); +extern void trap_entry(); +#define IRQ_M_SOFT 3 +#define IRQ_M_TIMER 7 +#define IRQ_M_EXT 11 + +#define NUM_INTERRUPTS 16 +#define MTIMER_NEXT_TICK_INC 1000 + +void handle_m_ext_interrupt(void); +void handle_m_time_interrupt(void); +uint32_t handle_trap(uint32_t mcause, uint32_t mepc, uint32_t sp); +void default_handler(void); +void _init(void); + +typedef void (*my_interrupt_function_ptr_t) (void); +my_interrupt_function_ptr_t localISR[NUM_INTERRUPTS] __attribute__((aligned(64))); + +static unsigned long mtime_lo(void) +{ + unsigned long ret; + __asm volatile("rdtime %0":"=r"(ret)); + return ret; +} + + +#if __riscv_xlen==32 + +static uint32_t mtime_hi(void) +{ + unsigned long ret; + __asm volatile("rdtimeh %0":"=r"(ret)); + return ret; +} + +uint64_t get_timer_value() +{ + while (1) { + uint32_t hi = mtime_hi(); + uint32_t lo = mtime_lo(); + if (hi == mtime_hi()) + return ((uint64_t)hi << 32) | lo; + } +} + +#elif __riscv_xlen==64 + +uint64_t get_timer_value() +{ + return mtime_lo(); +} + +#endif + +unsigned long get_timer_freq() +{ + return 32768; +} + +unsigned long get_cpu_freq() +{ + return 100000000; +} + +void init_pll(void){ + //TODO: implement initialization +} + +static void uart_init(size_t baud_rate) +{ + //TODO: implement initialization +} + +void __attribute__((weak)) handle_m_ext_interrupt(){ +} + +void __attribute__((weak)) handle_m_time_interrupt(){ + uint64_t time = ((uint64_t)mtimer->mtimeh)<<32 || mtimer->mtime; + time+=MTIMER_NEXT_TICK_INC; + mtimer->mtimecmph = time>>32; + mtimer->mtimecmp = time; +} + +void __attribute__((weak)) default_handler(void) { + puts("default handler\n"); +} + +uint32_t handle_trap(uint32_t mcause, uint32_t mepc, uint32_t sp){ + if ((mcause & MCAUSE_INT) && ((mcause & MCAUSE_CAUSE) == IRQ_M_EXT)) { + handle_m_ext_interrupt(); + // External Machine-Level interrupt from PLIC + } else if ((mcause & MCAUSE_INT) && ((mcause & MCAUSE_CAUSE) == IRQ_M_TIMER)){ + handle_m_time_interrupt(); + } else { + write(1, "trap\n", 5); + _exit(1 + mcause); + } + return mepc; +} + +void _init() +{ + +#ifndef NO_INIT + init_pll(); + uart_init(115200); + printf("core freq at %d Hz\n", get_cpu_freq()); + write_csr(mtvec, &trap_entry); + if (read_csr(misa) & (1 << ('F' - 'A'))) { // if F extension is present + write_csr(mstatus, MSTATUS_FS); // allow FPU instructions without trapping + write_csr(fcsr, 0); // initialize rounding mode, undefined at reset + } + int i=0; + while(iflash AT>flash :flash + + .text : + { + *(.text.unlikely .text.unlikely.*) + *(.text.startup .text.startup.*) + *(.text .text.*) + *(.gnu.linkonce.t.*) + } >flash AT>flash :flash + + .fini : + { + KEEP (*(SORT_NONE(.fini))) + } >flash AT>flash :flash + + PROVIDE (__etext = .); + PROVIDE (_etext = .); + PROVIDE (etext = .); + + .rodata : + { + *(.rdata) + *(.rodata .rodata.*) + *(.gnu.linkonce.r.*) + } >flash AT>flash :flash + + . = ALIGN(4); + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >flash AT>flash :flash + + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT_BY_INIT_PRIORITY(.init_array.*) SORT_BY_INIT_PRIORITY(.ctors.*))) + KEEP (*(.init_array EXCLUDE_FILE (*crtbegin.o *crtbegin?.o *crtend.o *crtend?.o ) .ctors)) + PROVIDE_HIDDEN (__init_array_end = .); + } >flash AT>flash :flash + + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT_BY_INIT_PRIORITY(.fini_array.*) SORT_BY_INIT_PRIORITY(.dtors.*))) + KEEP (*(.fini_array EXCLUDE_FILE (*crtbegin.o *crtbegin?.o *crtend.o *crtend?.o ) .dtors)) + PROVIDE_HIDDEN (__fini_array_end = .); + } >flash AT>flash :flash + + .ctors : + { + /* gcc uses crtbegin.o to find the start of + the constructors, so we make sure it is + first. Because this is a wildcard, it + doesn't matter if the user does not + actually link against crtbegin.o; the + linker won't look for a file to match a + wildcard. The wildcard also means that it + doesn't matter which directory crtbegin.o + is in. */ + KEEP (*crtbegin.o(.ctors)) + KEEP (*crtbegin?.o(.ctors)) + /* We don't want to include the .ctor section from + the crtend.o file until after the sorted ctors. + The .ctor section from the crtend file contains the + end of ctors marker and it must be last */ + KEEP (*(EXCLUDE_FILE (*crtend.o *crtend?.o ) .ctors)) + KEEP (*(SORT(.ctors.*))) + KEEP (*(.ctors)) + } >flash AT>flash :flash + + .dtors : + { + KEEP (*crtbegin.o(.dtors)) + KEEP (*crtbegin?.o(.dtors)) + KEEP (*(EXCLUDE_FILE (*crtend.o *crtend?.o ) .dtors)) + KEEP (*(SORT(.dtors.*))) + KEEP (*(.dtors)) + } >flash AT>flash :flash + + .lalign : + { + . = ALIGN(4); + PROVIDE( _data_lma = . ); + } >flash AT>flash :flash + + .dalign : + { + . = ALIGN(4); + PROVIDE( _data = . ); + } >ram AT>flash :ram_init + + .data : + { + __DATA_BEGIN__ = .; + *(.data .data.*) + *(.gnu.linkonce.d.*) + } >ram AT>flash :ram_init + + .srodata : + { + *(.srodata.cst16) + *(.srodata.cst8) + *(.srodata.cst4) + *(.srodata.cst2) + *(.srodata .srodata.*) + } >ram AT>flash :ram_init + + .sdata : + { + __SDATA_BEGIN__ = .; + *(.sdata .sdata.*) + *(.gnu.linkonce.s.*) + } >ram AT>flash :ram_init + + . = ALIGN(4); + PROVIDE( _edata = . ); + PROVIDE( edata = . ); + + PROVIDE( _fbss = . ); + PROVIDE( __bss_start = . ); + .bss : + { + *(.sbss*) + *(.gnu.linkonce.sb.*) + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + } >ram AT>ram :ram + + . = ALIGN(8); + __BSS_END__ = .; + __global_pointer$ = MIN(__SDATA_BEGIN__ + 0x800, MAX(__DATA_BEGIN__ + 0x800, __BSS_END__ - 0x800)); + PROVIDE( _end = . ); + PROVIDE( end = . ); + + .stack ORIGIN(ram) + LENGTH(ram) - __stack_size : + { + PROVIDE( _heap_end = . ); + . = __stack_size; + PROVIDE( _sp = . ); + } >ram AT>ram :ram + + PROVIDE( tohost = 0xfffffff0 ); + PROVIDE( fromhost = 0xfffffff8 ); +} diff --git a/env/ehrenberg/platform.h b/env/ehrenberg/platform.h new file mode 100644 index 0000000..7da9f8f --- /dev/null +++ b/env/ehrenberg/platform.h @@ -0,0 +1,119 @@ +// See LICENSE for license details. + +#ifndef _ISS_PLATFORM_H +#define _ISS_PLATFORM_H + +// Some things missing from the official encoding.h +#define MCAUSE_INT 0x80000000 +#define MCAUSE_CAUSE 0x7FFFFFFF + +#include "ehrenberg/const.h" +#include "ehrenberg/devices.h" + +/**************************************************************************** + * Platform definitions + *****************************************************************************/ + +// Memory map +#define MASKROM_BASE_ADDR _AC(0x00001000,UL) +#define TRAPVEC_TABLE_BASE_ADDR _AC(0x00001010,UL) +#define OTP_MMAP_ADDR _AC(0x00020000,UL) +#define CLINT_BASE_ADDR _AC(0x02000000,UL) +#define PLIC_BASE_ADDR _AC(0x0C000000,UL) +#define AON_BASE_ADDR _AC(0x10000000,UL) +#define PRCI_BASE_ADDR _AC(0x10008000,UL) +#define OTP_BASE_ADDR _AC(0x10010000,UL) +#define GPIO_BASE_ADDR _AC(0x10012000,UL) +#define UART0_BASE_ADDR _AC(0x10013000,UL) +#define SPI0_BASE_ADDR _AC(0x10014000,UL) +#define PWM0_BASE_ADDR _AC(0x10015000,UL) +#define UART1_BASE_ADDR _AC(0x10023000,UL) +#define SPI1_BASE_ADDR _AC(0x10024000,UL) +#define PWM1_BASE_ADDR _AC(0x10025000,UL) +#define SPI2_BASE_ADDR _AC(0x10034000,UL) +#define PWM2_BASE_ADDR _AC(0x10035000,UL) +#define SPI0_MMAP_ADDR _AC(0x20000000,UL) +#define MEM_BASE_ADDR _AC(0x80000000,UL) + +// IOF masks +#define IOF0_SPI1_MASK _AC(0x000007FC,UL) +#define SPI11_NUM_SS (4) +#define IOF_SPI1_SS0 (2u) +#define IOF_SPI1_SS1 (8u) +#define IOF_SPI1_SS2 (9u) +#define IOF_SPI1_SS3 (10u) +#define IOF_SPI1_MOSI (3u) +#define IOF_SPI1_MISO (4u) +#define IOF_SPI1_SCK (5u) +#define IOF_SPI1_DQ0 (3u) +#define IOF_SPI1_DQ1 (4u) +#define IOF_SPI1_DQ2 (6u) +#define IOF_SPI1_DQ3 (7u) + +#define IOF0_SPI2_MASK _AC(0xFC000000,UL) +#define SPI2_NUM_SS (1) +#define IOF_SPI2_SS0 (26u) +#define IOF_SPI2_MOSI (27u) +#define IOF_SPI2_MISO (28u) +#define IOF_SPI2_SCK (29u) +#define IOF_SPI2_DQ0 (27u) +#define IOF_SPI2_DQ1 (28u) +#define IOF_SPI2_DQ2 (30u) +#define IOF_SPI2_DQ3 (31u) + +//#define IOF0_I2C_MASK _AC(0x00003000,UL) + +#define IOF0_UART0_MASK _AC(0x00030000, UL) +#define IOF_UART0_RX (16u) +#define IOF_UART0_TX (17u) + +#define IOF0_UART1_MASK _AC(0x03000000, UL) +#define IOF_UART1_RX (24u) +#define IOF_UART1_TX (25u) + +#define IOF1_PWM0_MASK _AC(0x0000000F, UL) +#define IOF1_PWM1_MASK _AC(0x00780000, UL) +#define IOF1_PWM2_MASK _AC(0x00003C00, UL) + +// Interrupt numbers +#define INT_RESERVED 0 +#define INT_WDOGCMP 1 +#define INT_RTCCMP 2 +#define INT_UART0_BASE 3 +#define INT_UART1_BASE 4 +#define INT_SPI0_BASE 5 +#define INT_SPI1_BASE 6 +#define INT_SPI2_BASE 7 +#define INT_GPIO_BASE 8 +#define INT_PWM0_BASE 40 +#define INT_PWM1_BASE 44 +#define INT_PWM2_BASE 48 + +// Helper functions +#define _REG32(p, i) (*(volatile uint32_t *) ((p) + (i))) +#define _REG32P(p, i) ((volatile uint32_t *) ((p) + (i))) +#define AON_REG(offset) _REG32(AON_BASE_ADDR, offset) +#define CLINT_REG(offset) _REG32(CLINT_BASE_ADDR, offset) +#define GPIO_REG(offset) _REG32(GPIO_BASE_ADDR, offset) +#define OTP_REG(offset) _REG32(OTP_BASE_ADDR, offset) +#define PLIC_REG(offset) _REG32(PLIC_BASE_ADDR, offset) +#define PRCI_REG(offset) _REG32(PRCI_BASE_ADDR, offset) +#define PWM0_REG(offset) _REG32(PWM0_BASE_ADDR, offset) +#define PWM1_REG(offset) _REG32(PWM1_BASE_ADDR, offset) +#define PWM2_REG(offset) _REG32(PWM2_BASE_ADDR, offset) +#define SPI0_REG(offset) _REG32(SPI0_BASE_ADDR, offset) +#define SPI1_REG(offset) _REG32(SPI1_BASE_ADDR, offset) +#define SPI2_REG(offset) _REG32(SPI2_BASE_ADDR, offset) +#define UART0_REG(offset) _REG32(UART0_BASE_ADDR, offset) +#define UART1_REG(offset) _REG32(UART1_BASE_ADDR, offset) + +// Misc + +#include + +void init_pll(void); +unsigned long get_cpu_freq(void); +unsigned long get_timer_freq(void); +uint64_t get_timer_value(void); + +#endif /* _ISS_PLATFORM_H */ diff --git a/include/ehrenberg/const.h b/include/ehrenberg/const.h new file mode 100644 index 0000000..8dcffbb --- /dev/null +++ b/include/ehrenberg/const.h @@ -0,0 +1,18 @@ +// See LICENSE for license details. +/* Derived from */ + +#ifndef _SIFIVE_CONST_H +#define _SIFIVE_CONST_H + +#ifdef __ASSEMBLER__ +#define _AC(X,Y) X +#define _AT(T,X) X +#else +#define _AC(X,Y) (X##Y) +#define _AT(T,X) ((T)(X)) +#endif /* !__ASSEMBLER__*/ + +#define _BITUL(x) (_AC(1,UL) << (x)) +#define _BITULL(x) (_AC(1,ULL) << (x)) + +#endif /* _SIFIVE_CONST_H */ diff --git a/include/ehrenberg/devices.h b/include/ehrenberg/devices.h new file mode 100644 index 0000000..c18f422 --- /dev/null +++ b/include/ehrenberg/devices.h @@ -0,0 +1,37 @@ +/* + * devices.c + * + * Created on: Aug 15, 2020 + * Author: eyck + */ + +#ifndef _BSP_EHRENBERG_DEVICES_C_ +#define _BSP_EHRENBERG_DEVICES_C_ + +#define APB_BUS + +#include "devices/gpio.h" +#include "devices/interrupt.h" +#include "devices/timer.h" +#include "devices/uart.h" +#include "devices/qspi.h" + +#define PERIPH(TYPE, ADDR) ((volatile TYPE*) (ADDR)) + +#define APB_BASE 0xF0000000 +#define TIMER_BASE (APB_BASE+0x30000) + +#define gpio_a PERIPH(gpio_t, APB_BASE+0x00000) +//#define gpio_b PERIPH(gpio_t, APB_BASE+0x10000) +#define uart PERIPH(uart_t, APB_BASE+0x10000) +#define prescaler PERIPH(uart_t, TIMER_BASE+0x0) +#define timer_a PERIPH(uart_t, TIMER_BASE+0x10) +#define timer_b PERIPH(uart_t, TIMER_BASE+0x20) +#define mtimer PERIPH(mtimer_t, APB_BASE+0x30000) +#define irq PERIPH(irq_t, APB_BASE+0x40000) +#define qspi PERIPH(qspi_t, APB_BASE+0x50000) +//volatile qspi_t* const qspi = (qspi_t*)(APB_BASE+0x50000); + +#define XIP_START_LOC 0xE0040000 + +#endif /* _BSP_EHRENBERG_DEVICES_C_ */ diff --git a/include/ehrenberg/devices/gpio.h b/include/ehrenberg/devices/gpio.h new file mode 100644 index 0000000..cabf657 --- /dev/null +++ b/include/ehrenberg/devices/gpio.h @@ -0,0 +1,17 @@ +#ifndef _BSP_GPIO_H +#define _BSP_GPIO_H + +#include + +typedef struct __attribute((__packed__)) { + volatile uint32_t pin_in; + volatile uint32_t pin_out; + volatile uint32_t out_en; +} gpio_t; + +inline void gpio_init(gpio_t* reg) { + reg->out_en=0; + reg->pin_out=0; +} + +#endif /* _BSP_GPIO_H */ diff --git a/include/ehrenberg/devices/interrupt.h b/include/ehrenberg/devices/interrupt.h new file mode 100644 index 0000000..b395f14 --- /dev/null +++ b/include/ehrenberg/devices/interrupt.h @@ -0,0 +1,16 @@ +#ifndef _BSP_INTERRUPT_H +#define _BSP_INTERRUPT_H + +#include + +typedef struct __attribute((__packed__)) { + volatile uint32_t ip; + volatile uint32_t ie; +} irq_t; + +inline void irq_init(irq_t* reg){ + reg->ie = 0; + reg->ip = 0xFFFFFFFF; +} + +#endif /* _BSP_INTERRUPT_H */ diff --git a/include/ehrenberg/devices/qspi.h b/include/ehrenberg/devices/qspi.h new file mode 100644 index 0000000..f4f11bc --- /dev/null +++ b/include/ehrenberg/devices/qspi.h @@ -0,0 +1,112 @@ +#ifndef _BSP_QSPI_H +#define _BSP_QSPI_H + +#include + +#define __IO volatile + +typedef struct { + __IO uint32_t data; // 0x0/0: data, 8bits, 8:write, 9:read, 11:data/ctrl, 31:rxdata valid + __IO uint32_t status; // 0x4/0: txavail, 16: rxused + __IO uint32_t config; // 0x8/0:1 cpol/cpha, 4: transfer mode (0-FullDuplex) + __IO uint32_t intr; // 0xc/0: txien, 1: rxien, 8: txip, 9: rxip, 16: valid? + __IO uint32_t __fill0[4]; + __IO uint32_t clk_divider; // 0x20/0: sclkToogle + // ssGen config + __IO uint32_t ss_setup; // 0x24/0: setup + __IO uint32_t ss_hold; // 0x28/0: hold + __IO uint32_t ss_disable; // 0x2c/0: disable + __IO uint32_t ss_activeHigh; // 0x30/0: disable + __IO uint32_t __fill1[3]; + __IO uint32_t xip_enable; // 0x40/0: enable + __IO uint32_t xip_instr; // 0x44/0:7 data, 8: enable, 16:23 dummy data, 24:27 dummy count + __IO uint32_t xip_mode; // 0x48/0: instr transfer mode, 8: addr transfer mode, 16: dummy transfer mode, 24: data transfer mode + __IO uint32_t __fill2[2]; + __IO uint32_t xip_write32; // 0x50 + __IO uint32_t xip_readwrite32; // 0x54 + __IO uint32_t xip_read32; // 0x58 +} __attribute((__packed__)) qspi_t; + +typedef struct { + uint32_t cpol; + uint32_t cpha; + uint32_t mode; + uint32_t clkDivider; + uint32_t ssSetup; + uint32_t ssHold; + uint32_t ssDisable; +} spi_cfg; + +#define SPI_CMD_WRITE (1 << 8) +#define SPI_CMD_READ (1 << 9) +#define SPI_CMD_SS (1 << 11) + +#define SPI_RSP_VALID (1 << 31) + +#define SPI_STATUS_CMD_INT_ENABLE = (1 << 0) +#define SPI_STATUS_RSP_INT_ENABLE = (1 << 1) +#define SPI_STATUS_CMD_INT_FLAG = (1 << 8) +#define SPI_STATUS_RSP_INT_FLAG = (1 << 9) + +static inline void spi_configure(volatile qspi_t* reg, spi_cfg *config){ + reg->config = (config->cpol << 0) | (config->cpha << 1) | (config->mode << 4); + reg->clk_divider = config->clkDivider; + reg->ss_setup = config->ssSetup; + reg->ss_hold = config->ssHold; + reg->ss_disable =config->ssDisable; +} + +static inline void spi_init(volatile qspi_t* spi){ + spi_cfg spiCfg; + spiCfg.cpol = 0; + spiCfg.cpha = 0; + spiCfg.mode = 0; + spiCfg.clkDivider = 2; + spiCfg.ssSetup = 2; + spiCfg.ssHold = 2; + spiCfg.ssDisable = 2; + spi_configure(spi, &spiCfg); +} + +static inline uint32_t spi_cmd_avail(volatile qspi_t* reg){ + return reg->status & 0xFFFF; +} +static inline uint32_t spi_rsp_occupied(volatile qspi_t* reg){ + return reg->status >> 16; +} + +static inline void spi_write(volatile qspi_t* reg, uint8_t data){ + while(spi_cmd_avail(reg) == 0); + reg->data = data | SPI_CMD_WRITE; +} + +static inline uint8_t spi_write_read(volatile qspi_t* reg, uint8_t data){ + while(spi_cmd_avail(reg) == 0); + reg->data = data | SPI_CMD_READ | SPI_CMD_WRITE; + while(spi_rsp_occupied(reg) == 0); + return reg->data; +} + + +static inline uint8_t spi_read(volatile qspi_t* reg){ + while(spi_cmd_avail(reg) == 0); + reg->data = SPI_CMD_READ; + while(spi_rsp_occupied(reg) == 0); + while((reg->data & 0x80000000)==0); + return reg->data; +} + +static inline void spi_select(volatile qspi_t* reg, uint32_t slaveId){ + while(spi_cmd_avail(reg) == 0); + reg->data = slaveId | 0x80 | SPI_CMD_SS; +} + +static inline void spi_deselect(volatile qspi_t* reg, uint32_t slaveId){ + while(spi_cmd_avail(reg) == 0); + reg->data = slaveId | SPI_CMD_SS; +} + +static inline void spi_wait_tx_idle(volatile qspi_t* reg){ + while(spi_cmd_avail(reg) < 0x20); +} +#endif /* _BSP_QSPI_H */ diff --git a/include/ehrenberg/devices/timer.h b/include/ehrenberg/devices/timer.h new file mode 100644 index 0000000..48024c4 --- /dev/null +++ b/include/ehrenberg/devices/timer.h @@ -0,0 +1,52 @@ +#ifndef _BSP_TIMER_H +#define _BSP_TIMER_H + +#include + +typedef struct __attribute((__packed__)) { + volatile uint32_t mtime; // 0x0:0 + volatile uint32_t mtimeh; // 0x4:0 + volatile uint32_t mtimecmp; // 0x8:0 + volatile uint32_t mtimecmph; // 0xc:0 +} mtimer_t; + +#ifndef APB_BUS +typedef struct __attribute((__packed__)) { + volatile uint16_t count; +} prescaler_t; + +typedef struct __attribute((__packed__)) { + volatile uint16_t clk_en; // 0x0:0, 0->always, 1->prescaler + volatile uint16_t clr_en; // 0x2:0, 0->on overflow + volatile uint32_t limit; // 0x4:0, upper limit of counter + volatile uint32_t timer_value; // 0x8:0 current timer value +} timer_a_t; + +#else + +typedef struct __attribute((__packed__)) { + volatile uint32_t LIMIT; +} prescaler_t; + +typedef struct __attribute((__packed__)) { + volatile uint32_t CLEARS_TICKS; // 0x0/0:0->always, 1->prescaler; 16:0->on overflow + volatile uint32_t LIMIT; // 0x4/0 upper limit of counter + volatile uint32_t VALUE; // 0x8/0 current timer value +} timer_a_t; + +inline void prescaler_init(prescaler_t* reg){ + (void)reg; +} + +inline void timer_init(timer_a_t *reg){ + reg->CLEARS_TICKS = 0; + reg->VALUE = 0; +} + +inline void mtimer_init(mtimer_t *reg){ + reg->mtimecmph = UINT32_MAX; + reg->mtimecmp = UINT32_MAX; +} + +#endif +#endif /* _BSP_TIMER_H */ diff --git a/include/ehrenberg/devices/uart.h b/include/ehrenberg/devices/uart.h new file mode 100644 index 0000000..3017062 --- /dev/null +++ b/include/ehrenberg/devices/uart.h @@ -0,0 +1,67 @@ +#ifndef _BSP_UART_H +#define _BSP_UART_H + +#include + +enum uart_parity_e {NONE = 0, EVEN = 1, ODD = 2}; +enum uart_stop_e {ONE = 0, TWO = 1}; + +#ifndef APB_BUS +typedef struct __attribute((__packed__)){ + // 0x0 + volatile uint16_t rx_tx_reg; // 8bit, 0x0 + volatile uint16_t rx_avail; // 1bit, 0x0:16 + // 0x4 + volatile uint16_t irq_ctrl; // 0->tx_ie, 1->rx_ie, 8->tx_ip, 9->rx_ip + volatile uint8_t num_tx_avail; // 8bit, 0x4:16 + volatile uint8_t num_rx_avail; // 8bit, 0x4:24 + volatile uint32_t dummy; + // 0xc + volatile uint8_t clock_div; // 3bit, 0xc:0 + volatile uint8_t frame; // 2bit, 0xc:8 + volatile uint8_t stop_bits; // 1bit, 0xc:16 + // 0x10 + volatile uint8_t status; // readError->0, readOverflowError->1, + volatile uint8_t active; // rx_active->0, tx_active-1, set_tx_active->2, clear_tx_active->3 +} uart_t; +#else +typedef struct __attribute((__packed__)) { + volatile uint32_t DATA; + volatile uint32_t STATUS; + volatile uint32_t CLOCK_DIVIDER; + volatile uint32_t FRAME_CONFIG; +} uart_t; + + +typedef struct __attribute((__packed__)) { + uint32_t data_length; + enum uart_parity_e parity; + enum uart_stop_e stop; + uint32_t clock_divider; +} uart_config_t; + +static inline uint32_t uart_get_tx_free(volatile uart_t *reg){ + return (reg->STATUS >> 16) & 0xFF; +} + +static inline uint32_t uart_get_rx_avail(volatile uart_t *reg){ + return reg->STATUS >> 24; +} + +static void uart_write(volatile uart_t *reg, uint8_t data){ + while(uart_get_tx_free(reg) == 0); + reg->DATA = data; +} + +static inline uint8_t uart_read(volatile uart_t *reg){ + uint32_t res = reg->DATA; + while((res&0x10000) == 0) res = reg->DATA; + return res; +} + +static inline void uart_set_config(volatile uart_t *reg, uart_config_t *config){ + reg->CLOCK_DIVIDER = config->clock_divider; + reg->FRAME_CONFIG = ((config->data_length-1) << 0) | (config->parity << 8) | (config->stop << 16); +} +#endif +#endif /* _BSP_UART_H */ diff --git a/libwrap/sys/printf.c b/libwrap/sys/printf.c index 4440f53..ee94a79 100644 --- a/libwrap/sys/printf.c +++ b/libwrap/sys/printf.c @@ -21,6 +21,10 @@ size_t strnlen (const char *str, size_t n) return str - start; } +static void fprintf_putch(int ch, void** data) +{ + putchar(ch); +} static void sprintf_putch(int ch, void** data) { char** pstr = (char**)data; @@ -100,13 +104,13 @@ static void vprintfmt(void (*putch)(int, void**), void **putdat, const char *fmt { register const char* p; const char* last_fmt; - register int ch, err; + register int ch; unsigned long num; - int base, lflag, width, precision, altflag; + int base, lflag, width, precision; char padc; while (1) { - while ((ch = *(unsigned char *) fmt) != '%') { + while ((ch = *(const char *) fmt) != '%') { if (ch == '\0') return; fmt++; @@ -120,9 +124,8 @@ static void vprintfmt(void (*putch)(int, void**), void **putdat, const char *fmt width = -1; precision = -1; lflag = 0; - altflag = 0; reswitch: - switch (ch = *(unsigned char *) fmt++) { + switch (ch = *(const char *) fmt++) { // flag to pad on the right case '-': @@ -162,7 +165,6 @@ static void vprintfmt(void (*putch)(int, void**), void **putdat, const char *fmt goto reswitch; case '#': - altflag = 1; goto reswitch; process_precision: @@ -170,24 +172,17 @@ static void vprintfmt(void (*putch)(int, void**), void **putdat, const char *fmt width = precision, precision = -1; goto reswitch; - // long flag - case 'l': + case 'l': // long flag if (lflag) goto bad; goto reswitch; - - // character - case 'c': + case 'c': // character putch(va_arg(ap, int), putdat); break; - - // double - case 'f': + case 'f': // double print_double(putch, putdat, va_arg(ap, double), width, precision); break; - - // string - case 's': + case 's': // string if ((p = va_arg(ap, char *)) == NULL) p = "(null)"; if (width > 0 && padc != '-') @@ -200,9 +195,7 @@ static void vprintfmt(void (*putch)(int, void**), void **putdat, const char *fmt for (; width > 0; width--) putch(' ', putdat); break; - - // (signed) decimal - case 'd': + case 'd': // (signed) decimal num = getint(&ap, lflag); if ((long) num < 0) { putch('-', putdat); @@ -210,41 +203,30 @@ static void vprintfmt(void (*putch)(int, void**), void **putdat, const char *fmt } base = 10; goto signed_number; - - // unsigned decimal - case 'u': + case 'u': // unsigned decimal base = 10; goto unsigned_number; - - // (unsigned) octal - case 'o': + case 'o': // (unsigned) octal // should do something with padding so it's always 3 octits base = 8; goto unsigned_number; - - // pointer - case 'p': + case 'p':// pointer lflag = 1; putch('0', putdat); putch('x', putdat); /* fall through to 'x' */ - - // (unsigned) hexadecimal - case 'x': + __attribute__((fallthrough)); + case 'x': // (unsigned) hexadecimal base = 16; - unsigned_number: + unsigned_number: num = getuint(&ap, lflag); - signed_number: + signed_number: printnum(putch, putdat, num, base, width, padc); break; - - // escaped '%' character - case '%': + case '%': // escaped '%' character putch(ch, putdat); break; - - // unrecognized escape sequence - just print it literally - default: + default: // unrecognized escape sequence - just print it literally bad: putch('%', putdat); fmt = last_fmt; @@ -258,7 +240,7 @@ int __wrap_printf(const char* fmt, ...) va_list ap; va_start(ap, fmt); - vprintfmt((void*)putchar, 0, fmt, ap); + vprintfmt(fprintf_putch, 0, fmt, ap); va_end(ap); return 0; // incorrect return value, but who cares, anyway? diff --git a/libwrap/sys/puts.c b/libwrap/sys/puts.c index 50d6437..c036cb7 100644 --- a/libwrap/sys/puts.c +++ b/libwrap/sys/puts.c @@ -12,6 +12,10 @@ int __wrap_puts(const char *s) { while (*s != '\0') { +#if defined(BOARD_ehrenberg) + while (uart_get_tx_free(uart)==0) ; + uart_write(uart, *s); +#else while (UART0_REG(UART_REG_TXFIFO) & 0x80000000) ; UART0_REG(UART_REG_TXFIFO) = *s; @@ -19,7 +23,7 @@ int __wrap_puts(const char *s) while (UART0_REG(UART_REG_TXFIFO) & 0x80000000) ; UART0_REG(UART_REG_TXFIFO) = '\r'; } - +#endif ++s; } diff --git a/libwrap/sys/read.c b/libwrap/sys/read.c index 06bafb2..db11a8f 100644 --- a/libwrap/sys/read.c +++ b/libwrap/sys/read.c @@ -4,32 +4,40 @@ #include #include #include - +#if defined(BOARD_ehrenberg) +#include "platform.h" +#endif #include "platform.h" #include "stub.h" #include "weak_under_alias.h" ssize_t __wrap_read(int fd, void* ptr, size_t len) { -#if defined(BOARD_hifive1) uint8_t * current = (uint8_t *)ptr; +#if defined(BOARD_hifive1) volatile uint32_t * uart_rx = (uint32_t *)(UART0_CTRL_ADDR + UART_REG_RXFIFO); volatile uint8_t * uart_rx_cnt = (uint8_t *)(UART0_CTRL_ADDR + UART_REG_RXCTRL + 2); -#else - uint8_t * current = (uint8_t *)ptr; +#elif !defined(BOARD_ehrenberg) volatile uint32_t * uart_rx = (uint32_t *)(UART0_BASE_ADDR + UART_REG_RXFIFO); volatile uint8_t * uart_rx_cnt = (uint8_t *)(UART0_BASE_ADDR + UART_REG_RXCTRL + 2); #endif - ssize_t result = 0; - if (isatty(fd)) { +#if defined(BOARD_ehrenberg) + for (current = (uint8_t *)ptr; + (current < ((uint8_t *)ptr) + len) && (uart_get_rx_avail(uart) > 0); + current ++) { + *current = uart_read(uart); + result++; + } +#else for (current = (uint8_t *)ptr; (current < ((uint8_t *)ptr) + len) && (*uart_rx_cnt > 0); current ++) { *current = *uart_rx; result++; } +#endif return result; } return _stub(EBADF); diff --git a/libwrap/sys/sbrk.c b/libwrap/sys/sbrk.c index a43c191..50f6be7 100644 --- a/libwrap/sys/sbrk.c +++ b/libwrap/sys/sbrk.c @@ -10,7 +10,7 @@ void *__wrap_sbrk(ptrdiff_t incr) static char *curbrk = _end; if ((curbrk + incr < _end) || (curbrk + incr > _heap_end)) - return NULL - 1; + return (void*)- 1; curbrk += incr; return curbrk - incr; diff --git a/libwrap/sys/write.c b/libwrap/sys/write.c index b1e9a7e..c1ab0a0 100644 --- a/libwrap/sys/write.c +++ b/libwrap/sys/write.c @@ -11,21 +11,29 @@ ssize_t __wrap_write(int fd, const void* ptr, size_t len) { - const uint8_t * current = (const char *)ptr; + const uint8_t * current = (const uint8_t *)ptr; + if (isatty(fd)) { + for (size_t jj = 0; jj < len; jj++) { +#if defined(BOARD_ehrenberg) + while (uart_get_tx_free(uart)==0) ; + uart_write(uart, current[jj]); + if (current[jj] == '\n') { + while (uart_get_tx_free(uart)==0) ; + uart_write(uart, '\r'); + } +#else + while (UART0_REG(UART_REG_TXFIFO) & 0x80000000) ; + UART0_REG(UART_REG_TXFIFO) = current[jj]; - if (isatty(fd)) { - for (size_t jj = 0; jj < len; jj++) { - while (UART0_REG(UART_REG_TXFIFO) & 0x80000000) ; - UART0_REG(UART_REG_TXFIFO) = current[jj]; - - if (current[jj] == '\n') { - while (UART0_REG(UART_REG_TXFIFO) & 0x80000000) ; - UART0_REG(UART_REG_TXFIFO) = '\r'; - } + if (current[jj] == '\n') { + while (UART0_REG(UART_REG_TXFIFO) & 0x80000000) ; + UART0_REG(UART_REG_TXFIFO) = '\r'; + } +#endif + } + return len; } - return len; - } - return _stub(EBADF); + return _stub(EBADF); } weak_under_alias(write);