broken

include/drivers/buttons.hpp

@@ -7,7 +7,7 @@
 #include "microbit_v2.h"
 
 namespace edge::drivers {
-void handle_callback();
+void handle_callback(int channel);
 
 class ButtonController {
     GPIOPin button_a{BTN_A, GPIOConfiguration::IN_PUR};
@@ -16,16 +16,24 @@ class ButtonController {
     GPIOTEPin int_button_a{
         0, BTN_A, aidan::GPIOTEEventPolarity::HI_TO_LO, &edge::drivers::handle_callback
     };
+    GPIOTEPin int_button_b{
+        1, BTN_B, aidan::GPIOTEEventPolarity::HI_TO_LO, &edge::drivers::handle_callback
+    };
 
-    etl::array<void*, MAX_PROCESSES> callbacks{nullptr};
-    etl::array<bool, MAX_PROCESSES> callback_ready{false};
+    using SubscriptionArray = etl::array<ButtonCallbackPtr, MAX_PROCESSES>;
+    etl::array<SubscriptionArray, 2> subscriptions;
+    etl::array<etl::vector<button_subscribe_callback, 10>, MAX_PROCESSES>
+        ready_callbacks;
 
 public:
-    void handle_callback()
+    void handle_callback(int button_type)
     {
+        auto& button_subscriptions = subscriptions[button_type];
         for (int i = 0; i < MAX_PROCESSES; i++) {
-            if (callbacks[i] != nullptr) {
-                callback_ready[i] = true;
+            if (button_subscriptions[i]) {
+                ready_callbacks[i].emplace_back(
+                    button_subscriptions[i], static_cast<ButtonType>(button_type)
+                );
             }
         }
     }
@@ -40,19 +48,21 @@ class ButtonController {
         }
     }
 
-    void subscribe_button_press(ButtonType type, void* callback, uint8_t process_id)
+    void subscribe_button_press(
+        ButtonType type, ButtonCallbackPtr callback, uint8_t process_id
+    )
     {
-        callbacks[process_id] = callback;
+        subscriptions[static_cast<unsigned>(type)][process_id] = callback;
     }
 
-    etl::optional<void*> get_ready_callback(uint8_t process_id)
+    etl::optional<button_subscribe_callback> get_ready_callback(uint8_t process_id)
     {
-        if (callbacks[process_id] == nullptr || !callback_ready[process_id])
+        if (ready_callbacks[process_id].empty()) {
             return etl::nullopt;
-        etl::optional<void*> opt = callbacks[process_id];
-        callbacks[process_id] = nullptr;
-        callback_ready[process_id] = false;
-        return opt;
+        }
+        button_subscribe_callback ret = ready_callbacks[process_id].back();
+        ready_callbacks[process_id].pop_back();
+        return ret;
     }
 };
 

include/drivers/driver_controller.hpp

@@ -11,9 +11,10 @@ void do_async_work();
 etl::optional<int> handle_command(DriverCommand type, int arg1, int arg2, int arg3);
 
 void handle_subscribe(
-    DriverSubscribe type, void* callback, int arg1, int arg2, uint8_t process_id
+    DriverSubscribe type, ButtonCallbackPtr callback, int arg1, int arg2,
+    uint8_t process_id
 );
 
-etl::optional<void*> get_ready_callback(uint8_t process_id);
+etl::optional<button_subscribe_callback> get_ready_callback(uint8_t process_id);
 
 } // namespace edge::drivers

include/drivers/driver_enums.hpp

@@ -12,5 +12,13 @@ enum class DriverSubscribe { NOTIFY_BUTTON_PRESS = 0 };
 
 enum class GPIOConfiguration { OUT, IN_NORES, IN_PDR, IN_PUR };
 
-enum class ButtonType { A, B };
+enum class ButtonType { A = 0, B = 1 };
+
+using ButtonCallbackPtr = void (*)(ButtonType);
+
+struct button_subscribe_callback {
+    ButtonCallbackPtr callback;
+    ButtonType type;
+};
+
 } // namespace edge::drivers

