migrate: デュアルコア対応

CMakeLists.txt

@@ -37,20 +37,20 @@ pico_sdk_init()
 add_subdirectory(tinykernel)
 
 # Add executable demo
-add_executable(tinykernel_app main.cpp)
+add_executable(tinykernel main.cpp)
 
-pico_set_program_name(tinykernel_app "tinykernel")
-pico_set_program_version(tinykernel_app "0.1")
+pico_set_program_name(tinykernel "tinykernel")
+pico_set_program_version(tinykernel "0.1")
 
 # Modify the below lines to enable/disable output over UART/USB
-pico_enable_stdio_uart(tinykernel_app 1)
-pico_enable_stdio_usb(tinykernel_app 0)
+pico_enable_stdio_uart(tinykernel 1)
+pico_enable_stdio_usb(tinykernel 0)
 
 # Link libraries
-target_link_libraries(tinykernel_app
+target_link_libraries(tinykernel
         pico_stdlib
-        tinykernel
+        tinykernel_lib
 )
 
-pico_add_extra_outputs(tinykernel_app)
+pico_add_extra_outputs(tinykernel)
 

main.cpp

@@ -5,10 +5,13 @@
 
 #include "tinykernel/kernel.hpp"
 
+#include "pico/multicore.h"
+
 // Atomic printf helper: stdio is shared, so wrap each call with the kernel lock.
 #define P(...)                                  \
     do {                                       \
         bool _lk = tk::lock();                 \
+        printf("[C%u] ", (unsigned)get_core_num()); \
         printf(__VA_ARGS__);                   \
         tk::unlock(_lk);                       \
     } while (0)

tinykernel/CMakeLists.txt

@@ -1,16 +1,19 @@
 # tinykernel: minimal preemptive task scheduler library (RP2350 / Pico 2)
 
-add_library(tinykernel STATIC
+add_library(tinykernel_lib STATIC
     src/scheduler.cpp
     src/port/m33/port_m33.c
     src/port/m33/context.S
 )
 
-target_include_directories(tinykernel PUBLIC
+
+target_include_directories(tinykernel_lib PUBLIC
     ${CMAKE_CURRENT_LIST_DIR}/include
 )
 
-target_link_libraries(tinykernel PUBLIC
+target_link_libraries(tinykernel_lib PUBLIC
     pico_stdlib
+    pico_multicore
+    hardware_sync
 )
 

tinykernel/src/port/m33/context.S

@@ -22,16 +22,24 @@
     .global tk_port_start
     .global tk_port_make_frame
 
-// clobbers r3
+// clobbers r2, r3
 .macro save_fpu_to_scratch
-    ldr     r3, =tk_fpu_scratch
+    ldr     r3, =0xd0000000           // SIO_BASE
+    ldr     r3, [r3]                  // CPUID (0 or 1)
+    lsls    r3, r3, #7                // core * 128 (sizeof(FpuCtx))
+    ldr     r2, =tk_fpu_scratch
+    adds    r3, r2, r3                // &tk_fpu_scratch[core]
     vstmia  r3!, {d0-d7}
     vstmia  r3!, {d8-d15}
 .endm
 
-// clobbers r3
+// clobbers r2, r3
 .macro restore_fpu_from_scratch
-    ldr     r3, =tk_fpu_scratch
+    ldr     r3, =0xd0000000           // SIO_BASE
+    ldr     r3, [r3]                  // CPUID (0 or 1)
+    lsls    r3, r3, #7                // core * 128 (sizeof(FpuCtx))
+    ldr     r2, =tk_fpu_scratch
+    adds    r3, r2, r3                // &tk_fpu_scratch[core]
     vldmia  r3!, {d0-d7}
     vldmia  r3!, {d8-d15}
 .endm

tinykernel/src/scheduler.cpp

@@ -3,13 +3,11 @@
 #include <cstring>
 
 #include "pico/platform.h"
+#include "pico/multicore.h"
+#include "hardware/sync.h"
 
 #include "port/m33/port_m33.h"
 
-// FPU state saved explicitly on every switch. Lazy stacking is disabled
-// (FPCCR.LSPEN=0 at kernel start and never re-enabled), so hardware never pushes
-// VFP frames onto task stacks; S0-S31 are the only floating point state that
-// matters (see context.S).
 struct FpuCtx {
     float s[32];
 };
@@ -17,7 +15,9 @@ static_assert(sizeof(FpuCtx) == 128, "FPU context layout must match context.S");
 
 namespace {
 
-enum class State : uint8_t { Free = 0, Ready, Sleeping, Terminated };
+constexpr uint32_t TK_NUM_CORES = 2;
+
+enum class State : uint8_t { Free = 0, Ready, Running, Sleeping, Terminated };
 
 struct TaskSlot {
     State state;
@@ -34,18 +34,28 @@ struct TaskSlot {
 
 static_assert(TK_MAX_TASKS <= 255, "slot index must fit in a byte");
 
-alignas(16) static TaskSlot g_slots[TK_MAX_TASKS];  // slot 0 = idle (reserved)
+alignas(16) static TaskSlot g_slots[TK_MAX_TASKS];  // slot 0 = idle0, slot 1 = idle1
 alignas(8) static char g_stacks[TK_MAX_TASKS][TK_DEFAULT_STACK_SIZE];
 
-static uint8_t g_cur = 0;
+static uint8_t g_cur[TK_NUM_CORES] = {0, 1};
 static int64_t g_tick = 0;
 static bool g_started = false;
 static uint8_t g_rr_last[16] = {0};
 
+static spin_lock_t* g_sched_lock = nullptr;
+
 constexpr uint32_t kIdlePrio = 15u;
 constexpr int32_t kSliceTicks = (int32_t)(((uint64_t)TK_TIME_SLICE_MS * TK_TICKS_PER_SEC + 999u) / 1000u);
 static_assert(kSliceTicks >= 1, "time slice must be at least one tick");
 
+static inline uint32_t sched_lock(void) {
+    return spin_lock_blocking(g_sched_lock);
+}
+
+static inline void sched_unlock(uint32_t save) {
+    spin_unlock(g_sched_lock, save);
+}
+
 static void idle_fn(void* arg) {
     (void)arg;
     for (;;) {
@@ -53,72 +63,79 @@ static void idle_fn(void* arg) {
     }
 }
 
-static inline bool is_runnable(const TaskSlot& s) {
-    if (s.is_idle) return true;
+static inline bool is_runnable(uint32_t slot_idx, uint32_t core) {
+    const TaskSlot& s = g_slots[slot_idx];
+    if (s.is_idle) {
+        return slot_idx == core; // Core 0 only runs idle 0, Core 1 only runs idle 1
+    }
     return s.state == State::Ready;
 }
 
-// Highest priority level (lowest number) that has at least one runnable task. Idle
-// guarantees that level 15 always qualifies, so this never returns -1 after start().
-static int32_t find_prio_level(void) {
+// Highest priority level with at least one runnable task for this core.
+static int32_t find_prio_level(uint32_t core) {
     for (uint32_t p = 0; p < 16u; ++p) {
         for (uint32_t i = 0; i < TK_MAX_TASKS; ++i) {
-            if (g_slots[i].prio == p && is_runnable(g_slots[i])) return (int32_t)p;
+            if (g_slots[i].prio == p && is_runnable(i, core)) return (int32_t)p;
         }
     }
-    return -1;
+    return (int32_t)kIdlePrio;
 }
 
-// Round-robin pick among runnable tasks of the given priority level, starting after
-// g_rr_last[level] so that equal-priority tasks rotate fairly.
-static uint32_t pick_round_robin(uint32_t level) {
+static uint32_t pick_round_robin(uint32_t level, uint32_t core) {
+    if (level == kIdlePrio) return core;
     for (uint32_t k = 1; k <= TK_MAX_TASKS; ++k) {
         uint32_t i = (g_rr_last[level] + k) % TK_MAX_TASKS;
-        if (g_slots[i].prio == level && is_runnable(g_slots[i])) return i;
+        if (i >= TK_NUM_CORES && g_slots[i].prio == level && is_runnable(i, core)) return i;
     }
-    return g_cur;
+    return core;
 }
 
-// Choose the next task to run. Returns a slot index, or g_cur to keep running.
-static uint32_t select_next(bool force_switch) {
-    int32_t level = find_prio_level();
-    if (level < 0) return g_cur;
-    const TaskSlot& cur = g_slots[g_cur];
-    if (!force_switch && cur.prio == (uint32_t)level && is_runnable(cur)) return g_cur;
-    // pick_round_robin only ever returns a runnable slot, so if the current task is no
-    // longer runnable it can never be picked back here.
-    return pick_round_robin((uint32_t)level);
+// Choose the next task for the given core.
+static uint32_t select_next(uint32_t core) {
+    int32_t level = find_prio_level(core);
+    if (level < 0) return core;
+    return pick_round_robin((uint32_t)level, core);
 }
 
 }  // namespace
 
-// Referenced (unmangled) from context.S: scratch area the port asm uses to move FPU
-// state between hardware registers and the per-task copies in g_slots[].
+// Referenced (unmangled) from context.S: scratch area per-core for FPU state transfer.
 extern "C" {
-    alignas(8) FpuCtx tk_fpu_scratch = {};
+    alignas(8) FpuCtx tk_fpu_scratch[TK_NUM_CORES] = {};
 }
 
-// SysTick interrupt handler: advances ticks, wakes sleeping tasks, and triggers PendSV when needed.
+// SysTick interrupt handler: advances ticks on Core 0, wakes sleeping tasks, and checks time slices.
 extern "C" void isr_systick(void) {
-    ++g_tick;
+    uint32_t core = get_core_num();
+    uint32_t save = sched_lock();
+
     bool need_switch = false;
-    for (uint32_t i = 1; i < TK_MAX_TASKS; ++i) {
-        TaskSlot& s = g_slots[i];
-        if (s.state == State::Sleeping && g_tick >= s.wake_tick) {
-            s.state = State::Ready;
-            if (s.prio < g_slots[g_cur].prio) {
+    if (core == 0) {
+        ++g_tick;
+        for (uint32_t i = TK_NUM_CORES; i < TK_MAX_TASKS; ++i) {
+            TaskSlot& s = g_slots[i];
+            if (s.state == State::Sleeping && g_tick >= s.wake_tick) {
+                s.state = State::Ready;
                 need_switch = true;
             }
         }
     }
-    TaskSlot& cur = g_slots[g_cur];
-    if (!cur.is_idle) {
+
+    TaskSlot& cur = g_slots[g_cur[core]];
+    if (cur.is_idle) {
+        if (find_prio_level(core) < (int32_t)kIdlePrio) {
+            need_switch = true;
+        }
+    } else {
         --cur.slice_left;
         if (cur.slice_left <= 0) {
             cur.slice_left = kSliceTicks;
             need_switch = true;
         }
     }
+
+    sched_unlock(save);
+
     if (need_switch) {
         tk_port_trigger_pendsv();
     }
@@ -126,20 +143,29 @@ extern "C" void isr_systick(void) {
 
 // PendSV context switch function: saves cur_sp, selects next task, and returns next SP.
 extern "C" uintptr_t tk_switch_context(uintptr_t cur_sp) {
-    TaskSlot& cur = g_slots[g_cur];
+    uint32_t core = get_core_num();
+    uint32_t save = sched_lock();
+
+    TaskSlot& cur = g_slots[g_cur[core]];
     cur.sp = cur_sp;
-    memcpy(&cur.fpu, &tk_fpu_scratch, sizeof(cur.fpu));
+    memcpy(&cur.fpu, &tk_fpu_scratch[core], sizeof(cur.fpu));
+    if (cur.state == State::Running) {
+        cur.state = State::Ready;
+    }
 
-    uint32_t next = select_next(/*force_switch=*/true);
+    uint32_t next = select_next(core);
     TaskSlot& nxt = g_slots[next];
     if (nxt.sp == 0) {
         nxt.sp = tk_port_make_frame(reinterpret_cast<uint8_t*>(&g_stacks[next][0]),
                                     TK_DEFAULT_STACK_SIZE, reinterpret_cast<void*>(nxt.fn), nxt.arg);
     }
-    memcpy(&tk_fpu_scratch, &nxt.fpu, sizeof(nxt.fpu));
+    nxt.state = State::Running;
+    memcpy(&tk_fpu_scratch[core], &nxt.fpu, sizeof(nxt.fpu));
     nxt.slice_left = kSliceTicks;
     g_rr_last[nxt.prio] = (uint8_t)next;
-    g_cur = (uint8_t)next;
+    g_cur[core] = (uint8_t)next;
+
+    sched_unlock(save);
     return nxt.sp;
 }
 
@@ -147,7 +173,7 @@ namespace tk::detail {
 
 void static_register(const char* name, TaskFn fn, uint32_t prio) {
     if (prio >= kIdlePrio) prio = 14u;
-    for (uint32_t i = 1; i < TK_MAX_TASKS; ++i) {
+    for (uint32_t i = TK_NUM_CORES; i < TK_MAX_TASKS; ++i) {
         if (g_slots[i].state == State::Free) {
             g_slots[i] = TaskSlot{};
             g_slots[i].fn = fn;
@@ -164,7 +190,7 @@ void static_register(const char* name, TaskFn fn, uint32_t prio) {
 tk::TaskHandle tk::create(TaskFn fn, void* arg, uint32_t prio) {
     TaskHandle h{0xFF};
     if (prio >= kIdlePrio) prio = 14u;
-    for (uint32_t i = 1; i < TK_MAX_TASKS; ++i) {
+    for (uint32_t i = TK_NUM_CORES; i < TK_MAX_TASKS; ++i) {
         if (g_slots[i].state == State::Free) {
             g_slots[i] = TaskSlot{};
             g_slots[i].fn = fn;
@@ -179,39 +205,67 @@ tk::TaskHandle tk::create(TaskFn fn, void* arg, uint32_t prio) {
     return h;
 }
 
-void tk::start() {
-    if (g_started) for (;;) __asm__ volatile("wfi");
-    g_started = true;
-
-    TaskSlot& idle = g_slots[0];
-    idle.fn = idle_fn;
-    idle.name = "idle";
-    idle.prio = kIdlePrio;
-    idle.is_idle = true;
-    idle.state = State::Ready;
+static void core_start_routine(uint32_t core) {
+    tk_port_init(TK_TICKS_PER_SEC);
 
-    tk_port_init(TK_TICKS_PER_SEC);  // FPU policy + SysTick (enabled at the end)
-
-    uint32_t next = select_next(/*force_switch=*/true);
+    uint32_t save = sched_lock();
+    uint32_t next = select_next(core);
     TaskSlot& nxt = g_slots[next];
     if (nxt.sp == 0) {
         nxt.sp = tk_port_make_frame(reinterpret_cast<uint8_t*>(&g_stacks[next][0]),
                                     TK_DEFAULT_STACK_SIZE, reinterpret_cast<void*>(nxt.fn), nxt.arg);
     }
-    memcpy(&tk_fpu_scratch, &nxt.fpu, sizeof(nxt.fpu));
+    nxt.state = State::Running;
+    memcpy(&tk_fpu_scratch[core], &nxt.fpu, sizeof(nxt.fpu));
     nxt.slice_left = kSliceTicks;
     g_rr_last[nxt.prio] = (uint8_t)next;
-    g_cur = (uint8_t)next;
+    g_cur[core] = (uint8_t)next;
+    sched_unlock(save);
 
     tk_port_start(nxt.sp);  // never returns
     for (;;) __asm__ volatile("wfi");
 }
 
+static void core1_entry(void) {
+    core_start_routine(1);
+}
+
+void tk::start() {
+    if (g_started) for (;;) __asm__ volatile("wfi");
+    g_started = true;
+
+    int lock_num = spin_lock_claim_unused(true);
+    g_sched_lock = spin_lock_instance(lock_num);
+
+    // Core 0 Idle Task
+    TaskSlot& idle0 = g_slots[0];
+    idle0.fn = idle_fn;
+    idle0.name = "idle0";
+    idle0.prio = kIdlePrio;
+    idle0.is_idle = true;
+    idle0.state = State::Ready;
+
+    // Core 1 Idle Task
+    TaskSlot& idle1 = g_slots[1];
+    idle1.fn = idle_fn;
+    idle1.name = "idle1";
+    idle1.prio = kIdlePrio;
+    idle1.is_idle = true;
+    idle1.state = State::Ready;
+
+    // Launch Core 1
+    multicore_launch_core1(core1_entry);
+
+    // Launch Core 0
+    core_start_routine(0);
+}
+
 void tk::yield() {
-    bool lk = tk::lock();
-    g_slots[g_cur].slice_left = 0;
+    uint32_t core = get_core_num();
+    uint32_t save = sched_lock();
+    g_slots[g_cur[core]].slice_left = 0;
     tk_port_trigger_pendsv();
-    tk::unlock(lk);
+    sched_unlock(save);
 }
 
 void tk::sleep_ms(uint32_t ms) {
@@ -219,21 +273,23 @@ void tk::sleep_ms(uint32_t ms) {
     if (ticks < 1u) ticks = 1u;
     int32_t sleep_ticks = (int32_t)(ticks > 0x7FFFFFFFu ? 0x7FFFFFFFu : ticks);
 
-    bool lk = tk::lock();
-    TaskSlot& cur = g_slots[g_cur];
+    uint32_t core = get_core_num();
+    uint32_t save = sched_lock();
+    TaskSlot& cur = g_slots[g_cur[core]];
     cur.state = State::Sleeping;
     cur.wake_tick = g_tick + sleep_ticks;
     tk_port_trigger_pendsv();
-    tk::unlock(lk);
+    sched_unlock(save);
 }
 
 void tk::exit(int code) {
     (void)code;
-    bool lk = tk::lock();
-    g_slots[g_cur].state = State::Terminated;
+    uint32_t core = get_core_num();
+    uint32_t save = sched_lock();
+    g_slots[g_cur[core]].state = State::Terminated;
     tk_port_trigger_pendsv();
-    tk::unlock(lk);
-    for (;;) __asm__ volatile("wfi");  // unreachable: the scheduler switches away
+    sched_unlock(save);
+    for (;;) __asm__ volatile("wfi");
 }
 
 extern "C" void tk_task_return_trampoline(void) {
@@ -245,13 +301,10 @@ uint64_t tk::now_ms() {
 }
 
 bool tk::lock() {
-    uint32_t primask;
-    __asm__ volatile("mrs %0, primask\n\tcpsid i" : "=r"(primask) :: "memory");
-    return primask == 0;
+    uint32_t save = sched_lock();
+    return save == 0; // PRIMASK was 0 (enabled)
 }
 
 void tk::unlock(bool was_enabled) {
-    if (was_enabled) {
-        __asm__ volatile("cpsie i" ::: "memory");
-    }
+    sched_unlock(was_enabled ? 0 : 1);
 }