From af2ad82e9267d77742195a8c8f1be33bd368e6b4 Mon Sep 17 00:00:00 2001 From: Anthony Rabine Date: Fri, 21 Jul 2023 14:35:04 +0200 Subject: [PATCH] stable QoRTOS (for now), before cleaning --- .../.cortex-debug.peripherals.state.json | 2 +- .../platform/raspberry-pico-w/CMakeLists.txt | 2 +- .../raspberry-pico-w/pico_hal_wrapper.c | 4 +- software/system/main.c | 75 +++----- software/system/qor.c | 176 +++++++++++------- software/system/qor.h | 13 +- software/system/qor_armv6m.s | 8 +- 7 files changed, 149 insertions(+), 131 deletions(-) diff --git a/software/.vscode/.cortex-debug.peripherals.state.json b/software/.vscode/.cortex-debug.peripherals.state.json index 58e8efe..7aed850 100644 --- a/software/.vscode/.cortex-debug.peripherals.state.json +++ b/software/.vscode/.cortex-debug.peripherals.state.json @@ -1 +1 @@ -[{"node":"TIMER","expanded":true,"format":0,"pinned":false}] \ No newline at end of file +[{"node":"TIMER","expanded":true,"format":0,"pinned":false},{"node":"TIMER.INTR","expanded":true,"format":0},{"node":"TIMER.INTE","expanded":true,"format":0}] \ No newline at end of file diff --git a/software/platform/raspberry-pico-w/CMakeLists.txt b/software/platform/raspberry-pico-w/CMakeLists.txt index 438a3f3..d9af471 100644 --- a/software/platform/raspberry-pico-w/CMakeLists.txt +++ b/software/platform/raspberry-pico-w/CMakeLists.txt @@ -10,7 +10,7 @@ project(${PROJECT_NAME} LANGUAGES CXX C ASM) set(CMAKE_INCLUDE_CURRENT_DIR ON) -set(CMAKE_VERBOSE_MAKEFILE ON) +set(CMAKE_VERBOSE_MAKEFILE OFF) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/software/platform/raspberry-pico-w/pico_hal_wrapper.c b/software/platform/raspberry-pico-w/pico_hal_wrapper.c index 2439840..10a1513 100644 --- a/software/platform/raspberry-pico-w/pico_hal_wrapper.c +++ b/software/platform/raspberry-pico-w/pico_hal_wrapper.c @@ -48,7 +48,7 @@ static __attribute__((aligned(8))) pio_i2s i2s; void ost_system_delay_ms(uint32_t delay) { - sleep_ms(delay); + busy_wait_ms(delay); } const uint8_t LED_PIN = 14; // GP 14 @@ -87,8 +87,6 @@ void gpio_callback(uint gpio, uint32_t events) one_time = false; // debouncer debug_printf("G\n"); - - qor_svc_call(); } } diff --git a/software/system/main.c b/software/system/main.c index 9a26ef5..3663c4c 100644 --- a/software/system/main.c +++ b/software/system/main.c @@ -36,6 +36,17 @@ int main(void) } #else +// Raspberry Pico SDK +#include "pico/stdlib.h" +#include "hardware/uart.h" +#include "hardware/spi.h" +#include "hardware/dma.h" +#include "hardware/irq.h" +#include "hardware/pio.h" +#include "hardware/clocks.h" +#include "pico.h" +#include "pico/stdlib.h" + #include "sdcard.h" const uint16_t tones[3][8] = @@ -135,29 +146,23 @@ void UserTask_2(void *args) } } -// Raspberry Pico SDK -#include "pico/stdlib.h" -#include "hardware/uart.h" -#include "hardware/spi.h" -#include "hardware/dma.h" -#include "hardware/irq.h" -#include "hardware/pio.h" -#include "hardware/clocks.h" -#include "pico.h" - void UserTask_3(void *args) { - gpio_init(1); - gpio_set_dir(1, GPIO_OUT); + int cpt = 0; while (1) { - // gpio_put(1, 0); + ost_hal_gpio_set(OST_GPIO_DEBUG_LED, 0); - qor_sleep(1000); - // gpio_put(1, 1); + qor_sleep(500); ost_hal_gpio_set(OST_GPIO_DEBUG_LED, 1); - qor_sleep(1000); + qor_sleep(500); + + // if (++cpt >= 10) + // { + // cpt = 0; + // debug_printf("SU: %d, SO: %d\r\n", tcb3.stack_usage, tcb3.so); + // } } } @@ -166,40 +171,16 @@ void IdleTaskFunction(void *args) while (1) { // Instrumentation, power saving, os functions won't work here - // __asm volatile("wfi"); + // __asm volatile("wfi"); } } -/* -void UserTask_2(void) -{ - InstrumentTriggerPE13_Init(); - uint32_t count = 0; - while (1) - { - InstrumentTriggerPE13_Toggle(); - count++; - if (count % 35 == 0) - OS_Thread_Sleep(4500); - else - HAL_Delay(70); - } -} - -void UserTask_3(void) -{ - InstrumentTriggerPE14_Init(); - while (1) - { - InstrumentTriggerPE14_Toggle(); - HAL_Delay(60); - } -} -*/ -#include "pico/stdlib.h" - int main() { + // timer_hw->inte = 0; + // timer_hw->alarm[3] = 0; + // timer_hw->dbgpause = 1; + ost_system_initialize(); // 1. Test the printf output @@ -213,8 +194,8 @@ int main() qor_init(THREADFREQ); - // qor_create_thread(&tcb1, UserTask_1, 2, "UserTask_0"); - // qor_create_thread(&tcb2, UserTask_2, 1, "UserTask_1"); + // qor_create_thread(&tcb1, UserTask_1, 2, "UserTask_1"); + // qor_create_thread(&tcb2, UserTask_2, 1, "UserTask_2"); qor_create_thread(&tcb3, UserTask_3, 3, "UserTask_3"); qor_start(&idle, IdleTaskFunction); diff --git a/software/system/qor.c b/software/system/qor.c index 4eda436..0c65623 100644 --- a/software/system/qor.c +++ b/software/system/qor.c @@ -14,6 +14,7 @@ #include "debug.h" #include "qor.h" #include +#include // Raspberry Pico SDK #include "pico/stdlib.h" @@ -29,66 +30,60 @@ #include "RP2040.h" void qor_switch_context(); -static void timer_stop(); +void qor_go(); +// void qor_svc_call(void); + +#define portNVIC_INT_CTRL_REG (*((volatile uint32_t *)0xe000ed04)) +#define portNVIC_PENDSVSET_BIT (1UL << 28UL) + +#define qor_svc_call() \ + do \ + { \ + portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; \ + } while (0) // =========================================================================================================== // ARM GENERIC // =========================================================================================================== -inline static void enable_irq() -{ - __asm volatile("cpsie i"); -} -inline static void disable_irq() -{ - __asm volatile("cpsid i"); -} +static uint32_t gCritialNesting = 0; -void qor_sleep_ms(uint8_t svc, uint32_t ms) -{ - - __wfi; -} +#define enable_irq() __asm volatile("cpsie i") +#define disable_irq() __asm volatile("cpsid i") +/* static inline uint32_t qor_enter_critical(void) { uint32_t primask = __get_PRIMASK(); disable_irq(); + gCritialNesting++; + __asm volatile("dsb" :: + : "memory"); + __asm volatile("isb"); return primask; } void qor_exit_critical(uint32_t status) { + gCritialNesting--; + + if (gCritialNesting == 0) + { + enable_irq(); + } __set_PRIMASK(status); } - -__attribute__((naked)) void PendSV_Handler() -{ - qor_switch_context(); -} +*/ static const bool qor_inside_interrupt(void) { uint32_t ulCurrentInterrupt; - bool xReturn; - // Obtain the number of the currently executing interrupt __asm volatile("mrs %0, ipsr" : "=r"(ulCurrentInterrupt)::"memory"); return ulCurrentInterrupt == 0 ? false : true; } -void qor_svc_call(void) -{ - timer_stop(); - volatile uint32_t *icsr = (void *)0xE000ED04; - // Pend a PendSV exception using by writing 1 to PENDSVSET at bit 28 - *icsr = 0x1 << 28; - // flush pipeline to ensure exception takes effect before we - // return from this routine - __asm("isb"); -} - // =========================================================================================================== // RASPBERRY PICO // =========================================================================================================== @@ -101,40 +96,52 @@ static volatile uint32_t timer_period; #include "hardware/structs/systick.h" //----------------------------------------------------------------------------- +// Beware, do not consume any local variable here, and keep the "naked" attribute +// Since it is an Exception (interrupt), we work on the MSB +// The syscall wil then call the scheduler so we will never free any local variables consumed +// It will result on a MSP infinite growing (0x42000 on the Pico) until user data overriding and at the end an hardfault will occur +// static void timer_irq(void) +// { +// // Clear the alarm irq +// hw_clear_bits(&timer_hw->intr, 1u << ALARM_NUM); +// qor_svc_call(); +// } -static void timer_irq(void) -{ - // Clear the alarm irq - hw_clear_bits(&timer_hw->intr, 1u << ALARM_NUM); - qor_svc_call(); -} +static uint64_t gNextAlarm = 0; -static void timer_set_alam(uint32_t delay_ms) +static void timer_set_alarm(uint32_t delay_ms) { if (delay_ms > 0) { // Alarm is only 32 bits so if trying to delay more // than that need to be careful and keep track of the upper // bits - uint64_t target = timer_hw->timerawl + delay_ms * 1000; - timer_hw->alarm[ALARM_NUM] = (uint32_t)target; + gNextAlarm = timer_hw->timerawl + delay_ms * 1000; + timer_hw->alarm[ALARM_NUM] = (uint32_t)gNextAlarm; // Enable the interrupt for our alarm (the timer outputs 4 alarm irqs) hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM); } } -static void timer_stop() +// __attribute__((naked)) +static void timer_end() { + timer_hw->armed = 0xF; + // Clear the alarm irq + hw_clear_bits(&timer_hw->intr, 1u << ALARM_NUM); + // Disable the intterupt for this alarm hw_clear_bits(&timer_hw->inte, 1u << ALARM_NUM); + + qor_svc_call(); } //----------------------------------------------------------------------------- static void timer_init() { // Set irq handler for alarm irq - irq_set_exclusive_handler(ALARM_IRQ, timer_irq); - // Enable the interrupt for our alarm (the timer outputs 4 alarm irqs) - hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM); + irq_set_exclusive_handler(ALARM_IRQ, timer_end); + // Disable the intterupt for this alarm + hw_clear_bits(&timer_hw->inte, 1u << ALARM_NUM); // Enable the alarm irq irq_set_enabled(ALARM_IRQ, true); } @@ -157,19 +164,34 @@ static uint32_t ActiveTCBsCount = 0; // =========================================================================================================== // Quite Ok RTOS private and public functions // =========================================================================================================== -void qor_init(uint32_t scheduler_frequency_hz) +// void qor_svc_call(void) +// { +// // hw_clear_bits(&timer_hw->inte, 1u << ALARM_NUM); +// // volatile uint32_t *icsr = (void *)0xE000ED04; +// // // Pend a PendSV exception using by writing 1 to PENDSVSET at bit 28 +// // *icsr = 0x1 << 28; +// // // flush pipeline to ensure exception takes effect before we +// // // return from this routine +// // +// // __asm volatile("svc 0"); +// } + +void __attribute__((naked)) swc() { - exception_set_exclusive_handler(PENDSV_EXCEPTION, PendSV_Handler); + qor_switch_context(); } -void qor_exit_loop() +void qor_init(uint32_t scheduler_frequency_hz) +{ + exception_set_exclusive_handler(PENDSV_EXCEPTION, qor_switch_context); +} + +static void qor_exit_loop() { for (;;) ; } -extern void qor_go(); - uint32_t *qor_initialize_stack(uint32_t *top_of_stack, thread_func_t task, void *args) { // ARM Calling convention: the folowwing registers are automatically saved onto the stack by the processor (in this ordoer on the stack) @@ -193,6 +215,12 @@ void qor_create_thread(qor_tcb_t *tcb, thread_func_t task, uint8_t priority, con assert_or_panic(ActiveTCBsCount >= 0 && ActiveTCBsCount < MAXNUMTHREADS); disable_irq(); + memset(&Stacks[ActiveTCBsCount][0], 0xAAAAAAAA, sizeof(Stacks[ActiveTCBsCount][0]) * STACKSIZE); + + tcb->stack_bottom = &Stacks[ActiveTCBsCount][0]; + tcb->stack_usage = 0; + tcb->so = false; + tcb->state = qor_tcb_state_active; tcb->wait_time = 0; tcb->state = qor_tcb_state_active; @@ -223,7 +251,7 @@ void qor_create_thread(qor_tcb_t *tcb, thread_func_t task, uint8_t priority, con enable_irq(); } -bool qor_start(qor_tcb_t *idle_tcb, thread_func_t idle_task) +bool __attribute__((naked)) qor_start(qor_tcb_t *idle_tcb, thread_func_t idle_task) { assert_or_panic(ActiveTCBsCount > 0); @@ -242,8 +270,7 @@ bool qor_start(qor_tcb_t *idle_tcb, thread_func_t idle_task) timer_init(); /* Prevent the timer's ISR from firing before start is called */ - disable_irq(); - + enable_irq(); qor_go(); /* This statement should not be reached */ @@ -252,6 +279,7 @@ bool qor_start(qor_tcb_t *idle_tcb, thread_func_t idle_task) return true; } +// void __not_in_flash_func(qor_scheduler)(void) void qor_scheduler(void) { /* @@ -321,17 +349,27 @@ void qor_scheduler(void) RunPt = IdleTcb; } - timer_set_alam(next_alarm); + // Statistics + if (RunPt->stack_bottom[0] != 0xAAAAAAAA) + { + RunPt->so = true; + } + RunPt->stack_usage = RunPt->sp - RunPt->stack_bottom; + + timer_set_alarm(next_alarm); } void qor_sleep(uint32_t sleep_duration_ms) { - uint32_t status = qor_enter_critical(); - RunPt->state = qor_tcb_state_sleep; - RunPt->ts = time_us_64() / 1000; - RunPt->wait_time = sleep_duration_ms; - qor_exit_critical(status); - qor_svc_call(); // call scheduler, recompute next timeout + if (sleep_duration_ms > 0) + { + disable_irq(); + RunPt->state = qor_tcb_state_sleep; + RunPt->ts = time_us_64() / 1000; + RunPt->wait_time = sleep_duration_ms; + enable_irq(); + qor_svc_call(); // call scheduler, recompute next timeout + } } // =========================================================================================================== @@ -349,7 +387,7 @@ void qor_mbox_init(qor_mbox_t *mbox, void **msgBuffer, uint32_t maxCount) uint32_t qor_mbox_wait(qor_mbox_t *mbox, void **msg, uint32_t wait_ms) { - uint32_t status = qor_enter_critical(); + disable_irq(); // No any data, block on that resource if (mbox->count == 0) @@ -362,30 +400,28 @@ uint32_t qor_mbox_wait(qor_mbox_t *mbox, void **msg, uint32_t wait_ms) } else { - qor_exit_critical(status); + enable_irq(); return QOR_MBOX_ERROR; } } - status = qor_enter_critical(); - --mbox->count; *msg = mbox->msgBuffer[mbox->read++]; if (mbox->read >= mbox->maxCount) { mbox->read = 0; } - qor_exit_critical(status); + enable_irq(); return QOR_MBOX_OK; } uint32_t qor_mbox_notify(qor_mbox_t *mbox, void *msg, uint32_t notifyOption) { - uint32_t status = qor_enter_critical(); + disable_irq(); if (mbox->count >= mbox->maxCount) { - qor_exit_critical(status); + enable_irq(); return QOR_MBOX_FULL; } if (notifyOption == QOR_MBOX_OPTION_SEND_FRONT) @@ -417,14 +453,14 @@ uint32_t qor_mbox_notify(qor_mbox_t *mbox, void *msg, uint32_t notifyOption) t->wait_time = 0; } - qor_exit_critical(status); + enable_irq(); qor_svc_call(); // call scheduler return QOR_MBOX_OK; } void qor_mbox_get_stats(qor_mbox_t *mbox, mbox_stats_t *info) { - uint32_t status = qor_enter_critical(); + disable_irq(); info->count = mbox->count; info->maxCount = mbox->maxCount; @@ -437,5 +473,5 @@ void qor_mbox_get_stats(qor_mbox_t *mbox, mbox_stats_t *info) head = head->wait_next; } - qor_exit_critical(status); + enable_irq(); } diff --git a/software/system/qor.h b/software/system/qor.h index cddb44e..335cdcc 100644 --- a/software/system/qor.h +++ b/software/system/qor.h @@ -16,9 +16,9 @@ extern void ost_hal_panic(); * exposes the functions for interacting with it. */ -#define MAXNUMTHREADS 10 /* Maximum number of threads, allocated at compile time */ -#define STACKSIZE 100 /* Number of 32-bit words in each TCB's stack */ -#define THREADFREQ 1 /* Maximum time-slice, in Hz, before the scheduler is run */ +#define MAXNUMTHREADS 4 /* Maximum number of threads, allocated at compile time */ +#define STACKSIZE 100 /* Number of 32-bit words in each TCB's stack */ +#define THREADFREQ 1 /* Maximum time-slice, in Hz, before the scheduler is run */ #define OS_SCHEDL_PRIO_MIN 1 /* Lowest priority that can be assigned to a thread */ #define OS_SCHEDL_PRIO_MAX UINT8_MAX /* Highest priority that can be assigned to a thread */ @@ -59,6 +59,11 @@ typedef struct TCB uint64_t ts; //!< system timestamp const char *name; //!< Descriptive name to facilitate debugging + // Debug/traces + uint32_t stack_usage; + uint32_t *stack_bottom; + bool so; // stack overflow detected + } qor_tcb_t; void qor_init(uint32_t scheduler_frequency_hz); @@ -69,8 +74,6 @@ bool qor_start(qor_tcb_t *idle_tcb, thread_func_t idle_task); void qor_sleep(uint32_t sleep_duration_ms); -void qor_svc_call(void); - // =========================================================================================================== // MAILBOX API // =========================================================================================================== diff --git a/software/system/qor_armv6m.s b/software/system/qor_armv6m.s index a1ab212..0d7d74a 100644 --- a/software/system/qor_armv6m.s +++ b/software/system/qor_armv6m.s @@ -105,6 +105,7 @@ qor_switch_context: @ Les registers r0-r3 r12 etc. sont sauvegardés par le processeur + cpsid i mrs r0, psp @ get the current stack address () ldr r1, =RunPt @@ -125,11 +126,9 @@ qor_switch_context: mov r7, r11 stmia r0!, {r4-r7} - cpsid i bl qor_scheduler - cpsie i - ldr R0, =RunPt + ldr r0, =RunPt ldr r1, [r0] ldr r0, [r1] @ R0 is the stack address @@ -143,6 +142,7 @@ qor_switch_context: msr psp, r0 @ new task stack top address + str r0, [r1] @ Save the new top of stack subs r0, r0, #32 ldmia r0!, {r4-r7} @@ -150,6 +150,6 @@ qor_switch_context: // Exit handler. Using a bx to the special EXC_RETURN values causes the // processor to perform the exception return behavior. ldr r0, =EXC_RETURN - + cpsie i bx r0