├── LICENSE ├── Makefile ├── README.md ├── arch ├── boot_arm.s ├── common.h ├── common.ld ├── common.mk ├── stm32f0 │ ├── build.mk │ ├── link.ld │ ├── mcu.h │ └── openocd.cfg ├── stm32f1 │ ├── build.mk │ ├── link.ld │ ├── mcu.h │ └── openocd.cfg ├── stm32f3 │ ├── build.mk │ ├── link.ld │ ├── mcu.h │ └── openocd.cfg └── stm32f7 │ ├── build.mk │ ├── link.ld │ ├── mcu.h │ └── openocd.cfg ├── examples ├── blinky │ ├── Makefile │ └── main.c ├── uart-echo │ ├── Makefile │ └── main.c └── uart-json-rpc │ ├── Makefile │ ├── README.md │ ├── main.c │ ├── mjson.c │ └── mjson.h ├── rtos.c └── rtos.h /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Sergey Lyubka 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | EXAMPLES = $(wildcard examples/*) 2 | ARCHITECTURES = stm32f0 stm32f1 stm32f3 #stm32f7 3 | 4 | # Build firmwares for all supported architectures 5 | all: 6 | for A in $(ARCHITECTURES) ; do \ 7 | for E in $(EXAMPLES) ; do \ 8 | make -C $$E ARCH=$$A clean all ; \ 9 | done; \ 10 | done 11 | 12 | clean: 13 | for E in $(EXAMPLES) ; do make -C $$E clean ; done 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RTOS for embedded systems 2 | 3 | An open source firmware development framework that includes RTOS and 4 | drivers. Features include: 5 | 6 | - Uses GCC + newlib and no extra dependencies 7 | - Unified API across architectures 8 | - RTOS scheduling is round robin with 1ms time slice 9 | 10 | # Feature support matrix 11 | 12 | | ARCH | Clock | Task | Mutex | Timer | Queue | GPIO | UART | SPI | I2C | 13 | | ------- | ------- | ---- | ---- | ----- | ----- | ---- | ---- | --- | --- | 14 | | stm32f0 | 48Mhz | yes | - | - | - | yes | yes | - | - | 15 | | stm32f1 | 72Mhz | yes | - | - | - | yes | yes | - | - | 16 | | stm32f3 | 72MHz | yes | - | - | - | yes | yes | - | - | 17 | | stm32f7 | 216MHz | yes | - | - | - | yes | - | - | - | 18 | 19 | # Build 20 | 21 | Install packages `arm-none-eabi-gcc`, `st-link`, `openocd` (assuming Ubuntu): 22 | ```sh 23 | $ apt-get install gcc-arm-none-eabi binutils-arm-none-eabi gdb-arm-none-eabi openocd stlink-tools 24 | ``` 25 | Build example firmware: 26 | ```sh 27 | $ make -C examples/blinky clean all ARCH=stm32f1 28 | ``` 29 | To flash a firmware, connect your board and run: 30 | ```sh 31 | $ make -C examples/blinky flash 32 | ``` 33 | 34 | # API Reference 35 | 36 | - **Task management** 37 | - **rtos_init();** - must be the first function called by main() 38 | - **rtos_task_create(fn, data, stacksize);** - create task 39 | - **rtos_schedule();** - start task scheduler, do not return 40 | - **Utility** 41 | - **rtos_msleep(ms);** - sleep `ms` milliseconds 42 | - **rtos_ram_free();** - return available RAM in bytes 43 | - **rtos_ram_used();** - return used RAM in bytes 44 | - **GPIO** 45 | - **PIN(bank, num);** - declare a GPIO pin: `uint16_t pin = PIN('A', 5);` 46 | - **gpio_init(pin, mode, type, speed, pull, af);** - init GPIO pin 47 | - **gpio_on(pin);** - set GPIO pin on 48 | - **gpio_off(pin);** - set GPIO pin off 49 | - **gpio_toggle(pin);** - toggle GPIO pin 50 | - **gpio_input(pin);** - (convenience) set input mode on a GPIO pin 51 | - **gpio_output(pin);** - (convenience) set output mode on a GPIO pin 52 | - **UART** 53 | - **uart_init(uart, tx_pin, rx_pin, baud);** - initialise UART 54 | - **uart_write_byte(uart, byte);** - write one byte 55 | - **uart_write_buf(uart, buf, len);** - write buffer 56 | - **uart_read_ready(uart);** - return non-0 if UART can be read 57 | - **uart_read_byte(uart);** - read one byte 58 | 59 | # Debugging 60 | 61 | In order to debug, first run `openocd` in one terminal: 62 | 63 | ```sh 64 | $ make -C examples/blinky ARCH=stm32f3 openocd 65 | ``` 66 | 67 | In another terminal, build with `DEBUG=1` and start a GDB debugger: 68 | 69 | ```sh 70 | $ make -C examples/blinky ARCH=stm32f3 DEBUG=1 make clean all gdb 71 | ``` 72 | 73 | By default, GDB breakpoints in `main()`. Tip: use `layout split` GDB command 74 | for a handy source + disassembly display mode. 75 | 76 | The `DEBUG=1` adds a `--specs rdimon` linker flag, which enables a semihosting 77 | feature on ARM. That means, all stdio functions like `printf()` will be 78 | retargeted to an openocd console output - useful for a quick printf-debugging 79 | -------------------------------------------------------------------------------- /arch/boot_arm.s: -------------------------------------------------------------------------------- 1 | .cpu cortex-m3 2 | .thumb 3 | 4 | .section .vectors,"a",%progbits // Place interrupt vectors into its own section 5 | // Cortex-M standard interrupt handlers 6 | .word _estack // 0 Stack top address 7 | .word _reset // 1 Reset 8 | .word pass // 2 NMI 9 | .word halt // 3 Hard Fault 10 | .word halt // 4 MM Fault 11 | .word halt // 5 Bus Fault 12 | .word halt // 6 Usage Fault 13 | .word 0 // 7 RESERVED 14 | .word 0 // 8 RESERVED 15 | .word 0 // 9 RESERVED 16 | .word 0 // 10 RESERVED 17 | .word halt // 11 SV call 18 | .word halt // 12 Debug reserved 19 | .word 0 // 13 RESERVED 20 | .word PendSV_Handler // 14 PendSV 21 | .word SysTick_Handler // 15 SysTick 22 | // STM32 specific handlers 23 | .word halt,halt,halt,halt,halt,halt,halt,halt,halt,halt 24 | .word halt,halt,halt,halt,halt,halt,halt,halt,halt,halt 25 | .word halt,halt,halt,halt,halt,halt,halt,halt,halt,halt 26 | .word halt,halt,halt,halt,halt,halt,halt,halt,halt,halt 27 | .word halt,halt,halt,halt,halt,halt,halt,halt,halt,halt 28 | 29 | .section .text // Place interrupt handlers into the .text section 30 | 31 | .thumb_func // Important. Marks ARM thumb mode function, see 32 | // http://twins.ee.nctu.edu.tw/courses/ip_core_01/lab_hw_pdf/lab_1.pdf 33 | // .thumb_func makes linker add a state change code block 34 | .global _reset // Make _reset handler visible to the linker 35 | _reset: 36 | bl main // Jump to main() 37 | b . // Should not be here. Halt 38 | 39 | .thumb_func 40 | halt: b . // `b` is jump, `.` is current address: infinite loop 41 | 42 | .thumb_func // `bx` jumps to an address in `lr` register 43 | pass: bx lr // `lr` is a link register, contains return address 44 | -------------------------------------------------------------------------------- /arch/common.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | 4 | #include 5 | #include 6 | 7 | #include "rtos.h" 8 | 9 | void rtos_heap_init(void *start, void *end); // Initialise malloc 10 | 11 | #define BIT(x) (1UL << (x)) 12 | #define PIN(bank, num) ((((bank) - 'A') << 8) | (num)) 13 | 14 | static inline void init_ram(void) { 15 | extern uint32_t _sbss, _ebss; 16 | extern uint32_t _sdata, _edata, _sidata; 17 | memset(&_sbss, 0, (size_t)(((char *) &_ebss - (char *) &_sbss))); 18 | memcpy(&_sdata, &_sidata, (size_t)(((char *) &_edata - (char *) &_sdata))); 19 | 20 | extern uint32_t _end, _estack; 21 | rtos_heap_init(&_end, &_estack); 22 | } 23 | 24 | static inline void spin(volatile uint32_t count) { 25 | while (count--) asm("nop"); 26 | } 27 | 28 | struct systick { 29 | volatile uint32_t CTRL, LOAD, VAL, CALIB; 30 | }; 31 | #define SysTick ((struct systick *) 0xe000e010) 32 | 33 | struct nvic { 34 | volatile uint32_t ISER[8], RESERVED0[24], ICER[8], RSERVED1[24], ISPR[8], 35 | RESERVED2[24], ICPR[8], RESERVED3[24], IABR[8], RESERVED4[56], IP[240], 36 | RESERVED5[644], STIR; 37 | } NVIC_Type; 38 | #define NVIC ((struct nvic *) 0xe000e100) 39 | 40 | static inline void Set_NVIC_Prio(int irq, uint32_t prio) { 41 | NVIC->IP[irq] = prio << 4; 42 | } 43 | 44 | static inline uint32_t SysTick_Config(uint32_t ticks) { 45 | if ((ticks - 1) > 0xffffff) return 1; // Systick timer is 24 bit 46 | SysTick->LOAD = ticks - 1; 47 | SysTick->VAL = 0; 48 | SysTick->CTRL = BIT(0) | BIT(1) | BIT(2); 49 | return 0; 50 | } 51 | 52 | struct flash { 53 | volatile uint32_t ACR, KEYR, OPTKEYR, SR, CR, AR, RESERVED, OBR, WRPR; 54 | }; 55 | #define FLASH ((struct flash *) 0x40022000) 56 | 57 | enum { GPIO_MODE_INPUT, GPIO_MODE_OUTPUT, GPIO_MODE_AF, GPIO_MODE_ANALOG }; 58 | enum { GPIO_OTYPE_PUSH_PULL, GPIO_OTYPE_OPEN_DRAIN }; 59 | enum { GPIO_SPEED_LOW, GPIO_SPEED_MEDIUM, GPIO_SPEED_HIGH, GPIO_SPEED_INSANE }; 60 | enum { GPIO_PULL_NONE, GPIO_PULL_UP, GPIO_PULL_DOWN }; 61 | -------------------------------------------------------------------------------- /arch/common.ld: -------------------------------------------------------------------------------- 1 | ENTRY(_reset); 2 | _estack = ORIGIN(ram) + LENGTH(ram); /* for boot.s and init_heap() */ 3 | SECTIONS { 4 | .vectors : { KEEP(*(.vectors)) } > rom 5 | .text : { *(.text*) } > rom 6 | .rodata : { *(.rodata*) } > rom 7 | 8 | .data : { 9 | _sdata = .; /* for init_ram() */ 10 | *(.first_data) 11 | *(.data SORT(.data.*)) 12 | _edata = .; /* for init_ram() */ 13 | } > ram AT > rom 14 | _sidata = LOADADDR(.data); 15 | 16 | .bss : { 17 | _sbss = .; /* for init_ram() */ 18 | __bss_start__ = _sbss; /* for libc */ 19 | *(.bss SORT(.bss.*) COMMON) 20 | _ebss = .; /* for init_ram() */ 21 | __bss_end__ = _ebss; /* for libc */ 22 | } > ram 23 | 24 | . = ALIGN(8); /* Make sure that heap pointer is 8-byte aligned */ 25 | end = .; /* for syscalls.c */ 26 | _end = .; /* for cmsis_gcc.h and init_ram() */ 27 | __end__ = .; /* for libc */ 28 | } 29 | -------------------------------------------------------------------------------- /arch/common.mk: -------------------------------------------------------------------------------- 1 | PROG ?= firmware 2 | ROOT_PATH = $(realpath $(dir $(lastword $(MAKEFILE_LIST)))/..) 3 | ARCH_PATH = $(ROOT_PATH)/arch/$(ARCH) 4 | BOARD_PATH = $(ROOT_PATH)/boards/$(BOARD) 5 | TOOLCHAIN ?= arm-none-eabi 6 | OBJ_PATH = obj 7 | 8 | INCLUDES ?= -I. -I$(ROOT_PATH) -I$(ARCH_PATH) 9 | WARN ?= -W -Wall -Wextra -Werror -Wundef -Wshadow -Wdouble-promotion -Wformat-truncation -fno-common -Wconversion 10 | COPT ?= -Os -g3 -ffunction-sections -fdata-sections -DARCH=$(ARCH) 11 | CFLAGS += $(WARN) $(COPT) $(MCU_FLAGS) $(INCLUDES) $(EXTRA) 12 | 13 | LINKFLAGS ?= $(MCU_FLAGS) -T$(ARCH_PATH)/link.ld -T$(ROOT_PATH)/arch/common.ld -specs=nano.specs -Wl,--gc-sections 14 | ifeq "$(DEBUG)" "1" 15 | LINKFLAGS += -specs=rdimon.specs -lrdimon 16 | CFLAGS += -D'DEBUG(x)=printf x' 17 | else 18 | CFLAGS += -D'DEBUG(x)=' 19 | endif 20 | 21 | SOURCES += $(ROOT_PATH)/rtos.c 22 | OBJECTS = $(OBJ_PATH)/boot.o $(SOURCES:%.c=$(OBJ_PATH)/%.o) 23 | 24 | all: $(OBJ_PATH)/$(PROG).hex 25 | 26 | $(OBJ_PATH)/$(PROG).bin: $(OBJ_PATH)/$(PROG).elf 27 | $(TOOLCHAIN)-objcopy -O binary $< $@ 28 | 29 | $(OBJ_PATH)/$(PROG).hex: $(OBJ_PATH)/$(PROG).bin 30 | $(TOOLCHAIN)-objcopy -I binary -O ihex --change-address 0x8000000 $< $@ 31 | 32 | $(OBJ_PATH)/$(PROG).elf: $(OBJECTS) $(ARCH_PATH)/link.ld 33 | $(TOOLCHAIN)-gcc $(OBJECTS) $(LINKFLAGS) -o $@ 34 | $(TOOLCHAIN)-size $@ 35 | 36 | $(OBJ_PATH)/%.o: %.c 37 | @mkdir -p $(dir $@) 38 | $(TOOLCHAIN)-gcc $(CFLAGS) -c $< -o $@ 39 | 40 | $(OBJ_PATH)/boot.o: $(ROOT_PATH)/arch/boot_arm.s 41 | @mkdir -p $(dir $@) 42 | $(TOOLCHAIN)-as -g --warn --fatal-warnings $(MCU_FLAGS) $< -o $@ 43 | 44 | flash: $(OBJ_PATH)/$(PROG).bin 45 | st-flash --reset write $< 0x8000000 46 | 47 | openocd: 48 | openocd -f $(ARCH_PATH)/openocd.cfg 49 | 50 | ELF ?= $(OBJ_PATH)/$(PROG).elf 51 | ARGS ?= -ex 'b main' 52 | gdb: 53 | $(TOOLCHAIN)-gdb \ 54 | -ex 'set confirm off' \ 55 | -ex 'target extended-remote :3333' \ 56 | -ex 'monitor arm semihosting enable' \ 57 | -ex 'monitor reset halt' \ 58 | -ex 'load' \ 59 | -ex 'monitor reset init' \ 60 | $(ARGS) \ 61 | -ex 'r' \ 62 | $(ELF) 63 | 64 | d: 65 | $(TOOLCHAIN)-gcc $(CFLAGS) -s -S -o - ~/tmp/foo.c | less 66 | 67 | clean: 68 | @rm -rf *.{bin,elf,map,lst,tgz,zip,hex} $(OBJ_PATH) 69 | -------------------------------------------------------------------------------- /arch/stm32f0/build.mk: -------------------------------------------------------------------------------- 1 | SELF_DIR = $(dir $(lastword $(MAKEFILE_LIST))) 2 | MCU_FLAGS ?= -mcpu=cortex-m0 -mthumb -mfloat-abi=soft 3 | 4 | include $(SELF_DIR)/../../arch/common.mk 5 | -------------------------------------------------------------------------------- /arch/stm32f0/link.ld: -------------------------------------------------------------------------------- 1 | MEMORY { 2 | ram(rwx) : ORIGIN = 0x20000000, LENGTH = 16k 3 | rom(rx) : ORIGIN = 0x08000000, LENGTH = 128k 4 | } 5 | -------------------------------------------------------------------------------- /arch/stm32f0/mcu.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | // 4 | // RM0091 5 | 6 | #pragma once 7 | 8 | #include "arch/common.h" 9 | 10 | struct gpio { 11 | volatile uint32_t MODER, OTYPER, OSPEEDR, PUPDR, IDR, ODR, BSRR; 12 | volatile uint32_t LCKR, AFR[2], BRR; 13 | }; 14 | static inline struct gpio *gpio_bank(uint16_t pin) { 15 | return (struct gpio *) (0x48000000 + 0x400 * (pin >> 8)); 16 | } 17 | static inline void gpio_on(uint16_t pin) { 18 | gpio_bank(pin)->ODR |= BIT(pin & 255); 19 | } 20 | static inline void gpio_off(uint16_t pin) { 21 | gpio_bank(pin)->ODR &= ~BIT(pin & 255); 22 | } 23 | static inline void gpio_toggle(uint16_t pin) { 24 | gpio_bank(pin)->ODR ^= BIT(pin & 255); 25 | } 26 | 27 | static inline void gpio_init(uint16_t pin, uint8_t mode, uint8_t type, 28 | uint8_t speed, uint8_t pull, uint8_t af) { 29 | struct gpio *gpio = gpio_bank(pin); 30 | uint8_t n = (uint8_t)(pin & 255); 31 | gpio->MODER &= ~(3UL << (n * 2)); 32 | gpio->MODER |= ((uint32_t) mode) << (n * 2); 33 | gpio->OTYPER &= ~(1UL << n); 34 | gpio->OTYPER |= ((uint32_t) type) << n; 35 | gpio->OSPEEDR &= ~(3UL << (n * 2)); 36 | gpio->OSPEEDR |= ((uint32_t) speed) << (n * 2); 37 | gpio->PUPDR &= ~(3UL << (n * 2)); 38 | gpio->PUPDR |= ((uint32_t) pull) << (n * 2); 39 | if (n < 8) { 40 | gpio->AFR[0] &= ~(15UL << (n * 4)); 41 | gpio->AFR[0] |= ((uint32_t) af) << (n * 4); 42 | } else { 43 | gpio->AFR[1] &= ~(15UL << ((n - 8) * 4)); 44 | gpio->AFR[1] |= ((uint32_t) af) << ((n - 8) * 4); 45 | } 46 | } 47 | 48 | static inline void gpio_input(uint16_t pin) { 49 | gpio_init(pin, GPIO_MODE_INPUT, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 50 | GPIO_PULL_NONE, 0); 51 | } 52 | 53 | static inline void gpio_output(uint16_t pin) { 54 | gpio_init(pin, GPIO_MODE_OUTPUT, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 55 | GPIO_PULL_NONE, 0); 56 | } 57 | 58 | struct rcc { 59 | volatile uint32_t CR, CFGR, CIR, APB2RSTR, APB1RSTR, AHBENR, APB2ENR, APB1ENR, 60 | BDCR, CSR, AHBRSTR, CFGR2, CFGR3, CR2; 61 | }; 62 | 63 | #define RCC ((struct rcc *) 0x40021000) 64 | 65 | struct uart { 66 | volatile uint32_t CR1, CR2, CR3, BRR, GTPR, RTOR, RQR, ISR, ICR; 67 | volatile uint16_t RDR, RESERVED1, TDR, RESERVED2; 68 | }; 69 | #define UART1 ((struct uart *) 0x40013800) 70 | #define UART2 ((struct uart *) 0x40004400) 71 | #define UART3 ((struct uart *) 0x40004800) 72 | #define UART4 ((struct uart *) 0x40004c00) 73 | 74 | static inline void uart_init(struct uart *uart, unsigned long baud) { 75 | // https://www.st.com/resource/en/datasheet/stm32f072rb.pdf table 15 76 | uint8_t af = 0; // Alternate function 77 | uint16_t rx = 0, tx = 0; // pins 78 | if (uart == UART1) af = 1, tx = PIN('B', 6), rx = PIN('B', 7); 79 | if (uart == UART2) af = 1, tx = PIN('A', 2), rx = PIN('A', 3); 80 | if (uart == UART3) af = 4, tx = PIN('B', 10), rx = PIN('B', 11); 81 | if (uart == UART4) af = 4, tx = PIN('A', 0), rx = PIN('A', 1); 82 | gpio_init(tx, GPIO_MODE_AF, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 0, af); 83 | gpio_init(rx, GPIO_MODE_AF, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 0, af); 84 | uart->BRR = rtos_freq_get() / baud; // Set baud rate 85 | uart->CR1 |= BIT(0) | BIT(2) | BIT(3); // Enable this UART, RE, TE 86 | } 87 | 88 | static inline void uart_write_byte(struct uart *uart, uint8_t byte) { 89 | uart->TDR = byte; 90 | while ((uart->ISR & BIT(7)) == 0) (void) 0; 91 | } 92 | 93 | static inline void uart_write_buf(struct uart *uart, char *buf, size_t len) { 94 | while (len-- > 0) uart_write_byte(uart, *(uint8_t *) buf++); 95 | } 96 | 97 | static inline int uart_read_ready(struct uart *uart) { 98 | return uart->ISR & BIT(5); // If RXNE bit is set, data is ready 99 | } 100 | 101 | static inline uint8_t uart_read_byte(struct uart *uart) { 102 | return (uint8_t)(uart->RDR & 255); 103 | } 104 | 105 | struct scb { 106 | volatile uint32_t CPUID, ICSR, RESERVED0, AIRCR, SCR, CCR, RESERVED1, SHP[2]; 107 | }; 108 | #define SCB ((struct scb *) 0xe000ed00) 109 | 110 | static inline void init_clock(void) { 111 | FLASH->ACR |= 0x11; // Set flash wait states 112 | RCC->CFGR = 0x280000; // PLL_MUL_12 113 | RCC->CR |= BIT(24); // PLL_ON 114 | while (!(RCC->CR & BIT(25))) (void) 0; // PLL_READY 115 | RCC->CFGR &= ~3UL; // Clear off current clock source 116 | RCC->CFGR |= 2; // Set clock source to PLL 117 | 118 | rtos_freq_set(48000000); // HSI/2 * PLL_MUL_12 = 48 119 | SysTick_Config(rtos_freq_get() / 1000); // 1KHz SysTick interrupt 120 | } 121 | 122 | #if !defined(LED1) 123 | #define LED1 PIN('A', 5) // On-board LED pin 124 | #endif 125 | 126 | #if !defined(UART) 127 | #define UART UART2 128 | #endif 129 | 130 | static inline void rtos_init(void) { 131 | init_ram(); 132 | init_clock(); 133 | 134 | RCC->AHBENR |= BIT(17) | BIT(18) | BIT(19) | BIT(20); // Enable GPIO banks 135 | RCC->APB1ENR |= BIT(17) | BIT(18) | BIT(19); // Enable USART2 and USART3 136 | RCC->APB2ENR |= BIT(14); // Enable USART1 137 | } 138 | -------------------------------------------------------------------------------- /arch/stm32f0/openocd.cfg: -------------------------------------------------------------------------------- 1 | source [find interface/stlink-v2-1.cfg] 2 | transport select hla_swd 3 | source [find target/stm32f0x.cfg] 4 | reset_config srst_only srst_nogate 5 | -------------------------------------------------------------------------------- /arch/stm32f1/build.mk: -------------------------------------------------------------------------------- 1 | SELF_DIR = $(dir $(lastword $(MAKEFILE_LIST))) 2 | MCU_FLAGS ?= -mcpu=cortex-m3 -mthumb -mfloat-abi=soft 3 | 4 | include $(SELF_DIR)/../../arch/common.mk 5 | -------------------------------------------------------------------------------- /arch/stm32f1/link.ld: -------------------------------------------------------------------------------- 1 | MEMORY { 2 | ram(rwx) : ORIGIN = 0x20000000, LENGTH = 20k 3 | rom(rx) : ORIGIN = 0x08000000, LENGTH = 128k 4 | } 5 | -------------------------------------------------------------------------------- /arch/stm32f1/mcu.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | // 4 | // RM0008 https://www.st.com/resource/en/reference_manual/cd00171190.pdf 5 | 6 | #pragma once 7 | 8 | #include "arch/common.h" 9 | 10 | // RCC registers, TRM section 7.3, memory map section 3.3 11 | struct rcc { 12 | volatile uint32_t CR, CFGR, CIR, APB2RSTR, APB1RSTR, AHBENR, APB2ENR, APB1ENR, 13 | BDCR, CSR; 14 | }; 15 | #define RCC ((struct rcc *) 0x40021000) 16 | 17 | struct gpio { 18 | volatile uint32_t CRL, CRH, IDR, ODR, BSRR, BRR, LCKR; 19 | }; 20 | 21 | // GPIO registers, TRM section 9.2, memory map section 3.3, regs 9.1.0 22 | static inline struct gpio *gpio_bank(uint16_t pin) { 23 | return (struct gpio *) (0x40010800 + 0x400 * (pin >> 8)); 24 | } 25 | 26 | static inline void gpio_init(uint16_t pin, uint8_t mode, uint8_t type, 27 | uint8_t speed, uint8_t pull, uint8_t af) { 28 | struct gpio *gpio = gpio_bank(pin); 29 | int n = pin & 255, shift = (n < 8 ? n : (n - 8)) << 2; 30 | volatile uint32_t mask = 0, *cr = n < 8 ? &gpio->CRL : &gpio->CRH; 31 | 32 | if (mode == GPIO_MODE_OUTPUT || mode == GPIO_MODE_AF) { 33 | mask = 2; // 2MHz 34 | if (speed == GPIO_SPEED_MEDIUM) mask = 1; // 10 MHz 35 | if (speed > GPIO_SPEED_MEDIUM) mask = 3; // 50 MHz 36 | if (mode == GPIO_MODE_OUTPUT) { 37 | if (type == GPIO_OTYPE_PUSH_PULL) mask |= 0 << 2; // OUT PP 38 | if (type == GPIO_OTYPE_OPEN_DRAIN) mask |= 1 << 2; // OUT OD 39 | } else { 40 | if (type == GPIO_OTYPE_PUSH_PULL) mask |= 2 << 2; // AF PP 41 | if (type == GPIO_OTYPE_OPEN_DRAIN) mask |= 3 << 2; // AF OD 42 | } 43 | } else { 44 | if (pull == GPIO_PULL_NONE) mask |= 1 << 2; // Floating input 45 | if (pull != GPIO_PULL_NONE) mask |= 2 << 2; // Input with PU/PD 46 | } 47 | *cr &= ~(15UL << shift); 48 | *cr |= mask << shift; 49 | (void) af; 50 | } 51 | 52 | static inline void gpio_on(uint16_t pin) { 53 | gpio_bank(pin)->ODR |= BIT(pin & 255); 54 | } 55 | 56 | static inline void gpio_off(uint16_t pin) { 57 | gpio_bank(pin)->ODR &= ~BIT(pin & 255); 58 | } 59 | 60 | static inline void gpio_toggle(uint16_t pin) { 61 | gpio_bank(pin)->ODR ^= BIT(pin & 255); 62 | } 63 | 64 | static inline void gpio_input(uint16_t pin) { 65 | gpio_init(pin, GPIO_MODE_INPUT, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 66 | GPIO_PULL_NONE, 0); 67 | } 68 | 69 | static inline void gpio_output(uint16_t pin) { 70 | gpio_init(pin, GPIO_MODE_OUTPUT, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 71 | GPIO_PULL_NONE, 0); 72 | } 73 | 74 | struct uart { 75 | volatile uint32_t SR, DR, BRR, CR1, CR2, CR3, GTPR; 76 | }; 77 | 78 | #define UART1 ((struct uart *) 0x40013800) 79 | #define UART2 ((struct uart *) 0x40004400) 80 | #define UART3 ((struct uart *) 0x40004800) 81 | 82 | static inline void uart_init(struct uart *uart, unsigned baud) { 83 | // https://www.st.com/resource/en/datasheet/stm32f103c8.pdf 84 | uint8_t af = 0; // Alternate function 85 | uint16_t rx = 0, tx = 0; // pins 86 | if (uart == UART1) af = 0, tx = PIN('A', 9), rx = PIN('A', 10); 87 | if (uart == UART2) af = 0, tx = PIN('A', 2), rx = PIN('A', 3); 88 | if (uart == UART3) af = 0, tx = PIN('B', 10), rx = PIN('B', 11); 89 | 90 | gpio_init(tx, GPIO_MODE_AF, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 0, af); 91 | gpio_init(rx, GPIO_MODE_AF, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 0, af); 92 | uart->CR1 = 0; // Disable this UART 93 | uart->BRR = rtos_freq_get() / baud; // Set baud rate, TRM 29.5.4 94 | uart->CR1 = BIT(13) | BIT(2) | BIT(3); // Enable this UART, RE, TE 95 | } 96 | 97 | static inline void uart_write_byte(struct uart *uart, uint8_t byte) { 98 | uart->DR = byte; 99 | while ((uart->SR & BIT(7)) == 0) (void) 0; 100 | } 101 | 102 | static inline void uart_write_buf(struct uart *uart, char *buf, size_t len) { 103 | while (len-- > 0) uart_write_byte(uart, *(uint8_t *) buf++); 104 | } 105 | 106 | static inline int uart_read_ready(struct uart *uart) { 107 | return uart->SR & BIT(5); // If RXNE bit is set, data is ready 108 | } 109 | 110 | static inline uint8_t uart_read_byte(struct uart *uart) { 111 | return uart->DR & 255; 112 | } 113 | 114 | static inline void init_clock(void) { 115 | FLASH->ACR = 0x14; // Set flash wait states 116 | RCC->CR |= BIT(0); // Enable HSI 117 | while (!(RCC->CR & BIT(1))) (void) 0; // Wait 118 | RCC->CFGR = 15UL << 18; // HSI | PLL_16. 4 * 16 = 64 MHz 119 | RCC->CR |= BIT(24); // Enable PLL 120 | while (!(RCC->CR & BIT(25))) (void) 0; // Wait 121 | RCC->CFGR |= 2; // Set PLL as the clock source 122 | 123 | rtos_freq_set(64000000); // Set clock 124 | SysTick_Config(rtos_freq_get() / 1000); // Enable 1KHz SysTick interrupt 125 | } 126 | 127 | #if !defined(LED1) 128 | #define LED1 PIN('A', 5) // On-board LED pin 129 | #endif 130 | 131 | #if !defined(UART) 132 | #define UART UART2 133 | #endif 134 | 135 | static inline void rtos_init(void) { 136 | init_ram(); 137 | init_clock(); 138 | 139 | RCC->APB2ENR |= BIT(2) | BIT(3) | BIT(4); // Enable GPIO bank A,B,C 140 | RCC->APB2ENR |= BIT(14); // Enable USART1 141 | RCC->APB1ENR |= BIT(17) | BIT(18); // Enable USART2 and USART3 142 | } 143 | -------------------------------------------------------------------------------- /arch/stm32f1/openocd.cfg: -------------------------------------------------------------------------------- 1 | source [find interface/stlink-v2-1.cfg] 2 | transport select hla_swd 3 | source [find target/stm32f1x.cfg] 4 | reset_config srst_only srst_nogate 5 | -------------------------------------------------------------------------------- /arch/stm32f3/build.mk: -------------------------------------------------------------------------------- 1 | SELF_DIR = $(dir $(lastword $(MAKEFILE_LIST))) 2 | MCU_FLAGS ?= -mfloat-abi=soft -mcpu=cortex-m4 -mthumb 3 | 4 | include $(SELF_DIR)/../../arch/common.mk 5 | -------------------------------------------------------------------------------- /arch/stm32f3/link.ld: -------------------------------------------------------------------------------- 1 | MEMORY { 2 | ram(rwx) : ORIGIN = 0x20000000, LENGTH = 12k 3 | rom(rx) : ORIGIN = 0x08000000, LENGTH = 64k 4 | } 5 | -------------------------------------------------------------------------------- /arch/stm32f3/mcu.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | // 4 | // RM0316 https://www.st.com/resource/en/reference_manual/DM00043574-.pdf 5 | 6 | #pragma once 7 | 8 | #include "arch/common.h" 9 | 10 | struct gpio { 11 | volatile uint32_t MODER, OTYPER, OSPEEDR, PUPDR, IDR, ODR, BSRR, LCKR, AFR[2], 12 | BRR; 13 | }; 14 | 15 | static inline struct gpio *gpio_bank(uint16_t pin) { 16 | return (struct gpio *) (0x48000000 + 0x400 * (pin >> 8)); 17 | } 18 | static inline void gpio_on(uint16_t pin) { 19 | gpio_bank(pin)->ODR |= BIT(pin & 255); 20 | } 21 | static inline void gpio_off(uint16_t pin) { 22 | gpio_bank(pin)->ODR &= ~BIT(pin & 255); 23 | } 24 | static inline void gpio_toggle(uint16_t pin) { 25 | gpio_bank(pin)->ODR ^= BIT(pin & 255); 26 | } 27 | 28 | static inline void gpio_init(uint16_t pin, uint8_t mode, uint8_t type, 29 | uint8_t speed, uint8_t pull, uint8_t af) { 30 | struct gpio *gpio = gpio_bank(pin); 31 | uint8_t n = (uint8_t)(pin & 255); 32 | gpio->MODER &= ~(3UL << (n * 2)); 33 | gpio->MODER |= ((uint32_t) mode) << (n * 2); 34 | gpio->OTYPER &= ~(1UL << n); 35 | gpio->OTYPER |= ((uint32_t) type) << n; 36 | gpio->OSPEEDR &= ~(3UL << (n * 2)); 37 | gpio->OSPEEDR |= ((uint32_t) speed) << (n * 2); 38 | gpio->PUPDR &= ~(3UL << (n * 2)); 39 | gpio->PUPDR |= ((uint32_t) pull) << (n * 2); 40 | if (n < 8) { 41 | gpio->AFR[0] &= 15UL << (n * 4); 42 | gpio->AFR[0] |= ((uint32_t) af) << (n * 4); 43 | } else { 44 | gpio->AFR[1] &= 15UL << ((n - 8) * 4); 45 | gpio->AFR[1] |= ((uint32_t) af) << ((n - 8) * 4); 46 | } 47 | } 48 | static inline void gpio_input(uint16_t pin) { 49 | gpio_init(pin, GPIO_MODE_INPUT, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 50 | GPIO_PULL_NONE, 0); 51 | } 52 | static inline void gpio_output(uint16_t pin) { 53 | gpio_init(pin, GPIO_MODE_OUTPUT, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 54 | GPIO_PULL_NONE, 0); 55 | } 56 | 57 | struct rcc { 58 | volatile uint32_t CR, CFGR, CIR, APB2RSTR, APB1RSTR, AHBENR, APB2ENR, APB1ENR, 59 | BDCR, CSR, AHBRSTR, CFGR2, CFGR3; 60 | }; 61 | #define RCC ((struct rcc *) 0x40021000) 62 | 63 | static inline void init_clock(void) { 64 | FLASH->ACR = 0x12; 65 | RCC->CR |= BIT(0); // HSI ON 66 | while (!(RCC->CR & BIT(1))) (void) 0; // Wait 67 | RCC->CFGR = 15UL << 18; // PLL_16. 8MHz HSI / 2 * 16 = 64MHz 68 | RCC->CR |= BIT(24); // PLL ON 69 | while (!(RCC->CR & BIT(25))) (void) 0; // Wait 70 | RCC->CFGR |= 2; // Use PLL 71 | rtos_freq_set(64000000); 72 | SysTick_Config(rtos_freq_get() / 1000); // 1KHz SysTick 73 | } 74 | 75 | struct uart { 76 | volatile uint32_t CR1, CR2, CR3, BRR, GTPR, RTOR, RQR, ISR, ICR; 77 | volatile uint16_t RDR, RESERVED1, TDR, RESERVED2; 78 | }; 79 | #define UART1 ((struct uart *) 0x40013800) 80 | #define UART2 ((struct uart *) 0x40004400) 81 | #define UART3 ((struct uart *) 0x40004800) 82 | 83 | static inline void uart_init(struct uart *uart, unsigned long baud) { 84 | // https://www.st.com/resource/en/datasheet/stm32f303k8.pdf 85 | uint8_t af = 0; // Alternate function 86 | uint16_t rx = 0, tx = 0; // pins 87 | if (uart == UART1) af = 7, tx = PIN('A', 9), rx = PIN('A', 10); 88 | if (uart == UART2) af = 7, tx = PIN('A', 2), rx = PIN('A', 15); 89 | if (uart == UART3) af = 7, tx = PIN('B', 10), rx = PIN('B', 11); 90 | gpio_init(tx, GPIO_MODE_AF, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 0, af); 91 | gpio_init(rx, GPIO_MODE_AF, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 0, af); 92 | uart->BRR = rtos_freq_get() / baud; // Set baud rate, TRM 29.5.4 93 | uart->CR1 |= BIT(0) | BIT(2) | BIT(3); // Enable this UART, RE, TE 94 | } 95 | 96 | static inline void uart_write_byte(struct uart *uart, uint8_t byte) { 97 | uart->TDR = byte; 98 | while ((uart->ISR & BIT(7)) == 0) (void) 0; 99 | } 100 | 101 | static inline void uart_write_buf(struct uart *uart, char *buf, size_t len) { 102 | while (len-- > 0) uart_write_byte(uart, *(uint8_t *) buf++); 103 | } 104 | 105 | static inline int uart_read_ready(struct uart *uart) { 106 | return uart->ISR & BIT(5); // If RXNE bit is set, data is ready 107 | } 108 | 109 | static inline uint8_t uart_read_byte(struct uart *uart) { 110 | return (uint8_t)(uart->RDR & 255); 111 | } 112 | 113 | #ifndef LED1 114 | #define LED1 PIN('B', 3) // On-board LED pin 115 | #endif 116 | 117 | #ifndef UART 118 | #define UART UART2 119 | #endif 120 | 121 | static inline void rtos_init(void) { 122 | init_ram(); 123 | init_clock(); 124 | 125 | RCC->AHBENR |= BIT(17) | BIT(18) | BIT(19); // Enable GPIO banks A,B,C 126 | RCC->APB2ENR |= BIT(14); // Enable USART1 127 | RCC->APB1ENR |= BIT(17) | BIT(18); // Enable USART2 and USART3 128 | } 129 | -------------------------------------------------------------------------------- /arch/stm32f3/openocd.cfg: -------------------------------------------------------------------------------- 1 | source [find interface/stlink-v2-1.cfg] 2 | transport select hla_swd 3 | source [find target/stm32f3x.cfg] 4 | reset_config srst_only srst_nogate 5 | -------------------------------------------------------------------------------- /arch/stm32f7/build.mk: -------------------------------------------------------------------------------- 1 | SELF_DIR = $(dir $(lastword $(MAKEFILE_LIST))) 2 | MCU_FLAGS ?= -mcpu=cortex-m7 -mthumb -mfpu=vfpv4 3 | 4 | include $(SELF_DIR)/../../arch/common.mk 5 | -------------------------------------------------------------------------------- /arch/stm32f7/link.ld: -------------------------------------------------------------------------------- 1 | MEMORY { 2 | ram(rwx) : ORIGIN = 0x20000000, LENGTH = 320k 3 | rom(rx) : ORIGIN = 0x08000000, LENGTH = 1024k 4 | } 5 | -------------------------------------------------------------------------------- /arch/stm32f7/mcu.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | // https://www.st.com/resource/en/reference_manual/dm00124865-stm32f75xxx-and-stm32f74xxx-advanced-arm-based-32-bit-mcus-stmicroelectronics.pdf 4 | // Memory map: 2.2.2 5 | 6 | #pragma once 7 | 8 | #include "arch/common.h" 9 | 10 | struct gpio { 11 | volatile uint32_t MODER, OTYPER, OSPEEDR, PUPDR, IDR, ODR, BSRR, LCKR, AFR[2]; 12 | }; 13 | 14 | static inline struct gpio *gpio_bank(uint16_t pin) { 15 | return (struct gpio *) (0x40020000 + 0x400 * (pin >> 8)); 16 | } 17 | static inline void gpio_on(uint16_t pin) { 18 | gpio_bank(pin)->ODR |= BIT(pin & 255); 19 | } 20 | static inline void gpio_off(uint16_t pin) { 21 | gpio_bank(pin)->ODR &= ~BIT(pin & 255); 22 | } 23 | static inline void gpio_toggle(uint16_t pin) { 24 | gpio_bank(pin)->ODR ^= BIT(pin & 255); 25 | } 26 | static inline void gpio_init(uint16_t pin, uint8_t mode, uint8_t type, 27 | uint8_t speed, uint8_t pull, uint8_t af) { 28 | struct gpio *gpio = gpio_bank(pin); 29 | uint8_t n = (uint8_t)(pin & 255); 30 | gpio->MODER &= ~(3UL << (n * 2)); 31 | gpio->MODER |= ((uint32_t) mode) << (n * 2); 32 | gpio->OTYPER &= ~(1UL << n); 33 | gpio->OTYPER |= ((uint32_t) type) << n; 34 | gpio->OSPEEDR &= ~(3UL << (n * 2)); 35 | gpio->OSPEEDR |= ((uint32_t) speed) << (n * 2); 36 | gpio->PUPDR &= ~(3UL << (n * 2)); 37 | gpio->PUPDR |= ((uint32_t) pull) << (n * 2); 38 | if (n < 8) { 39 | gpio->AFR[0] &= 15UL << (n * 4); 40 | gpio->AFR[0] |= ((uint32_t) af) << (n * 4); 41 | } else { 42 | gpio->AFR[1] &= 15UL << ((n - 8) * 4); 43 | gpio->AFR[1] |= ((uint32_t) af) << ((n - 8) * 4); 44 | } 45 | } 46 | static inline void gpio_input(uint16_t pin) { 47 | gpio_init(pin, GPIO_MODE_INPUT, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 48 | GPIO_PULL_NONE, 0); 49 | } 50 | static inline void gpio_output(uint16_t pin) { 51 | gpio_init(pin, GPIO_MODE_OUTPUT, GPIO_OTYPE_PUSH_PULL, GPIO_SPEED_MEDIUM, 52 | GPIO_PULL_NONE, 0); 53 | } 54 | 55 | struct rcc { 56 | volatile uint32_t CR, PLLCFGR, CFGR, CIR, AHB1RSTR, AHB2RSTR, AHB3RSTR, 57 | RESERVED0, APB1RSTR, APB2RSTR, RESERVED1[2], AHB1ENR, AHB2ENR, AHB3ENR, 58 | RESERVED2, APB1ENR, APB2ENR, RESERVED3[2], AHB1LPENR, AHB2LPENR, 59 | AHB3LPENR, RESERVED4, APB1LPENR, APB2LPENR, RESERVED5[2], BDCR, CSR, 60 | RESERVED6[2], SSCGR, PLLI2SCFGR, PLLSAICFGR, DCKCFGR1, DCKCFGR2; 61 | }; 62 | #define RCC ((struct rcc *) 0x40023800) 63 | 64 | static inline void init_clock(void) { 65 | FLASH->ACR = 0x12; // Set flash 66 | RCC->CR |= BIT(0); // HSI ON, 16 MHz 67 | while (!(RCC->CR & BIT(1))) (void) 0; // Wait 68 | 69 | // f(VCO clock) = f(PLL clock input) x (PLLN / PLLM) 70 | // f(PLL general clock) = f(VCO clock) / PLLP 71 | // f(USB OTG FS, SDMMC, RNG) = f(VCO clock) / PLLQ 72 | // PLLM PLLN PLLP SRC = HSI 73 | RCC->PLLCFGR &= ~((63UL << 0) | (511UL << 6) | (3UL << 16) | (1UL << 22)); 74 | RCC->PLLCFGR |= (32UL << 0) | (54UL << 6) | (0UL << 16); 75 | // RCC->PLLCFGR |= (2UL << 0) | (54UL << 6) | (0UL << 16); 76 | 77 | RCC->CR |= BIT(24); // PLL ON 78 | while (!(RCC->CR & BIT(25))) (void) 0; // Wait 79 | RCC->CFGR &= ~3UL; // Clear clock source 80 | RCC->CFGR |= 2; // Set PLL clock source 81 | 82 | rtos_freq_set(16000000); 83 | SysTick_Config(rtos_freq_get() / 1000); // 1KHz SysTick 84 | } 85 | 86 | #define LED1 PIN('B', 0) // On-board LED pin 87 | #define LED2 PIN('B', 7) // On-board LED pin 88 | #define LED3 PIN('B', 14) // On-board LED pin 89 | 90 | static inline void rtos_init(void) { 91 | init_ram(); 92 | init_clock(); 93 | 94 | RCC->AHB1ENR |= BIT(0) | BIT(1) | BIT(2) | BIT(6); // Init GPIO banks A,B,C,G 95 | gpio_output(LED1); 96 | gpio_output(LED2); 97 | gpio_output(LED3); 98 | gpio_on(LED2); 99 | } 100 | -------------------------------------------------------------------------------- /arch/stm32f7/openocd.cfg: -------------------------------------------------------------------------------- 1 | source [find interface/stlink-v2-1.cfg] 2 | transport select hla_swd 3 | source [find target/stm32f7x.cfg] 4 | reset_config srst_only srst_nogate 5 | -------------------------------------------------------------------------------- /examples/blinky/Makefile: -------------------------------------------------------------------------------- 1 | ARCH ?= stm32f1 2 | SOURCES = main.c 3 | 4 | include ../../arch/$(ARCH)/build.mk 5 | -------------------------------------------------------------------------------- /examples/blinky/main.c: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | 4 | #include 5 | 6 | #include "mcu.h" 7 | 8 | static void taskfunc(void *param) { 9 | unsigned long delay = *(unsigned long *) param; 10 | 11 | for (;;) { 12 | rtos_msleep(delay); 13 | gpio_toggle(LED1); 14 | DEBUG(("%lu, RAM: %d\n", delay, rtos_ram_free())); 15 | } 16 | } 17 | 18 | int main(void) { 19 | rtos_init(); 20 | DEBUG(("free RAM: %d\n", rtos_ram_free())); 21 | 22 | gpio_output(LED1); 23 | // for (;;) rtos_msleep(500), gpio_toggle(LED1); 24 | 25 | // Run two blinking tasks with different intervals 26 | unsigned long arg1 = 500, arg2 = 700; 27 | rtos_task_create(taskfunc, &arg1, 512); 28 | rtos_task_create(taskfunc, &arg2, 512); 29 | 30 | rtos_schedule(); 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /examples/uart-echo/Makefile: -------------------------------------------------------------------------------- 1 | ARCH ?= stm32f3 2 | SOURCES = main.c 3 | 4 | include ../../arch/$(ARCH)/build.mk 5 | -------------------------------------------------------------------------------- /examples/uart-echo/main.c: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | 4 | #include 5 | 6 | #include "mcu.h" 7 | 8 | static void taskfunc(void *param) { 9 | struct uart *uart = param; 10 | for (;;) { 11 | if (uart_read_ready(uart)) { // Is UART readable? 12 | uint8_t byte = uart_read_byte(uart); // Read one byte 13 | uart_write_byte(uart, byte); // And write it back 14 | } 15 | } 16 | } 17 | 18 | int main(void) { 19 | rtos_init(); 20 | uart_init(UART, 115200); // UART, UART_TX and UART_RX 21 | rtos_task_create(taskfunc, UART, 512); // are defined in mcu.h 22 | rtos_schedule(); 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /examples/uart-json-rpc/Makefile: -------------------------------------------------------------------------------- 1 | ARCH ?= stm32f3 2 | SOURCES = main.c mjson.c 3 | 4 | include ../../arch/$(ARCH)/build.mk 5 | -------------------------------------------------------------------------------- /examples/uart-json-rpc/README.md: -------------------------------------------------------------------------------- 1 | This example demonstrates how to implement a JSON-RPC server over UART. 2 | JSON-RPC frames are separated by new lines. 3 | 4 | This firmware implements two RPC methods: 5 | - `math.sum`. Expected params: array with two numbers. Returns a sum of two numbers 6 | - `led`. Expected params: object with attr `on`. Sets LED on or off 7 | 8 | Build and flash the example: 9 | 10 | ```sh 11 | $ make clean all flash 12 | ``` 13 | 14 | Use any terminal program to send JSON-RPC frames and get responses. 15 | In the example below, we use `miniterm` - but any other program would 16 | too, like `cu`, `minicom`, or `putty` on Windows. 17 | 18 | Start `miniterm` with local echo turned on (note that the exact 19 | device name could be different on your machine): 20 | 21 | ```sh 22 | $ miniterm -q -e /dev/cu.usbmodem14202 115200 23 | ``` 24 | 25 | Now `miniterm` waits for an input. 26 | Copy a `{"id":1, "method":"rpc.list"}` string, which is a JSON-RPC 27 | request, paste into `miniterm`, and press Enter to send a finishing 28 | new line. We should see a response with a list of registered RPC methods: 29 | 30 | ```sh 31 | {"id":1,"method":"rpc.list"} 32 | {"id":1,"result":["math.sum","led","rpc.list"]} 33 | ``` 34 | 35 | Now let's call our function `sum`: 36 | 37 | ```sh 38 | {"id":2, "method":"math.sum", "params": [1.23,2.73]} 39 | {"id":2,"result":{"result":3.96}} 40 | ``` 41 | 42 | Turn LED on: 43 | 44 | ```sh 45 | {"id":3, "method":"led", "params": {"on": true}} 46 | {"id":3,"result":{"ram":9724}} 47 | ``` 48 | 49 | The LED lits up! The response shows free RAM available - on your 50 | board it could be different. Turn LED off: 51 | 52 | ```sh 53 | {"id":3, "method":"led", "params": {"on": false}} 54 | {"id":3,"result":{"ram":9724}} 55 | ``` 56 | 57 | That's it! Congratulations, now you could register more functions 58 | on your board and call them via UART. 59 | 60 | Hint: if you wire an ESP32 to your board in a way described at 61 | https://vcon.io, you could call your JSON-RPC functions remotely 62 | via the https://dash.vcon.io cloud. 63 | -------------------------------------------------------------------------------- /examples/uart-json-rpc/main.c: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | 4 | #include "mcu.h" 5 | #include "mjson.h" 6 | 7 | static void gpio_led_rpc(struct jsonrpc_request *r) { 8 | int on = 0; 9 | if (mjson_get_bool(r->params, r->params_len, "$.on", &on)) { 10 | if (on) gpio_on(LED1); 11 | if (!on) gpio_off(LED1); 12 | jsonrpc_return_success(r, "{%Q:%d}", "ram", rtos_ram_free()); 13 | } else { 14 | jsonrpc_return_error(r, 400, "set 'on' to true or false", NULL); 15 | } 16 | } 17 | 18 | static void math_sum_rpc(struct jsonrpc_request *r) { 19 | double a, b; 20 | if (mjson_get_number(r->params, r->params_len, "$[0]", &a) && 21 | mjson_get_number(r->params, r->params_len, "$[1]", &b)) { 22 | jsonrpc_return_success(r, "{%Q:%g}", "result", a + b); 23 | } else { 24 | jsonrpc_return_error(r, 400, "pass [NUM1, NUM2]", NULL); 25 | } 26 | } 27 | 28 | static void taskfunc(void *param) { 29 | struct uart *uart = param; 30 | char buf[1024]; // UART input buffer 31 | int len = 0; // Number of bytes already read 32 | 33 | // Set up JSON-RPC server 34 | jsonrpc_init(NULL, NULL); 35 | jsonrpc_export("led", gpio_led_rpc); 36 | jsonrpc_export("math.sum", math_sum_rpc); 37 | 38 | for (;;) { 39 | if (uart_read_ready(uart)) { // Is UART readable? 40 | uint8_t byte = uart_read_byte(uart); // Read one byte 41 | if (byte == '\n') { // Newline? We've got an RPC frame 42 | char *response = NULL; // Prepare response 43 | jsonrpc_process(buf, len, mjson_print_dynamic_buf, &response, NULL); 44 | if (response != NULL) { 45 | uart_write_buf(uart, response, strlen(response)); 46 | free(response); 47 | } 48 | len = 0; 49 | } else { 50 | buf[len++] = byte; // Append to the input buffer 51 | if (len >= (int) sizeof(buf)) len = 0; // Flush on overflow 52 | } 53 | } 54 | } 55 | } 56 | 57 | int main(void) { 58 | rtos_init(); 59 | gpio_output(LED1); 60 | uart_init(UART, 115200); 61 | #if 0 62 | for (;;) rtos_msleep(500), gpio_toggle(LED1), uart_write_buf(UART, "hi\n", 3); 63 | #endif 64 | rtos_task_create(taskfunc, UART, 2048); // are defined in mcu.h 65 | rtos_schedule(); 66 | return 0; 67 | } 68 | -------------------------------------------------------------------------------- /examples/uart-json-rpc/mjson.c: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018-2020 Cesanta Software Limited 2 | // All rights reserved 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #include 23 | #include 24 | 25 | #include "mjson.h" 26 | 27 | #if defined(_MSC_VER) 28 | #define alloca(x) _alloca(x) 29 | #endif 30 | 31 | #if defined(_MSC_VER) && _MSC_VER < 1700 32 | #define va_copy(x, y) (x) = (y) 33 | #define isinf(x) !_finite(x) 34 | #define isnan(x) _isnan(x) 35 | #endif 36 | 37 | static double mystrtod(const char *str, char **end); 38 | 39 | static int mjson_esc(int c, int esc) { 40 | const char *p, *esc1 = "\b\f\n\r\t\\\"", *esc2 = "bfnrt\\\""; 41 | for (p = esc ? esc1 : esc2; *p != '\0'; p++) { 42 | if (*p == c) return esc ? esc2[p - esc1] : esc1[p - esc2]; 43 | } 44 | return 0; 45 | } 46 | 47 | static int mjson_escape(int c) { 48 | return mjson_esc(c, 1); 49 | } 50 | 51 | static int mjson_pass_string(const char *s, int len) { 52 | int i; 53 | for (i = 0; i < len; i++) { 54 | if (s[i] == '\\' && i + 1 < len && mjson_escape(s[i + 1])) { 55 | i++; 56 | } else if (s[i] == '\0') { 57 | return MJSON_ERROR_INVALID_INPUT; 58 | } else if (s[i] == '"') { 59 | return i; 60 | } 61 | } 62 | return MJSON_ERROR_INVALID_INPUT; 63 | } 64 | 65 | int mjson(const char *s, int len, mjson_cb_t cb, void *ud) { 66 | enum { S_VALUE, S_KEY, S_COLON, S_COMMA_OR_EOO } expecting = S_VALUE; 67 | unsigned char nesting[MJSON_MAX_DEPTH]; 68 | int i, depth = 0; 69 | #define MJSONCALL(ev) \ 70 | if (cb != NULL && cb(ev, s, start, i - start + 1, ud)) return i + 1; 71 | 72 | // In the ascii table, the distance between `[` and `]` is 2. 73 | // Ditto for `{` and `}`. Hence +2 in the code below. 74 | #define MJSONEOO() \ 75 | do { \ 76 | if (c != nesting[depth - 1] + 2) return MJSON_ERROR_INVALID_INPUT; \ 77 | depth--; \ 78 | if (depth == 0) { \ 79 | MJSONCALL(tok); \ 80 | return i + 1; \ 81 | } \ 82 | } while (0) 83 | 84 | for (i = 0; i < len; i++) { 85 | int start = i; 86 | unsigned char c = ((unsigned char *) s)[i]; 87 | int tok = c; 88 | if (c == ' ' || c == '\t' || c == '\n' || c == '\r') continue; 89 | // printf("- %c [%.*s] %d %d\n", c, i, s, depth, expecting); 90 | switch (expecting) { 91 | case S_VALUE: 92 | if (c == '{') { 93 | if (depth >= (int) sizeof(nesting)) return MJSON_ERROR_TOO_DEEP; 94 | nesting[depth++] = c; 95 | expecting = S_KEY; 96 | break; 97 | } else if (c == '[') { 98 | if (depth >= (int) sizeof(nesting)) return MJSON_ERROR_TOO_DEEP; 99 | nesting[depth++] = c; 100 | break; 101 | } else if (c == ']' && depth > 0) { // Empty array 102 | MJSONEOO(); 103 | } else if (c == 't' && i + 3 < len && memcmp(&s[i], "true", 4) == 0) { 104 | i += 3; 105 | tok = MJSON_TOK_TRUE; 106 | } else if (c == 'n' && i + 3 < len && memcmp(&s[i], "null", 4) == 0) { 107 | i += 3; 108 | tok = MJSON_TOK_NULL; 109 | } else if (c == 'f' && i + 4 < len && memcmp(&s[i], "false", 5) == 0) { 110 | i += 4; 111 | tok = MJSON_TOK_FALSE; 112 | } else if (c == '-' || ((c >= '0' && c <= '9'))) { 113 | char *end = NULL; 114 | mystrtod(&s[i], &end); 115 | if (end != NULL) i += (int) (end - &s[i] - 1); 116 | tok = MJSON_TOK_NUMBER; 117 | } else if (c == '"') { 118 | int n = mjson_pass_string(&s[i + 1], len - i - 1); 119 | if (n < 0) return n; 120 | i += n + 1; 121 | tok = MJSON_TOK_STRING; 122 | } else { 123 | return MJSON_ERROR_INVALID_INPUT; 124 | } 125 | if (depth == 0) { 126 | MJSONCALL(tok); 127 | return i + 1; 128 | } 129 | expecting = S_COMMA_OR_EOO; 130 | break; 131 | 132 | case S_KEY: 133 | if (c == '"') { 134 | int n = mjson_pass_string(&s[i + 1], len - i - 1); 135 | if (n < 0) return n; 136 | i += n + 1; 137 | tok = MJSON_TOK_KEY; 138 | expecting = S_COLON; 139 | } else if (c == '}') { // Empty object 140 | MJSONEOO(); 141 | expecting = S_COMMA_OR_EOO; 142 | } else { 143 | return MJSON_ERROR_INVALID_INPUT; 144 | } 145 | break; 146 | 147 | case S_COLON: 148 | if (c == ':') { 149 | expecting = S_VALUE; 150 | } else { 151 | return MJSON_ERROR_INVALID_INPUT; 152 | } 153 | break; 154 | 155 | case S_COMMA_OR_EOO: 156 | if (depth <= 0) return MJSON_ERROR_INVALID_INPUT; 157 | if (c == ',') { 158 | expecting = (nesting[depth - 1] == '{') ? S_KEY : S_VALUE; 159 | } else if (c == ']' || c == '}') { 160 | MJSONEOO(); 161 | } else { 162 | return MJSON_ERROR_INVALID_INPUT; 163 | } 164 | break; 165 | } 166 | MJSONCALL(tok); 167 | } 168 | return MJSON_ERROR_INVALID_INPUT; 169 | } 170 | 171 | struct msjon_get_data { 172 | const char *path; // Lookup json path 173 | int pos; // Current path index 174 | int d1; // Current depth of traversal 175 | int d2; // Expected depth of traversal 176 | int i1; // Index in an array 177 | int i2; // Expected index in an array 178 | int obj; // If the value is array/object, offset where it starts 179 | const char **tokptr; // Destination 180 | int *toklen; // Destination length 181 | int tok; // Returned token 182 | }; 183 | 184 | #include 185 | 186 | static int plen1(const char *s) { 187 | int i = 0, n = 0; 188 | while (s[i] != '\0' && s[i] != '.' && s[i] != '[') 189 | n++, i += s[i] == '\\' ? 2 : 1; 190 | // printf("PLEN: s: [%s], [%.*s] => %d\n", s, i, s, n); 191 | return n; 192 | } 193 | 194 | static int plen2(const char *s) { 195 | int i = 0, n = 0; 196 | while (s[i] != '\0' && s[i] != '.' && s[i] != '[') 197 | n++, i += s[i] == '\\' ? 2 : 1; 198 | // printf("PLEN: s: [%s], [%.*s] => %d\n", s, i, s, n); 199 | return i; 200 | } 201 | 202 | static int kcmp(const char *a, const char *b, int n) { 203 | int i = 0, j = 0, r = 0; 204 | for (i = 0, j = 0; j < n; i++, j++) { 205 | if (b[i] == '\\') i++; 206 | if ((r = a[j] - b[i]) != 0) return r; 207 | } 208 | // printf("KCMP: a: [%.*s], b:[%.*s] ==> %d\n", n, a, i, b, r); 209 | return r; 210 | } 211 | 212 | static int mjson_get_cb(int tok, const char *s, int off, int len, void *ud) { 213 | struct msjon_get_data *data = (struct msjon_get_data *) ud; 214 | // printf("--> %2x %2d %2d %2d %2d\t'%s'\t'%.*s'\t\t'%.*s'\n", tok, data->d1, 215 | // data->d2, data->i1, data->i2, data->path + data->pos, off, s, len, 216 | // s + off); 217 | if (data->tok != MJSON_TOK_INVALID) return 1; // Found 218 | 219 | if (tok == '{') { 220 | if (!data->path[data->pos] && data->d1 == data->d2) data->obj = off; 221 | data->d1++; 222 | } else if (tok == '[') { 223 | if (data->d1 == data->d2 && data->path[data->pos] == '[') { 224 | data->i1 = 0; 225 | data->i2 = (int) mystrtod(&data->path[data->pos + 1], NULL); 226 | if (data->i1 == data->i2) { 227 | data->d2++; 228 | data->pos += 3; 229 | } 230 | } 231 | if (!data->path[data->pos] && data->d1 == data->d2) data->obj = off; 232 | data->d1++; 233 | } else if (tok == ',') { 234 | if (data->d1 == data->d2 + 1) { 235 | data->i1++; 236 | if (data->i1 == data->i2) { 237 | while (data->path[data->pos] != ']') data->pos++; 238 | data->pos++; 239 | data->d2++; 240 | } 241 | } 242 | } else if (tok == MJSON_TOK_KEY && data->d1 == data->d2 + 1 && 243 | data->path[data->pos] == '.' && s[off] == '"' && 244 | s[off + len - 1] == '"' && 245 | plen1(&data->path[data->pos + 1]) == len - 2 && 246 | kcmp(s + off + 1, &data->path[data->pos + 1], len - 2) == 0) { 247 | data->d2++; 248 | data->pos += plen2(&data->path[data->pos + 1]) + 1; 249 | } else if (tok == MJSON_TOK_KEY && data->d1 == data->d2) { 250 | return 1; // Exhausted path, not found 251 | } else if (tok == '}' || tok == ']') { 252 | data->d1--; 253 | // data->d2--; 254 | if (!data->path[data->pos] && data->d1 == data->d2 && data->obj != -1) { 255 | data->tok = tok - 2; 256 | if (data->tokptr) *data->tokptr = s + data->obj; 257 | if (data->toklen) *data->toklen = off - data->obj + 1; 258 | return 1; 259 | } 260 | } else if (MJSON_TOK_IS_VALUE(tok)) { 261 | // printf("TOK --> %d\n", tok); 262 | if (data->d1 == data->d2 && !data->path[data->pos]) { 263 | data->tok = tok; 264 | if (data->tokptr) *data->tokptr = s + off; 265 | if (data->toklen) *data->toklen = len; 266 | return 1; 267 | } 268 | } 269 | return 0; 270 | } 271 | 272 | int mjson_find(const char *s, int n, const char *jp, const char **tp, int *tl) { 273 | struct msjon_get_data data = {jp, 1, 0, 0, 0, 274 | 0, -1, tp, tl, MJSON_TOK_INVALID}; 275 | if (jp[0] != '$') return MJSON_TOK_INVALID; 276 | if (mjson(s, n, mjson_get_cb, &data) < 0) return MJSON_TOK_INVALID; 277 | return data.tok; 278 | } 279 | 280 | int mjson_get_number(const char *s, int len, const char *path, double *v) { 281 | const char *p; 282 | int tok, n; 283 | if ((tok = mjson_find(s, len, path, &p, &n)) == MJSON_TOK_NUMBER) { 284 | if (v != NULL) *v = mystrtod(p, NULL); 285 | } 286 | return tok == MJSON_TOK_NUMBER ? 1 : 0; 287 | } 288 | 289 | int mjson_get_bool(const char *s, int len, const char *path, int *v) { 290 | int tok = mjson_find(s, len, path, NULL, NULL); 291 | if (tok == MJSON_TOK_TRUE && v != NULL) *v = 1; 292 | if (tok == MJSON_TOK_FALSE && v != NULL) *v = 0; 293 | return tok == MJSON_TOK_TRUE || tok == MJSON_TOK_FALSE ? 1 : 0; 294 | } 295 | 296 | static unsigned char unhex(unsigned char c) { 297 | return (c >= '0' && c <= '9') ? (unsigned char) (c - '0') 298 | : (c >= 'A' && c <= 'F') ? (unsigned char) (c - '7') 299 | : (unsigned char) (c - 'W'); 300 | } 301 | 302 | static unsigned char mjson_unhex_nimble(const char *s) { 303 | return (unsigned char) (unhex(((unsigned char *) s)[0]) << 4) | 304 | unhex(((unsigned char *) s)[1]); 305 | } 306 | 307 | static int mjson_unescape(const char *s, int len, char *to, int n) { 308 | int i, j; 309 | for (i = 0, j = 0; i < len && j < n; i++, j++) { 310 | if (s[i] == '\\' && i + 5 < len && s[i + 1] == 'u') { 311 | // \uXXXX escape. We could process a simple one-byte chars 312 | // \u00xx from the ASCII range. More complex chars would require 313 | // dragging in a UTF8 library, which is too much for us 314 | if (s[i + 2] != '0' || s[i + 3] != '0') return -1; // Too much, give up 315 | ((unsigned char *) to)[j] = mjson_unhex_nimble(s + i + 4); 316 | i += 5; 317 | } else if (s[i] == '\\' && i + 1 < len) { 318 | int c = mjson_esc(s[i + 1], 0); 319 | if (c == 0) return -1; 320 | to[j] = (char) (unsigned char) c; 321 | i++; 322 | } else { 323 | to[j] = s[i]; 324 | } 325 | } 326 | if (j >= n) return -1; 327 | if (n > 0) to[j] = '\0'; 328 | return j; 329 | } 330 | 331 | int mjson_get_string(const char *s, int len, const char *path, char *to, 332 | int n) { 333 | const char *p; 334 | int sz; 335 | if (mjson_find(s, len, path, &p, &sz) != MJSON_TOK_STRING) return -1; 336 | return mjson_unescape(p + 1, sz - 2, to, n); 337 | } 338 | 339 | int mjson_get_hex(const char *s, int len, const char *x, char *to, int n) { 340 | const char *p; 341 | int i, j, sz; 342 | if (mjson_find(s, len, x, &p, &sz) != MJSON_TOK_STRING) return -1; 343 | for (i = j = 0; i < sz - 3 && j < n; i += 2, j++) { 344 | ((unsigned char *) to)[j] = mjson_unhex_nimble(p + i + 1); 345 | } 346 | if (j < n) to[j] = '\0'; 347 | return j; 348 | } 349 | 350 | #if MJSON_ENABLE_BASE64 351 | static unsigned char mjson_base64rev(int c) { 352 | if (c >= 'A' && c <= 'Z') { 353 | return (unsigned char) (c - 'A'); 354 | } else if (c >= 'a' && c <= 'z') { 355 | return (unsigned char) (c + 26 - 'a'); 356 | } else if (c >= '0' && c <= '9') { 357 | return (unsigned char) (c + 52 - '0'); 358 | } else if (c == '+') { 359 | return 62; 360 | } else if (c == '/') { 361 | return 63; 362 | } else { 363 | return 64; 364 | } 365 | } 366 | 367 | int mjson_base64_dec(const char *src, int n, char *dst, int dlen) { 368 | const char *end = src + n; 369 | int len = 0; 370 | while (src + 3 < end && len < dlen) { 371 | unsigned char a = mjson_base64rev(src[0]), b = mjson_base64rev(src[1]), 372 | c = mjson_base64rev(src[2]), d = mjson_base64rev(src[3]); 373 | dst[len++] = (char) (unsigned char) ((a << 2) | (b >> 4)); 374 | if (src[2] != '=' && len < dlen) { 375 | dst[len++] = (char) (unsigned char) ((b << 4) | (c >> 2)); 376 | if (src[3] != '=' && len < dlen) { 377 | dst[len++] = (char) (unsigned char) ((c << 6) | d); 378 | } 379 | } 380 | src += 4; 381 | } 382 | if (len < dlen) dst[len] = '\0'; 383 | return len; 384 | } 385 | 386 | int mjson_get_base64(const char *s, int len, const char *path, char *to, 387 | int n) { 388 | const char *p; 389 | int sz; 390 | if (mjson_find(s, len, path, &p, &sz) != MJSON_TOK_STRING) return 0; 391 | return mjson_base64_dec(p + 1, sz - 2, to, n); 392 | } 393 | #endif // MJSON_ENABLE_BASE64 394 | 395 | #if MJSON_ENABLE_NEXT 396 | struct nextdata { 397 | int off, len, depth, t, vo, arrayindex; 398 | int *koff, *klen, *voff, *vlen, *vtype; 399 | }; 400 | 401 | static int next_cb(int tok, const char *s, int off, int len, void *ud) { 402 | struct nextdata *d = (struct nextdata *) ud; 403 | // int i; 404 | switch (tok) { 405 | case '{': 406 | case '[': 407 | if (d->depth == 0 && tok == '[') d->arrayindex = 0; 408 | if (d->depth == 1 && off > d->off) { 409 | d->vo = off; 410 | d->t = tok == '{' ? MJSON_TOK_OBJECT : MJSON_TOK_ARRAY; 411 | if (d->voff) *d->voff = off; 412 | if (d->vtype) *d->vtype = d->t; 413 | } 414 | d->depth++; 415 | break; 416 | case '}': 417 | case ']': 418 | d->depth--; 419 | if (d->depth == 1 && d->vo) { 420 | d->len = off + len; 421 | if (d->vlen) *d->vlen = d->len - d->vo; 422 | if (d->arrayindex >= 0) { 423 | if (d->koff) *d->koff = d->arrayindex; // koff holds array index 424 | if (d->klen) *d->klen = 0; // klen holds 0 425 | } 426 | return 1; 427 | } 428 | if (d->depth == 1 && d->arrayindex >= 0) d->arrayindex++; 429 | break; 430 | case ',': 431 | case ':': 432 | break; 433 | case MJSON_TOK_KEY: 434 | if (d->depth == 1 && d->off < off) { 435 | if (d->koff) *d->koff = off; // And report back to the user 436 | if (d->klen) *d->klen = len; // If we have to 437 | } 438 | break; 439 | default: 440 | if (d->depth != 1) break; 441 | // If we're iterating over the array 442 | if (off > d->off) { 443 | d->len = off + len; 444 | if (d->vlen) *d->vlen = len; // value length 445 | if (d->voff) *d->voff = off; // value offset 446 | if (d->vtype) *d->vtype = tok; // value type 447 | if (d->arrayindex >= 0) { 448 | if (d->koff) *d->koff = d->arrayindex; // koff holds array index 449 | if (d->klen) *d->klen = 0; // klen holds 0 450 | } 451 | return 1; 452 | } 453 | if (d->arrayindex >= 0) d->arrayindex++; 454 | break; 455 | } 456 | (void) s; 457 | return 0; 458 | } 459 | 460 | int mjson_next(const char *s, int n, int off, int *koff, int *klen, int *voff, 461 | int *vlen, int *vtype) { 462 | struct nextdata d = {off, 0, 0, 0, 0, -1, koff, klen, voff, vlen, vtype}; 463 | mjson(s, n, next_cb, &d); 464 | return d.len; 465 | } 466 | #endif 467 | 468 | #if MJSON_ENABLE_PRINT 469 | int mjson_print_fixed_buf(const char *ptr, int len, void *fndata) { 470 | struct mjson_fixedbuf *fb = (struct mjson_fixedbuf *) fndata; 471 | int i, left = fb->size - 1 - fb->len; 472 | if (left < len) len = left; 473 | for (i = 0; i < len; i++) fb->ptr[fb->len + i] = ptr[i]; 474 | fb->len += len; 475 | fb->ptr[fb->len] = '\0'; 476 | return len; 477 | } 478 | 479 | // This function allocates memory in chunks of size MJSON_DYNBUF_CHUNK 480 | // to decrease memory fragmentation, when many calls are executed to 481 | // print e.g. a base64 string or a hex string. 482 | int mjson_print_dynamic_buf(const char *ptr, int len, void *fndata) { 483 | char *s, *buf = *(char **) fndata; 484 | size_t curlen = buf == NULL ? 0 : strlen(buf); 485 | size_t new_size = curlen + (size_t) len + 1 + MJSON_DYNBUF_CHUNK; 486 | new_size -= new_size % MJSON_DYNBUF_CHUNK; 487 | 488 | if ((s = (char *) realloc(buf, new_size)) == NULL) { 489 | return 0; 490 | } else { 491 | memcpy(s + curlen, ptr, (size_t) len); 492 | s[curlen + (size_t) len] = '\0'; 493 | *(char **) fndata = s; 494 | return len; 495 | } 496 | } 497 | 498 | int mjson_snprintf(char *buf, size_t len, const char *fmt, ...) { 499 | va_list ap; 500 | struct mjson_fixedbuf fb = {buf, (int) len, 0}; 501 | va_start(ap, fmt); 502 | mjson_vprintf(mjson_print_fixed_buf, &fb, fmt, &ap); 503 | va_end(ap); 504 | return fb.len; 505 | } 506 | 507 | char *mjson_aprintf(const char *fmt, ...) { 508 | va_list ap; 509 | char *result = NULL; 510 | va_start(ap, fmt); 511 | mjson_vprintf(mjson_print_dynamic_buf, &result, fmt, &ap); 512 | va_end(ap); 513 | return result; 514 | } 515 | 516 | int mjson_print_null(const char *ptr, int len, void *userdata) { 517 | (void) ptr; 518 | (void) userdata; 519 | return len; 520 | } 521 | 522 | int mjson_print_buf(mjson_print_fn_t fn, void *fnd, const char *buf, int len) { 523 | return fn(buf, len, fnd); 524 | } 525 | 526 | int mjson_print_long(mjson_print_fn_t fn, void *fnd, long val, int is_signed) { 527 | unsigned long v = (unsigned long) val, s = 0, n, i; 528 | char buf[20], t; 529 | if (is_signed && val < 0) buf[s++] = '-', v = (unsigned long) (-val); 530 | // This loop prints a number in reverse order. I guess this is because we 531 | // write numbers from right to left: least significant digit comes last. 532 | // Maybe because we use Arabic numbers, and Arabs write RTL? 533 | for (n = 0; v > 0; v /= 10) buf[s + n++] = "0123456789"[v % 10]; 534 | // Reverse a string 535 | for (i = 0; i < n / 2; i++) 536 | t = buf[s + i], buf[s + i] = buf[s + n - i - 1], buf[s + n - i - 1] = t; 537 | if (val == 0) buf[n++] = '0'; // Handle special case 538 | return fn(buf, (int) (s + n), fnd); 539 | } 540 | 541 | int mjson_print_int(mjson_print_fn_t fn, void *fnd, int v, int s) { 542 | return mjson_print_long(fn, fnd, s ? (long) v : (long) (unsigned) v, s); 543 | } 544 | 545 | static int addexp(char *buf, int e, int sign) { 546 | int n = 0; 547 | buf[n++] = 'e'; 548 | buf[n++] = (char) sign; 549 | if (e > 400) return 0; 550 | if (e < 10) buf[n++] = '0'; 551 | if (e >= 100) buf[n++] = (char) (e / 100 + '0'), e -= 100 * (e / 100); 552 | if (e >= 10) buf[n++] = (char) (e / 10 + '0'), e -= 10 * (e / 10); 553 | buf[n++] = (char) (e + '0'); 554 | return n; 555 | } 556 | 557 | int mjson_print_dbl(mjson_print_fn_t fn, void *fnd, double d, int width) { 558 | char buf[40]; 559 | int i, s = 0, n = 0, e = 0; 560 | double t, mul, saved; 561 | if (d == 0.0) return fn("0", 1, fnd); 562 | if (isinf(d)) return fn(d > 0 ? "inf" : "-inf", d > 0 ? 3 : 4, fnd); 563 | if (isnan(d)) return fn("nan", 3, fnd); 564 | if (d < 0.0) d = -d, buf[s++] = '-'; 565 | 566 | // Round 567 | saved = d; 568 | mul = 1.0; 569 | while (d >= 10.0 && d / mul >= 10.0) mul *= 10.0; 570 | while (d <= 1.0 && d / mul <= 1.0) mul /= 10.0; 571 | for (i = 0, t = mul * 5; i < width; i++) t /= 10.0; 572 | d += t; 573 | // Calculate exponent, and 'mul' for scientific representation 574 | mul = 1.0; 575 | while (d >= 10.0 && d / mul >= 10.0) mul *= 10.0, e++; 576 | while (d < 1.0 && d / mul < 1.0) mul /= 10.0, e--; 577 | // printf(" --> %g %d %g %g\n", saved, e, t, mul); 578 | 579 | if (e >= width) { 580 | struct mjson_fixedbuf fb = {buf + s, (int) sizeof(buf) - s, 0}; 581 | n = mjson_print_dbl(mjson_print_fixed_buf, &fb, saved / mul, width); 582 | // printf(" --> %.*g %d [%.*s]\n", 10, d / t, e, fb.len, fb.ptr); 583 | n += addexp(buf + s + n, e, '+'); 584 | return fn(buf, s + n, fnd); 585 | } else if (e <= -width) { 586 | struct mjson_fixedbuf fb = {buf + s, (int) sizeof(buf) - s, 0}; 587 | n = mjson_print_dbl(mjson_print_fixed_buf, &fb, saved / mul, width); 588 | // printf(" --> %.*g %d [%.*s]\n", 10, d / mul, e, fb.len, fb.ptr); 589 | n += addexp(buf + s + n, -e, '-'); 590 | return fn(buf, s + n, fnd); 591 | } else { 592 | for (i = 0, t = mul; d >= 1.0 && s + n < (int) sizeof(buf); i++) { 593 | int ch = (int) (d / t); 594 | if (n > 0 || ch > 0) buf[s + n++] = (char) (ch + '0'); 595 | d -= ch * t; 596 | t /= 10.0; 597 | } 598 | // printf(" --> [%g] -> %g %g (%d) [%.*s]\n", saved, d, t, n, s + n, buf); 599 | if (n == 0) buf[s++] = '0'; 600 | while (t >= 1.0 && n + s < (int) sizeof(buf)) buf[n++] = '0', t /= 10.0; 601 | if (s + n < (int) sizeof(buf)) buf[n + s++] = '.'; 602 | // printf(" 1--> [%g] -> [%.*s]\n", saved, s + n, buf); 603 | for (i = 0, t = 0.1; s + n < (int) sizeof(buf) && n < width; i++) { 604 | int ch = (int) (d / t); 605 | buf[s + n++] = (char) (ch + '0'); 606 | d -= ch * t; 607 | t /= 10.0; 608 | } 609 | } 610 | while (n > 0 && buf[s + n - 1] == '0') n--; // Trim trailing zeros 611 | if (n > 0 && buf[s + n - 1] == '.') n--; // Trim trailing dot 612 | return fn(buf, s + n, fnd); 613 | } 614 | 615 | int mjson_print_str(mjson_print_fn_t fn, void *fnd, const char *s, int len) { 616 | int i, n = fn("\"", 1, fnd); 617 | for (i = 0; i < len; i++) { 618 | char c = (char) (unsigned char) mjson_escape(s[i]); 619 | if (c) { 620 | n += fn("\\", 1, fnd); 621 | n += fn(&c, 1, fnd); 622 | } else { 623 | n += fn(&s[i], 1, fnd); 624 | } 625 | } 626 | return n + fn("\"", 1, fnd); 627 | } 628 | 629 | #if MJSON_ENABLE_BASE64 630 | int mjson_print_b64(mjson_print_fn_t fn, void *fnd, const unsigned char *s, 631 | int n) { 632 | const char *t = 633 | "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; 634 | int i, len = fn("\"", 1, fnd); 635 | for (i = 0; i < n; i += 3) { 636 | int a = s[i], b = i + 1 < n ? s[i + 1] : 0, c = i + 2 < n ? s[i + 2] : 0; 637 | char buf[4] = {t[a >> 2], t[(a & 3) << 4 | (b >> 4)], '=', '='}; 638 | if (i + 1 < n) buf[2] = t[(b & 15) << 2 | (c >> 6)]; 639 | if (i + 2 < n) buf[3] = t[c & 63]; 640 | len += fn(buf, sizeof(buf), fnd); 641 | } 642 | return len + fn("\"", 1, fnd); 643 | } 644 | #endif /* MJSON_ENABLE_BASE64 */ 645 | 646 | int mjson_vprintf(mjson_print_fn_t fn, void *fnd, const char *fmt, 647 | va_list *ap) { 648 | int i = 0, n = 0; 649 | while (fmt[i] != '\0') { 650 | if (fmt[i] == '%') { 651 | char fc = fmt[++i]; 652 | int is_long = 0; 653 | if (fc == 'l') { 654 | is_long = 1; 655 | fc = fmt[i + 1]; 656 | } 657 | if (fc == 'Q') { 658 | char *buf = va_arg(*ap, char *); 659 | n += mjson_print_str(fn, fnd, buf ? buf : "", 660 | buf ? (int) strlen(buf) : 0); 661 | } else if (strncmp(&fmt[i], ".*Q", 3) == 0) { 662 | int len = va_arg(*ap, int); 663 | char *buf = va_arg(*ap, char *); 664 | n += mjson_print_str(fn, fnd, buf, len); 665 | i += 2; 666 | } else if (fc == 'd' || fc == 'u') { 667 | int is_signed = (fc == 'd'); 668 | if (is_long) { 669 | long val = va_arg(*ap, long); 670 | n += mjson_print_long(fn, fnd, val, is_signed); 671 | i++; 672 | } else { 673 | int val = va_arg(*ap, int); 674 | n += mjson_print_int(fn, fnd, val, is_signed); 675 | } 676 | } else if (fc == 'B') { 677 | const char *s = va_arg(*ap, int) ? "true" : "false"; 678 | n += mjson_print_buf(fn, fnd, s, (int) strlen(s)); 679 | } else if (fc == 's') { 680 | char *buf = va_arg(*ap, char *); 681 | n += mjson_print_buf(fn, fnd, buf, (int) strlen(buf)); 682 | } else if (strncmp(&fmt[i], ".*s", 3) == 0) { 683 | int len = va_arg(*ap, int); 684 | char *buf = va_arg(*ap, char *); 685 | n += mjson_print_buf(fn, fnd, buf, len); 686 | i += 2; 687 | } else if (fc == 'g') { 688 | n += mjson_print_dbl(fn, fnd, va_arg(*ap, double), 6); 689 | } else if (strncmp(&fmt[i], ".*g", 3) == 0) { 690 | int width = va_arg(*ap, int); 691 | n += mjson_print_dbl(fn, fnd, va_arg(*ap, double), width); 692 | i += 2; 693 | #if MJSON_ENABLE_BASE64 694 | } else if (fc == 'V') { 695 | int len = va_arg(*ap, int); 696 | const char *buf = va_arg(*ap, const char *); 697 | n += mjson_print_b64(fn, fnd, (unsigned char *) buf, len); 698 | #endif 699 | } else if (fc == 'H') { 700 | const char *hex = "0123456789abcdef"; 701 | int j, len = va_arg(*ap, int); 702 | const unsigned char *p = va_arg(*ap, const unsigned char *); 703 | n += fn("\"", 1, fnd); 704 | for (j = 0; j < len; j++) { 705 | n += fn(&hex[(p[j] >> 4) & 15], 1, fnd); 706 | n += fn(&hex[p[j] & 15], 1, fnd); 707 | } 708 | n += fn("\"", 1, fnd); 709 | } else if (fc == 'M') { 710 | mjson_vprint_fn_t vfn = va_arg(*ap, mjson_vprint_fn_t); 711 | n += vfn(fn, fnd, ap); 712 | } 713 | i++; 714 | } else { 715 | n += mjson_print_buf(fn, fnd, &fmt[i++], 1); 716 | } 717 | } 718 | return n; 719 | } 720 | 721 | int mjson_printf(mjson_print_fn_t fn, void *fnd, const char *fmt, ...) { 722 | va_list ap; 723 | int len; 724 | va_start(ap, fmt); 725 | len = mjson_vprintf(fn, fnd, fmt, &ap); 726 | va_end(ap); 727 | return len; 728 | } 729 | #endif /* MJSON_ENABLE_PRINT */ 730 | 731 | static int is_digit(int c) { 732 | return c >= '0' && c <= '9'; 733 | } 734 | 735 | /* NOTE: strtod() implementation by Yasuhiro Matsumoto. */ 736 | static double mystrtod(const char *str, char **end) { 737 | double d = 0.0; 738 | int sign = 1, n = 0; 739 | const char *p = str, *a = str; 740 | 741 | /* decimal part */ 742 | if (*p == '-') { 743 | sign = -1; 744 | ++p; 745 | } else if (*p == '+') { 746 | ++p; 747 | } 748 | if (is_digit(*p)) { 749 | d = (double) (*p++ - '0'); 750 | while (*p && is_digit(*p)) { 751 | d = d * 10.0 + (double) (*p - '0'); 752 | ++p; 753 | ++n; 754 | } 755 | a = p; 756 | } else if (*p != '.') { 757 | goto done; 758 | } 759 | d *= sign; 760 | 761 | /* fraction part */ 762 | if (*p == '.') { 763 | double f = 0.0; 764 | double base = 0.1; 765 | ++p; 766 | 767 | if (is_digit(*p)) { 768 | while (*p && is_digit(*p)) { 769 | f += base * (*p - '0'); 770 | base /= 10.0; 771 | ++p; 772 | ++n; 773 | } 774 | } 775 | d += f * sign; 776 | a = p; 777 | } 778 | 779 | /* exponential part */ 780 | if ((*p == 'E') || (*p == 'e')) { 781 | int i, e = 0, neg = 0; 782 | p++; 783 | if (*p == '-') p++, neg++; 784 | if (*p == '+') p++; 785 | while (is_digit(*p)) e = e * 10 + *p++ - '0'; 786 | if (neg) e = -e; 787 | #if 0 788 | if (d == 2.2250738585072011 && e == -308) { 789 | d = 0.0; 790 | a = p; 791 | goto done; 792 | } 793 | if (d == 2.2250738585072012 && e <= -308) { 794 | d *= 1.0e-308; 795 | a = p; 796 | goto done; 797 | } 798 | #endif 799 | for (i = 0; i < e; i++) d *= 10; 800 | for (i = 0; i < -e; i++) d /= 10; 801 | a = p; 802 | } else if (p > str && !is_digit(*(p - 1))) { 803 | a = str; 804 | goto done; 805 | } 806 | 807 | done: 808 | if (end) *end = (char *) a; 809 | return d; 810 | } 811 | 812 | #if MJSON_ENABLE_MERGE 813 | int mjson_merge(const char *s, int n, const char *s2, int n2, 814 | mjson_print_fn_t fn, void *userdata) { 815 | int koff, klen, voff, vlen, t, t2, k, off = 0, len = 0, comma = 0; 816 | if (n < 2) return len; 817 | len += fn("{", 1, userdata); 818 | while ((off = mjson_next(s, n, off, &koff, &klen, &voff, &vlen, &t)) != 0) { 819 | char *path = (char *) alloca((size_t) klen + 1); 820 | const char *val; 821 | memcpy(path, "$.", 2); 822 | memcpy(path + 2, s + koff + 1, (size_t)(klen - 2)); 823 | path[klen] = '\0'; 824 | if ((t2 = mjson_find(s2, n2, path, &val, &k)) != MJSON_TOK_INVALID) { 825 | if (t2 == MJSON_TOK_NULL) continue; // null deletes the key 826 | } else { 827 | val = s + voff; // Key is not found in the update. Copy the old value. 828 | } 829 | if (comma) len += fn(",", 1, userdata); 830 | len += fn(s + koff, klen, userdata); 831 | len += fn(":", 1, userdata); 832 | if (t == MJSON_TOK_OBJECT && t2 == MJSON_TOK_OBJECT) { 833 | len += mjson_merge(s + voff, vlen, val, k, fn, userdata); 834 | } else { 835 | if (t2 != MJSON_TOK_INVALID) vlen = k; 836 | len += fn(val, vlen, userdata); 837 | } 838 | comma = 1; 839 | } 840 | // Add missing keys 841 | off = 0; 842 | while ((off = mjson_next(s2, n2, off, &koff, &klen, &voff, &vlen, &t)) != 0) { 843 | char *path = (char *) alloca((size_t) klen + 1); 844 | const char *val; 845 | if (t == MJSON_TOK_NULL) continue; 846 | memcpy(path, "$.", 2); 847 | memcpy(path + 2, s2 + koff + 1, (size_t)(klen - 2)); 848 | path[klen] = '\0'; 849 | if (mjson_find(s, n, path, &val, &vlen) != MJSON_TOK_INVALID) continue; 850 | if (comma) len += fn(",", 1, userdata); 851 | len += fn(s2 + koff, klen, userdata); 852 | len += fn(":", 1, userdata); 853 | len += fn(s2 + voff, vlen, userdata); 854 | comma = 1; 855 | } 856 | len += fn("}", 1, userdata); 857 | return len; 858 | } 859 | #endif // MJSON_ENABLE_MERGE 860 | 861 | #if MJSON_ENABLE_PRETTY 862 | struct prettydata { 863 | int level; 864 | int len; 865 | int prev; 866 | const char *pad; 867 | int padlen; 868 | mjson_print_fn_t fn; 869 | void *userdata; 870 | }; 871 | 872 | static int pretty_cb(int ev, const char *s, int off, int len, void *ud) { 873 | struct prettydata *d = (struct prettydata *) ud; 874 | int i; 875 | switch (ev) { 876 | case '{': 877 | case '[': 878 | d->level++; 879 | d->len += d->fn(s + off, len, d->userdata); 880 | break; 881 | case '}': 882 | case ']': 883 | d->level--; 884 | if (d->prev != '[' && d->prev != '{' && d->padlen > 0) { 885 | d->len += d->fn("\n", 1, d->userdata); 886 | for (i = 0; i < d->level; i++) 887 | d->len += d->fn(d->pad, d->padlen, d->userdata); 888 | } 889 | d->len += d->fn(s + off, len, d->userdata); 890 | break; 891 | case ',': 892 | d->len += d->fn(s + off, len, d->userdata); 893 | if (d->padlen > 0) { 894 | d->len += d->fn("\n", 1, d->userdata); 895 | for (i = 0; i < d->level; i++) 896 | d->len += d->fn(d->pad, d->padlen, d->userdata); 897 | } 898 | break; 899 | case ':': 900 | d->len += d->fn(s + off, len, d->userdata); 901 | if (d->padlen > 0) d->len += d->fn(" ", 1, d->userdata); 902 | break; 903 | case MJSON_TOK_KEY: 904 | if (d->prev == '{' && d->padlen > 0) { 905 | d->len += d->fn("\n", 1, d->userdata); 906 | for (i = 0; i < d->level; i++) 907 | d->len += d->fn(d->pad, d->padlen, d->userdata); 908 | } 909 | d->len += d->fn(s + off, len, d->userdata); 910 | break; 911 | default: 912 | if (d->prev == '[' && d->padlen > 0) { 913 | d->len += d->fn("\n", 1, d->userdata); 914 | for (i = 0; i < d->level; i++) 915 | d->len += d->fn(d->pad, d->padlen, d->userdata); 916 | } 917 | d->len += d->fn(s + off, len, d->userdata); 918 | break; 919 | } 920 | d->prev = ev; 921 | return 0; 922 | } 923 | 924 | int mjson_pretty(const char *s, int n, const char *pad, mjson_print_fn_t fn, 925 | void *userdata) { 926 | struct prettydata d = {0, 0, 0, pad, (int) strlen(pad), fn, userdata}; 927 | if (mjson(s, n, pretty_cb, &d) < 0) return -1; 928 | return d.len; 929 | } 930 | #endif // MJSON_ENABLE_PRETTY 931 | 932 | #if MJSON_ENABLE_RPC 933 | struct jsonrpc_ctx jsonrpc_default_context; 934 | 935 | int mjson_globmatch(const char *s1, int n1, const char *s2, int n2) { 936 | int i = 0, j = 0, ni = 0, nj = 0; 937 | while (i < n1 || j < n2) { 938 | if (i < n1 && j < n2 && (s1[i] == '?' || s2[j] == s1[i])) { 939 | i++, j++; 940 | } else if (i < n1 && (s1[i] == '*' || s1[i] == '#')) { 941 | ni = i, nj = j + 1, i++; 942 | } else if (nj > 0 && nj <= n2 && (s1[i - 1] == '#' || s2[j] != '/')) { 943 | i = ni, j = nj; 944 | } else { 945 | return 0; 946 | } 947 | } 948 | return 1; 949 | } 950 | 951 | void jsonrpc_return_errorv(struct jsonrpc_request *r, int code, 952 | const char *message, const char *data_fmt, 953 | va_list *ap) { 954 | if (r->id_len == 0) return; 955 | mjson_printf(r->fn, r->fndata, 956 | "{\"id\":%.*s,\"error\":{\"code\":%d,\"message\":%Q", r->id_len, 957 | r->id, code, message == NULL ? "" : message); 958 | if (data_fmt != NULL) { 959 | mjson_printf(r->fn, r->fndata, ",\"data\":"); 960 | mjson_vprintf(r->fn, r->fndata, data_fmt, ap); 961 | } 962 | mjson_printf(r->fn, r->fndata, "}}\n"); 963 | } 964 | 965 | void jsonrpc_return_error(struct jsonrpc_request *r, int code, 966 | const char *message, const char *data_fmt, ...) { 967 | va_list ap; 968 | va_start(ap, data_fmt); 969 | jsonrpc_return_errorv(r, code, message, data_fmt, &ap); 970 | va_end(ap); 971 | } 972 | 973 | void jsonrpc_return_successv(struct jsonrpc_request *r, const char *result_fmt, 974 | va_list *ap) { 975 | if (r->id_len == 0) return; 976 | mjson_printf(r->fn, r->fndata, "{\"id\":%.*s,\"result\":", r->id_len, r->id); 977 | if (result_fmt != NULL) { 978 | mjson_vprintf(r->fn, r->fndata, result_fmt, ap); 979 | } else { 980 | mjson_printf(r->fn, r->fndata, "%s", "null"); 981 | } 982 | mjson_printf(r->fn, r->fndata, "}\n"); 983 | } 984 | 985 | void jsonrpc_return_success(struct jsonrpc_request *r, const char *result_fmt, 986 | ...) { 987 | va_list ap; 988 | va_start(ap, result_fmt); 989 | jsonrpc_return_successv(r, result_fmt, &ap); 990 | va_end(ap); 991 | } 992 | 993 | void jsonrpc_ctx_process(struct jsonrpc_ctx *ctx, const char *buf, int len, 994 | mjson_print_fn_t fn, void *fndata, void *ud) { 995 | const char *result = NULL, *error = NULL; 996 | int result_sz = 0, error_sz = 0; 997 | struct jsonrpc_method *m = NULL; 998 | struct jsonrpc_request r = {ctx, buf, len, 0, 0, 0, 0, 0, 0, fn, fndata, ud}; 999 | 1000 | // Is is a response frame? 1001 | mjson_find(buf, len, "$.result", &result, &result_sz); 1002 | if (result == NULL) mjson_find(buf, len, "$.error", &error, &error_sz); 1003 | if (result_sz > 0 || error_sz > 0) { 1004 | if (ctx->response_cb) ctx->response_cb(buf, len, ctx->response_cb_data); 1005 | return; 1006 | } 1007 | 1008 | // Method must exist and must be a string 1009 | if (mjson_find(buf, len, "$.method", &r.method, &r.method_len) != 1010 | MJSON_TOK_STRING) { 1011 | mjson_printf(fn, fndata, "{\"error\":{\"code\":-32700,\"message\":%.*Q}}\n", 1012 | len, buf); 1013 | return; 1014 | } 1015 | 1016 | // id and params are optional 1017 | mjson_find(buf, len, "$.id", &r.id, &r.id_len); 1018 | mjson_find(buf, len, "$.params", &r.params, &r.params_len); 1019 | 1020 | for (m = ctx->methods; m != NULL; m = m->next) { 1021 | if (mjson_globmatch(m->method, m->method_sz, r.method + 1, 1022 | r.method_len - 2) > 0) { 1023 | if (r.params == NULL) r.params = ""; 1024 | m->cb(&r); 1025 | break; 1026 | } 1027 | } 1028 | if (m == NULL) { 1029 | jsonrpc_return_error(&r, JSONRPC_ERROR_NOT_FOUND, "method not found", NULL); 1030 | } 1031 | } 1032 | 1033 | static int jsonrpc_print_methods(mjson_print_fn_t fn, void *fndata, 1034 | va_list *ap) { 1035 | struct jsonrpc_ctx *ctx = va_arg(*ap, struct jsonrpc_ctx *); 1036 | struct jsonrpc_method *m; 1037 | int len = 0; 1038 | for (m = ctx->methods; m != NULL; m = m->next) { 1039 | if (m != ctx->methods) len += mjson_print_buf(fn, fndata, ",", 1); 1040 | len += mjson_print_str(fn, fndata, m->method, (int) strlen(m->method)); 1041 | } 1042 | return len; 1043 | } 1044 | 1045 | static void rpclist(struct jsonrpc_request *r) { 1046 | jsonrpc_return_success(r, "[%M]", jsonrpc_print_methods, r->ctx); 1047 | } 1048 | 1049 | void jsonrpc_ctx_init(struct jsonrpc_ctx *ctx, mjson_print_fn_t response_cb, 1050 | void *response_cb_data) { 1051 | ctx->response_cb = response_cb; 1052 | ctx->response_cb_data = response_cb_data; 1053 | jsonrpc_ctx_export(ctx, MJSON_RPC_LIST_NAME, rpclist); 1054 | } 1055 | 1056 | void jsonrpc_init(mjson_print_fn_t response_cb, void *userdata) { 1057 | jsonrpc_ctx_init(&jsonrpc_default_context, response_cb, userdata); 1058 | } 1059 | #endif // MJSON_ENABLE_RPC 1060 | -------------------------------------------------------------------------------- /examples/uart-json-rpc/mjson.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018-2020 Cesanta Software Limited 2 | // All rights reserved 3 | // 4 | // Permission is hereby granted, free of charge, to any person obtaining a copy 5 | // of this software and associated documentation files (the "Software"), to deal 6 | // in the Software without restriction, including without limitation the rights 7 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | // copies of the Software, and to permit persons to whom the Software is 9 | // furnished to do so, subject to the following conditions: 10 | // 11 | // The above copyright notice and this permission notice shall be included in 12 | // all copies or substantial portions of the Software. 13 | // 14 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | // SOFTWARE. 21 | 22 | #ifndef MJSON_H 23 | #define MJSON_H 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #ifndef MJSON_ENABLE_PRINT 30 | #define MJSON_ENABLE_PRINT 1 31 | #endif 32 | 33 | #ifndef MJSON_ENABLE_RPC 34 | #define MJSON_ENABLE_RPC 1 35 | #endif 36 | 37 | #ifndef MJSON_ENABLE_BASE64 38 | #define MJSON_ENABLE_BASE64 1 39 | #endif 40 | 41 | #ifndef MJSON_ENABLE_MERGE 42 | #define MJSON_ENABLE_MERGE 0 43 | #elif MJSON_ENABLE_MERGE 44 | #define MJSON_ENABLE_NEXT 1 45 | #endif 46 | 47 | #ifndef MJSON_ENABLE_PRETTY 48 | #define MJSON_ENABLE_PRETTY 0 49 | #elif MJSON_ENABLE_PRETTY 50 | #define MJSON_ENABLE_NEXT 1 51 | #endif 52 | 53 | #ifndef MJSON_ENABLE_NEXT 54 | #define MJSON_ENABLE_NEXT 0 55 | #endif 56 | 57 | #ifndef MJSON_RPC_LIST_NAME 58 | #define MJSON_RPC_LIST_NAME "rpc.list" 59 | #endif 60 | 61 | #ifndef MJSON_DYNBUF_CHUNK 62 | #define MJSON_DYNBUF_CHUNK 256 // Allocation granularity for print_dynamic_buf 63 | #endif 64 | 65 | #ifdef __cplusplus 66 | extern "C" { 67 | #endif 68 | 69 | #define MJSON_ERROR_INVALID_INPUT (-1) 70 | #define MJSON_ERROR_TOO_DEEP (-2) 71 | #define MJSON_TOK_INVALID 0 72 | #define MJSON_TOK_KEY 1 73 | #define MJSON_TOK_STRING 11 74 | #define MJSON_TOK_NUMBER 12 75 | #define MJSON_TOK_TRUE 13 76 | #define MJSON_TOK_FALSE 14 77 | #define MJSON_TOK_NULL 15 78 | #define MJSON_TOK_ARRAY 91 79 | #define MJSON_TOK_OBJECT 123 80 | #define MJSON_TOK_IS_VALUE(t) ((t) > 10 && (t) < 20) 81 | 82 | typedef int (*mjson_cb_t)(int ev, const char *s, int off, int len, void *ud); 83 | 84 | #ifndef MJSON_MAX_DEPTH 85 | #define MJSON_MAX_DEPTH 20 86 | #endif 87 | 88 | int mjson(const char *s, int len, mjson_cb_t cb, void *ud); 89 | int mjson_find(const char *s, int len, const char *jp, const char **, int *); 90 | int mjson_get_number(const char *s, int len, const char *path, double *v); 91 | int mjson_get_bool(const char *s, int len, const char *path, int *v); 92 | int mjson_get_string(const char *s, int len, const char *path, char *to, int n); 93 | int mjson_get_hex(const char *s, int len, const char *path, char *to, int n); 94 | 95 | #if MJSON_ENABLE_NEXT 96 | int mjson_next(const char *s, int n, int off, int *koff, int *klen, int *voff, 97 | int *vlen, int *vtype); 98 | #endif 99 | 100 | #if MJSON_ENABLE_BASE64 101 | int mjson_get_base64(const char *s, int len, const char *path, char *to, int n); 102 | int mjson_base64_dec(const char *src, int n, char *dst, int dlen); 103 | #endif 104 | 105 | #if MJSON_ENABLE_PRINT 106 | typedef int (*mjson_print_fn_t)(const char *buf, int len, void *userdata); 107 | typedef int (*mjson_vprint_fn_t)(mjson_print_fn_t, void *, va_list *); 108 | 109 | struct mjson_fixedbuf { 110 | char *ptr; 111 | int size, len; 112 | }; 113 | 114 | int mjson_printf(mjson_print_fn_t, void *, const char *fmt, ...); 115 | int mjson_vprintf(mjson_print_fn_t, void *, const char *fmt, va_list *ap); 116 | int mjson_print_str(mjson_print_fn_t, void *, const char *s, int len); 117 | int mjson_print_int(mjson_print_fn_t, void *, int value, int is_signed); 118 | int mjson_print_long(mjson_print_fn_t, void *, long value, int is_signed); 119 | int mjson_print_buf(mjson_print_fn_t fn, void *, const char *buf, int len); 120 | int mjson_print_dbl(mjson_print_fn_t fn, void *, double, int width); 121 | 122 | int mjson_print_null(const char *ptr, int len, void *userdata); 123 | int mjson_print_fixed_buf(const char *ptr, int len, void *userdata); 124 | int mjson_print_dynamic_buf(const char *ptr, int len, void *userdata); 125 | 126 | int mjson_snprintf(char *buf, size_t len, const char *fmt, ...); 127 | char *mjson_aprintf(const char *fmt, ...); 128 | 129 | #if MJSON_ENABLE_PRETTY 130 | int mjson_pretty(const char *, int, const char *, mjson_print_fn_t, void *); 131 | #endif 132 | 133 | #if MJSON_ENABLE_MERGE 134 | int mjson_merge(const char *, int, const char *, int, mjson_print_fn_t, void *); 135 | #endif 136 | 137 | #endif // MJSON_ENABLE_PRINT 138 | 139 | #if MJSON_ENABLE_RPC 140 | 141 | void jsonrpc_init(mjson_print_fn_t, void *userdata); 142 | int mjson_globmatch(const char *s1, int n1, const char *s2, int n2); 143 | 144 | struct jsonrpc_request { 145 | struct jsonrpc_ctx *ctx; 146 | const char *frame; // Points to the whole frame 147 | int frame_len; // Frame length 148 | const char *params; // Points to the "params" in the request frame 149 | int params_len; // Length of the "params" 150 | const char *id; // Points to the "id" in the request frame 151 | int id_len; // Length of the "id" 152 | const char *method; // Points to the "method" in the request frame 153 | int method_len; // Length of the "method" 154 | mjson_print_fn_t fn; // Printer function 155 | void *fndata; // Printer function data 156 | void *userdata; // Callback's user data as specified at export time 157 | }; 158 | 159 | struct jsonrpc_method { 160 | const char *method; 161 | int method_sz; 162 | void (*cb)(struct jsonrpc_request *); 163 | struct jsonrpc_method *next; 164 | }; 165 | 166 | // Main RPC context, stores current request information and a list of 167 | // exported RPC methods. 168 | struct jsonrpc_ctx { 169 | struct jsonrpc_method *methods; 170 | mjson_print_fn_t response_cb; 171 | void *response_cb_data; 172 | }; 173 | 174 | // Registers function fn under the given name within the given RPC context 175 | #define jsonrpc_ctx_export(ctx, name, fn) \ 176 | do { \ 177 | static struct jsonrpc_method m = {(name), sizeof(name) - 1, (fn), 0}; \ 178 | m.next = (ctx)->methods; \ 179 | (ctx)->methods = &m; \ 180 | } while (0) 181 | 182 | void jsonrpc_ctx_init(struct jsonrpc_ctx *ctx, mjson_print_fn_t, void *); 183 | void jsonrpc_return_error(struct jsonrpc_request *r, int code, 184 | const char *message, const char *data_fmt, ...); 185 | void jsonrpc_return_success(struct jsonrpc_request *r, const char *result_fmt, 186 | ...); 187 | void jsonrpc_ctx_process(struct jsonrpc_ctx *ctx, const char *req, int req_sz, 188 | mjson_print_fn_t fn, void *fndata, void *userdata); 189 | 190 | extern struct jsonrpc_ctx jsonrpc_default_context; 191 | 192 | #define jsonrpc_export(name, fn) \ 193 | jsonrpc_ctx_export(&jsonrpc_default_context, (name), (fn)) 194 | 195 | #define jsonrpc_process(buf, len, fn, fnd, ud) \ 196 | jsonrpc_ctx_process(&jsonrpc_default_context, (buf), (len), (fn), (fnd), (ud)) 197 | 198 | #define JSONRPC_ERROR_INVALID -32700 /* Invalid JSON was received */ 199 | #define JSONRPC_ERROR_NOT_FOUND -32601 /* The method does not exist */ 200 | #define JSONRPC_ERROR_BAD_PARAMS -32602 /* Invalid params passed */ 201 | #define JSONRPC_ERROR_INTERNAL -32603 /* Internal JSON-RPC error */ 202 | 203 | #endif // MJSON_ENABLE_RPC 204 | #ifdef __cplusplus 205 | } 206 | #endif 207 | #endif // MJSON_H 208 | -------------------------------------------------------------------------------- /rtos.c: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "rtos.h" 10 | 11 | struct task { 12 | unsigned long *sp; // Stack pointer - must be the first attribute 13 | void (*fn)(void *); // Task function 14 | void *arg; // Task function argument 15 | int id; // Task ID 16 | struct task *next; // Next in chain 17 | }; 18 | 19 | static struct task *s_tasks; // Tasks list 20 | static struct task *s_current_task; // Current task 21 | static uint32_t s_systick; // System tick counter 22 | static int s_start_scheduling; // Marker to start scheduling 23 | static char *s_heap_start; // Heap start. Set up by rtos_heap_init() 24 | static char *s_heap_end; // Heap end 25 | static char *s_brk; // Current heap usage marker 26 | static unsigned long s_freq; // MCU frequency in HZ 27 | 28 | static void rtos_task_delete(struct task *t) { 29 | DEBUG(("task %d deleting\n", t->id)); 30 | struct task **p = &s_tasks; 31 | while (*p != t) p = &(*p)->next; 32 | *p = s_current_task = t->next; 33 | free(t); 34 | for (;;) asm("nop"); // Loop until someone preempts us 35 | } 36 | 37 | // We run user function via a wrapper which deletes the task if the 38 | // user-provided function returns 39 | static inline void rtos_task_start(struct task *t) { 40 | DEBUG(("task %d starting\n", t->id)); 41 | s_start_scheduling = 1; 42 | t->fn(t->arg); 43 | rtos_task_delete(t); 44 | } 45 | 46 | int rtos_task_create(void (*fn)(void *), void *arg, size_t ssize) { 47 | static int id; 48 | struct task *task = malloc(sizeof(*task) + ssize); 49 | if (task == NULL) return 0; 50 | task->id = id++; 51 | task->fn = fn; 52 | task->arg = arg; 53 | task->next = s_tasks; 54 | s_tasks = s_current_task = task; 55 | 56 | // Hand-craft task's cpu context, as if it was interrupted 57 | task->sp = (unsigned long *) (task + 1) + ssize; 58 | if (((uint32_t) task->sp) & 7) task->sp--; // 8-byte align for soft float 59 | task->sp[8] = (uintptr_t) task; // argv[0] 60 | task->sp[14] = (uintptr_t) rtos_task_start; // return address 61 | task->sp[15] = 0x01000000; // xPSR 62 | 63 | DEBUG(("created task %d, sp: %p\n", task->id, task->sp)); 64 | return task->id; 65 | } 66 | 67 | void rtos_task_switch(void) { 68 | struct task *t = s_current_task, *next = t && t->next ? t->next : s_tasks; 69 | // DEBUG(("switch %d -> %d\n", t ? t->id : -1, next ? next->id : -1)); 70 | s_current_task = next; 71 | } 72 | 73 | void rtos_schedule(void) { 74 | if (s_current_task == NULL) return; // No tasks to run 75 | 76 | // Set psp = first task's stack pointer 77 | void *sp = s_current_task->sp; 78 | asm volatile("mov r0, %[v]; msr psp, r0; isb;" : : [v] "r"(sp) : "memory"); 79 | 80 | // Swith to the unprivileged mode 81 | // Set two low bits in the CONTROL register: 82 | // bit 0 - enable unprivileged mode 83 | // bit 1 - enable process stack (PSP) for threaded mode 84 | #if 0 85 | asm volatile("mrs r0, control; orr r0, #3; msr control, r0; isb;"); 86 | #else 87 | asm volatile( 88 | "mrs r1, control;" 89 | "movs r3, #3;" 90 | "orr r0, r3;" 91 | "msr control, r0;" 92 | "isb;"); 93 | #endif 94 | 95 | // Start s_current_task 96 | asm volatile("ldr r0, =s_current_task; ldr r0, [r0]; bl rtos_task_start;"); 97 | } 98 | 99 | __attribute__((naked)) void PendSV_Handler(void) { 100 | asm volatile( 101 | // If no task is running, don't save context before switching 102 | // if (!s_current_task) goto switch 103 | "ldr r3, =s_current_task;" // r3 = &s_current_task 104 | "ldr r3, [r3];" // r3 = *r3 105 | "cmp r3, #0;" // if (r3 == 0) 106 | "beq .switch;" // goto switch; (we don't use cbz for M0) 107 | 108 | // Save context of the s_current_task 109 | "mrs r0, psp;" // r0 = psp. Next is: 110 | #if 0 111 | "stmdb r0!, {r4-r11};" // Save r4-r11 into PSP. Does not work on M0 112 | "str r0, [r3];" // s_current_task->sp = r0 113 | #else 114 | "sub r0, #32;" // Space for another 8 registers 115 | "str r0, [r3];" // s_current_task->sp = r0 116 | "stmia r0!, {r4-r7};" // Store r4-r7 into the process's stack 117 | "mov r4, r8;" 118 | "mov r5, r9;" 119 | "mov r6, r10;" 120 | "mov r7, r11;" 121 | "stmia r0!, {r4-r7};" // Store r8-r11 into the process's stack 122 | #endif 123 | 124 | // Switch s_current_task to the next task 125 | ".switch:;" // Switch marker 126 | "push {lr};" // Save lr 127 | "bl rtos_task_switch;" // Update s_current_task 128 | "pop {r2};" // Restore lr into r2 129 | 130 | // Now s_current_task is different. Load its context 131 | "ldr r3, =s_current_task;" // r3 = &s_current_task 132 | "ldr r3, [r3];" // r3 = *r3 133 | "cmp r3, #0;" // if (r3 == 0) 134 | "beq .done;" // goto done; 135 | 136 | // s_current_task changed. Load its context 137 | "ldr r0, [r3];" // r0 = psp 138 | #if 0 139 | "ldmia r0!, {r4-r11};" // Load r4-r11 140 | #else 141 | "add r0, #16;" 142 | "ldmia r0!, {r4-r7};" // Load r8-r11 143 | "mov r8, r4;" 144 | "mov r9, r5;" 145 | "mov r10, r6;" 146 | "mov r11, r7;" 147 | "sub r0, #32;" 148 | "ldmia r0!, {r4-r7};" // Load r4-r7 149 | "add r0, #16;" 150 | #endif 151 | "msr psp, r0; isb;" // Update PSP 152 | 153 | ".done:;" // Exit marker 154 | "bx r2" // Return to the switched process 155 | ); 156 | } 157 | 158 | void SysTick_Handler(void) { 159 | s_systick++; 160 | if (s_start_scheduling && s_current_task) { 161 | // Trigger PendSV_Handler 162 | // SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk; 163 | ((uint32_t volatile *) 0xe000ed04)[0] |= 1UL << 28; 164 | } 165 | } 166 | 167 | unsigned long rtos_msleep(unsigned long milliseconds) { 168 | uint32_t until = s_systick + milliseconds; 169 | while (s_systick < until) asm("nop"); 170 | return s_systick; 171 | } 172 | 173 | void _exit(int code) { 174 | (void) code; 175 | for (;;) asm("nop"); 176 | } 177 | 178 | // Initialise newlib malloc. It expects us to implement _sbrk() call, 179 | // which should return a pointer to the requested memory. 180 | // Our `rtos_heap_init` function sets up an available memory region. 181 | // 182 | // s_heap_start s_brk s_heap_end 183 | // |--------------------------|----------------------| 184 | // | (used memory) | (free memory) | 185 | 186 | int rtos_ram_used(void) { 187 | return s_brk - s_heap_start; 188 | } 189 | 190 | int rtos_ram_free(void) { 191 | return s_heap_end - s_brk; 192 | } 193 | 194 | void rtos_heap_init(void *start, void *end) { 195 | s_heap_start = s_brk = start, s_heap_end = end; 196 | } 197 | 198 | void rtos_freq_set(unsigned long freq) { 199 | s_freq = freq; 200 | } 201 | 202 | unsigned long rtos_freq_get(void) { 203 | return s_freq; 204 | } 205 | 206 | __attribute__((weak)) void *_sbrk(ptrdiff_t diff) { 207 | char *old = s_brk; 208 | if (&s_brk[diff] > s_heap_end) { 209 | errno = ENOMEM; 210 | return NULL; 211 | } 212 | s_brk += diff; 213 | return old; 214 | } 215 | -------------------------------------------------------------------------------- /rtos.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Sergey Lyubka 2 | // All rights reserved 3 | 4 | #pragma once 5 | 6 | int rtos_task_create(void (*)(void *), void *, size_t); // Create RTOS task 7 | void rtos_schedule(void); // Run scheduler, never return 8 | int rtos_ram_used(void); // Return used ram size 9 | int rtos_ram_free(void); // Return available ram size 10 | unsigned long rtos_msleep(unsigned long ms); // Sleep millis, return systick 11 | unsigned long rtos_freq_get(void); // Get processor frequency in HZ 12 | void rtos_freq_set(unsigned long freq); // Set processor frequency in HZ 13 | --------------------------------------------------------------------------------