include/drivers/gpiote_pin.hpp

@@ -8,9 +8,10 @@ class GPIOTEPin {
     uint32_t channel;
 
 public:
+    // TODO: use PORT event instead so we aren't limited to 8 of these
     GPIOTEPin(
         uint32_t channel, uint32_t pin_number, aidan::GPIOTEEventPolarity polarity,
-        void (*callback)()
+        void (*callback)(int)
     ) : channel(channel)
     {
         aidan::configure_gpiote_event(channel, pin_number, polarity);

include/hil/gpiote_wrapper.hpp

@@ -6,7 +6,7 @@
 
 namespace edge::aidan {
 
-void set_gpiote_callback(uint32_t channel, void (*callback)());
+void set_gpiote_callback(uint32_t channel, void (*callback)(int));
 
 void clear_gpiote_event(uint32_t channel);
 

include/scheduler.hpp

@@ -7,12 +7,13 @@
 #include <cstdint>
 
 static constexpr size_t STACK_SIZE_BYTES = 2048;
-static constexpr size_t QUANTUM_MILLIS = 10;
+static constexpr size_t QUANTUM_MILLIS = 5;
 
 namespace edge {
 extern "C" {
 void PendSV_Handler();
 void SVC_Handler();
+void restore();
 }
 
 class Scheduler {
@@ -66,6 +67,7 @@ class Scheduler {
 
     friend void PendSV_Handler(void);
     friend void SVC_Handler(void);
+    friend void restore(void);
 };
 
 extern Scheduler scheduler;

include/userlib/syscalls.hpp

@@ -1,8 +1,6 @@
 #pragma once
 
 #include "drivers/driver_enums.hpp"
-#include "register_utils.hpp"
-#include "userlib/system_call_type.hpp"
 
 namespace edge::userlib {
 
@@ -16,18 +14,17 @@ void change_priority(uint8_t new_priority);
  *
  * In other words, this must be inlined
  */
-__attribute__((always_inline)) inline void yield()
-{
-    TRIGGER_SVC(SystemCallType::YIELD);
-}
+void yield();
 
 void set_led(uint8_t row, uint8_t col, bool enabled);
 
 int get_time_us();
 
 bool get_button_pressed(drivers::ButtonType button_type);
 
-void get_button_pressed(drivers::ButtonType button_type, void* callback);
+void get_button_pressed(
+    drivers::ButtonType button_type, void (*callback)(drivers::ButtonType)
+);
 
 // We need a syscall for this because SVC will not be preempted by SysTick
 // Technically this is insecure - it's mostly for debugging

src/buttons.cpp

@@ -3,8 +3,8 @@
 namespace edge::drivers {
 ButtonController button_controller;
 
-void handle_callback()
+void handle_callback(int channel)
 {
-    button_controller.handle_callback();
+    button_controller.handle_callback(channel);
 }
 } // namespace edge::drivers

src/driver_controller.cpp

@@ -32,7 +32,8 @@ etl::optional<int> handle_command(DriverCommand type, int arg1, int arg2, int ar
 }
 
 void handle_subscribe(
-    DriverSubscribe type, void* callback, int arg1, int arg2, uint8_t process_id
+    DriverSubscribe type, ButtonCallbackPtr callback, int arg1, int arg2,
+    uint8_t process_id
 )
 {
     switch (type) {
@@ -44,10 +45,7 @@ void handle_subscribe(
     }
 }
 
-// For now, no arguments in callbacks. Essentially just a trigger
-// I don't like this interface, but the overall arch is ok for now
-// We can discuss how to make this more generic
-etl::optional<void*> get_ready_callback(uint8_t process_id)
+etl::optional<button_subscribe_callback> get_ready_callback(uint8_t process_id)
 {
     return button_controller.get_ready_callback(process_id);
 }

src/gpiote_wrapper.cpp

@@ -8,9 +8,9 @@ namespace edge::aidan {
 
 constexpr uint32_t MAX_GPIOTE_CHANNELS = 8;
 
-etl::array<void (*)(), MAX_GPIOTE_CHANNELS> gpiote_callbacks{nullptr};
+etl::array<void (*)(int), MAX_GPIOTE_CHANNELS> gpiote_callbacks{nullptr};
 
-void set_gpiote_callback(uint32_t channel, void (*callback)())
+void set_gpiote_callback(uint32_t channel, void (*callback)(int))
 {
     gpiote_callbacks[channel] = callback;
     nrf_gpiote_int_enable(NRF_GPIOTE_INT_IN0_MASK << channel);
@@ -50,7 +50,7 @@ void GPIOTE_IRQHandler()
             nrf_gpiote_event_clear(GPIOTE_EVENTS[channel]);
 
             if (gpiote_callbacks[channel] != nullptr) {
-                gpiote_callbacks[channel]();
+                gpiote_callbacks[channel](channel);
             }
         }
     }

src/main.cpp

@@ -1,4 +1,3 @@
-#include "drivers/led_display.hpp"
 #include "nrf_delay.h"
 #include "scheduler.hpp"
 #include "userlib/syscalls.hpp"
@@ -19,13 +18,19 @@ void task(void)
     change_priority(1);
 
     static bool flipped = false;
-    static void (*on_button_press)();
-    on_button_press = []() {
+    static int num = 0;
+    static void (*on_button_press)(ButtonType) = [](ButtonType button_type) {
+		// volatile int num = 0;
+        // num++;
+        // etl::string<20> v;
+        // etl::to_string(num, v);
+        // debug_println(v);
         flipped = !flipped;
-        get_button_pressed(ButtonType::A, (void*)(+on_button_press));
+        nrf_delay_ms(1000);
     };
 
-    get_button_pressed(ButtonType::A, (void*)+on_button_press);
+    get_button_pressed(ButtonType::A, on_button_press);
+    // get_button_pressed(ButtonType::B, on_button_press);
 
     while (1) {
         if (flipped) {
@@ -45,10 +50,10 @@ int main(void)
 {
     printf("Starting EdgeOS\n");
 
-    edge::scheduler.add_task(task<0>);
-    edge::scheduler.add_task(task<1>);
-    edge::scheduler.add_task(task<2>);
-    edge::scheduler.add_task(task<3>);
+    // edge::scheduler.add_task(task<0>);
+    // edge::scheduler.add_task(task<1>);
+    // edge::scheduler.add_task(task<2>);
+    // edge::scheduler.add_task(task<3>);
     edge::scheduler.add_task(task<4>);
 
     edge::scheduler.start_scheduler();

src/scheduler.cpp

@@ -1,9 +1,7 @@
 #include "scheduler.hpp"
 
 #include "drivers/driver_controller.hpp"
-#include "drivers/led_display.hpp"
 #include "nrf52833.h"
-#include "register_utils.hpp"
 
 namespace edge {
 
@@ -20,6 +18,8 @@ void Scheduler::start_scheduler()
         SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk;
 
     NVIC_SetPriority(PendSV_IRQn, 0x3);
+    // TODO: move to bootloader?
+    NVIC_SetPriority(GPIOTE_IRQn, 0x2);
     NVIC_SetPriority(SysTick_IRQn, 0x1);
     asm volatile("CPSIE I");
     asm volatile("SVC #0");
@@ -58,7 +58,7 @@ __attribute__((naked, used)) void PendSV_Handler()
     asm volatile("CPSID I");
     if (--scheduler.slices_remaining == 0) {
         asm volatile("mrs r0,psp\n"
-                     "sub r0,#96\n"
+                     "sub r0,#32\n"
                      "stm r0!,{r4-r11}");
 
         // This function will dirty registers. That's okay
@@ -78,7 +78,7 @@ __attribute__((naked, used)) void PendSV_Handler()
         ));
 
         asm volatile("mrs r0,psp\n"
-                     "sub r0,#96\n"
+                     "sub r0,#32\n"
                      "ldm r0!,{r4-r11}\n");
     }
 
@@ -101,14 +101,41 @@ __attribute__((used)) void SysTick_Handler()
 }
 }
 
+__attribute__((used, naked)) void restore()
+{
+    asm volatile("mrs r0, psp\n"
+                 "add r0, #32\n"
+                 "msr psp, r0\n");
+    asm volatile("ldr     r0, [sp, #0]\n"
+                 "ldr     r1, [sp, #4]\n"
+                 "ldr     r2, [sp, #8]\n"
+                 "ldr     r3, [sp, #12]\n"
+                 "ldr     r12, [sp, #16]\n"
+                 "ldr     lr, [sp, #20]\n"
+                 "ldr     pc, [sp, #24]\n");
+}
+
 void Scheduler::yield_current_task()
 {
     auto callback_opt = drivers::get_ready_callback(current_task_index);
     if (callback_opt) {
-        auto addr = (uint32_t)callback_opt.value();
+        printf("Yield1\n");
+        auto [address, arg1] = callback_opt.value();
         auto& t = scheduler.task_stack[scheduler.current_task_index];
-        *(t.stack_ptr_loc + 5) = *(t.stack_ptr_loc + 6) + 1;
-        *(t.stack_ptr_loc + 6) = addr;
+        t.stack_ptr_loc = (unsigned*)__get_PSP();
+        (*(t.stack_ptr_loc + 6))++;
+
+        // "Push" registers
+        t.stack_ptr_loc -= 8;
+        *(t.stack_ptr_loc + 0) = *(t.stack_ptr_loc + 8 + 0);
+        *(t.stack_ptr_loc + 1) = *(t.stack_ptr_loc + 8 + 1);
+        *(t.stack_ptr_loc + 2) = *(t.stack_ptr_loc + 8 + 2);
+        *(t.stack_ptr_loc + 3) = *(t.stack_ptr_loc + 8 + 3);
+        *(t.stack_ptr_loc + 4) = *(t.stack_ptr_loc + 8 + 4);
+        *(t.stack_ptr_loc + 5) = (unsigned)&restore;
+        *(t.stack_ptr_loc + 6) = (unsigned)address;
+        *(t.stack_ptr_loc + 7) = *(t.stack_ptr_loc + 8 + 7);
+        __set_PSP((unsigned)t.stack_ptr_loc);
     }
     else {
         slices_remaining = 1;

src/svc.cpp

@@ -26,9 +26,11 @@ etl::optional<int> handle_driver_command(uint32_t* stack_ptr)
 void handle_driver_subscribe(uint32_t* stack_ptr)
 {
     auto type = static_cast<drivers::DriverSubscribe>(stack_ptr[0]);
+    auto callback = reinterpret_cast<drivers::ButtonCallbackPtr>(stack_ptr[1]);
+    int arg1 = stack_ptr[2];
+    int arg2 = stack_ptr[3];
     return drivers::handle_subscribe(
-        type, (void*)stack_ptr[1], stack_ptr[2], stack_ptr[3],
-        scheduler.get_current_task()
+        type, callback, arg1, arg2, scheduler.get_current_task()
     );
 }
 

src/syscalls.cpp

@@ -1,5 +1,6 @@
 #include "userlib/syscalls.hpp"
 
+#include "drivers/driver_enums.hpp"
 #include "register_utils.hpp"
 #include "userlib/system_call_type.hpp"
 
@@ -19,7 +20,14 @@ void change_priority(uint8_t new_priority)
     TRIGGER_SVC(SystemCallType::CHANGE_PRIORITY);
 }
 
-void get_button_pressed(drivers::ButtonType button_type, void* callback)
+void yield()
+{
+    TRIGGER_SVC(SystemCallType::YIELD);
+}
+
+void get_button_pressed(
+    drivers::ButtonType button_type, void (*callback)(drivers::ButtonType)
+)
 {
     SET_REGISTER(r0, (int)drivers::DriverSubscribe::NOTIFY_BUTTON_PRESS);
     SET_REGISTER(r1, (int)callback);