stable QoRTOS (for now), before cleaning

software/.vscode/.cortex-debug.peripherals.state.json

@@ -1 +1 @@
-[{"node":"TIMER","expanded":true,"format":0,"pinned":false}]
+[{"node":"TIMER","expanded":true,"format":0,"pinned":false},{"node":"TIMER.INTR","expanded":true,"format":0},{"node":"TIMER.INTE","expanded":true,"format":0}]

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)
 

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();
   }
 }
 

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);
+    int cpt = 0;
-    gpio_set_dir(1, GPIO_OUT);
 
     while (1)
     {
-        // gpio_put(1, 0);
+
         ost_hal_gpio_set(OST_GPIO_DEBUG_LED, 0);
-        qor_sleep(1000);
+        qor_sleep(500);
-        // gpio_put(1, 1);
         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);
+        // }
     }
 }
 
@@ -170,36 +175,12 @@ void IdleTaskFunction(void *args)
     }
 }
 
-/*
-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(&tcb1, UserTask_1, 2, "UserTask_1");
-    //   qor_create_thread(&tcb2, UserTask_2, 1, "UserTask_1");
+    // qor_create_thread(&tcb2, UserTask_2, 1, "UserTask_2");
     qor_create_thread(&tcb3, UserTask_3, 3, "UserTask_3");
 
     qor_start(&idle, IdleTaskFunction);

software/system/qor.c

@@ -14,6 +14,7 @@
 #include "debug.h"
 #include "qor.h"
 #include <stdlib.h>
+#include <string.h>
 
 // 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()
+static uint32_t gCritialNesting = 0;
-{
-    __asm volatile("cpsid i");
-}
 
-void qor_sleep_ms(uint8_t svc, uint32_t ms)
+#define enable_irq() __asm volatile("cpsie i")
-{
+#define disable_irq() __asm volatile("cpsid i")
-
-    __wfi;
-}
 
+/*
 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)
+static uint64_t gNextAlarm = 0;
-{
-    // Clear the alarm irq
-    hw_clear_bits(&timer_hw->intr, 1u << ALARM_NUM);
-    qor_svc_call();
-}
 
-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;
+        gNextAlarm = timer_hw->timerawl + delay_ms * 1000;
-        timer_hw->alarm[ALARM_NUM] = (uint32_t)target;
+        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);
+    irq_set_exclusive_handler(ALARM_IRQ, timer_end);
-    // Enable the interrupt for our alarm (the timer outputs 4 alarm irqs)
+    // Disable the intterupt for this alarm
-    hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
+    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();
+    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;
-    qor_exit_critical(status);
+        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();
 }

software/system/qor.h

@@ -16,7 +16,7 @@ extern void ost_hal_panic();
  * exposes the functions for interacting with it.
  */
 
-#define MAXNUMTHREADS 10 /* Maximum number of threads, allocated at compile time */
+#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 */
 
@@ -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
 // ===========================================================================================================

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