Advanced 2: User/Kernel Mode and SVC#

Sample code#

Directory: docs/os-on-arduino/code/adv2_syscall

uv run pio run -d adv2_syscall -t upload
uv run pio device monitor -b 115200

Source code#

The sketch for this chapter. Click a file name to expand. The code is mirrored automatically from the manuscript repository, so what you see here is always current.

src/main.cpp — 94 lines

Open on GitHub

/**
 * 応用編 第2章: ユーザー／カーネルモードと SVC
 *
 * タスクが Serial や os_sleep() を直接呼ばず、すべて svc 命令を経由して
 * カーネルに依頼する形にしたデモ。
 *
 * user_putchar() などのラッパは svc を発行するだけで、実際に Serial を
 * 触るのは SVC_Handler の先にいる sys_putchar()。ユーザーコードから
 * ペリフェラルへの直接アクセスが 1 箇所も無いことがポイント。
 *
 * なお本章 2.3 の「非特権モードへ落とす」（CONTROL.nPRIV=1）は、
 * PendSV でタスクごとに CONTROL を切り替える改造が要る。本書はそこを
 * 擬似コードで示すにとどめているため、このサンプルも特権スレッドのまま
 * 動かしている。SVC の仕組み自体は特権／非特権のどちらからでも同じ。
 */

#include <Arduino.h>
#include "os_kernel.h"
#include "os_syscall.h"
#include "os_syscall_user.h"

// システムコールだけで書いた文字列出力
static void user_print(const char *s)
{
    while (*s) user_putchar(*s++);
}

static void user_print_int(int v)
{
    char buf[12];
    snprintf(buf, sizeof(buf), "%d", v);
    user_print(buf);
}

// ユーザータスク A: 自分の PID を名乗って寝るだけ
static void task_a(void)
{
    while (1) {
        user_print("[A] pid=");
        user_print_int(user_getpid());
        user_print("\r\n");
        user_sleep(1000);
    }
}

// ユーザータスク B: yield を挟みながら数える
static void task_b(void)
{
    int n = 0;
    while (1) {
        user_print("[B] count=");
        user_print_int(n++);
        user_print("\r\n");
        user_yield();
        user_sleep(1500);
    }
}

// 未定義の SVC 番号を呼ぶと -1 (ENOSYS 相当) が返ることを確かめる
static void task_probe(void)
{
    user_sleep(2500);
    int r = (int)SYSCALL0(99);
    user_print("[probe] svc #99 -> ");
    user_print_int(r);
    user_print("  (-1 なら ENOSYS 相当が返っている)\r\n");
    user_exit(0);
    while (1) user_sleep(1000);   // ここへは戻らない
}

void setup()
{
    Serial.begin(115200);
    while (!Serial) { }

    Serial.println(F("=== Advanced 2: SVC system calls ==="));
    Serial.println(F("以降の出力はすべて svc 経由（sys_putchar）で出ています"));

    os_init();

    // SVC の優先度を下げておく（重要）。
    // リセット直後の SVC は最高優先度 0 のため、SVC_Handler の中で
    // Serial.write() がバッファ空きを待つと、それを配る UART/USB 割り込み
    // （優先度はもっと低い）が永遠に走れず、システム全体が止まる。
    // 「ペリフェラル割り込みより低く、PendSV/SysTick(15) より高く」が正解。
    NVIC_SetPriority(SVCall_IRQn, 14);

    os_create_task(task_a,     "UserA", 512);
    os_create_task(task_b,     "UserB", 512);
    os_create_task(task_probe, "Probe", 512);
    os_start();
}

void loop() { }

src/os_kernel.cpp — 231 lines

Open on GitHub

/**
 * os_kernel.cpp - Mini OS Kernel Implementation
 * Chapter 6: Memory Protection
 */

#include "os_kernel.h"
#include <Arduino.h>
#include <string.h>

TCB g_tasks[MAX_TASKS];
volatile int g_task_count = 0;
volatile int g_current_task = -1;
volatile int g_next_task = -1;
volatile uint32_t g_system_ticks = 0;

const uint32_t g_tcb_size = sizeof(TCB);

static uint32_t g_stack_pool[STACK_POOL_SIZE / 4];
static uint32_t g_stack_used;   // 切り出し済みのワード数

static void idle_task(void) { while (1) __WFI(); }

// タスク関数が return したときの受け皿。例外フレームの LR スロットは
// ここを指す (第3章 3.4)。EXC_RETURN を置く場所ではない。
static void task_exit(void)
{
    g_tasks[g_current_task].state = TASK_TERMINATED;
    os_yield();
    while (1) { }  // ここには到達しない
}

static uint32_t* init_task_stack(uint32_t *stack_top, void (*entry)(void))
{
    uint32_t *sp = stack_top;
    *(--sp) = 0x01000000;
    *(--sp) = (uint32_t)entry;
    *(--sp) = (uint32_t)task_exit;  // LR: タスクが return したときの戻り先
    *(--sp) = 0; *(--sp) = 0; *(--sp) = 0; *(--sp) = 0; *(--sp) = 0;
    // FPU を使ったタスクは例外フレームが拡張される（FP レジスタ込み）。
    // どちらのフレームで中断したかは EXC_RETURN の値に現れるため、
    // タスクごとに EXC_RETURN も保存する。初期値は「PSP・基本フレーム」。
    *(--sp) = 0xFFFFFFFD;
    *(--sp) = 0; *(--sp) = 0; *(--sp) = 0; *(--sp) = 0;
    *(--sp) = 0; *(--sp) = 0; *(--sp) = 0; *(--sp) = 0;
    return sp;
}

void os_init(void)
{
    memset(g_tasks, 0, sizeof(g_tasks));
    g_task_count = 0;
    g_current_task = -1;
    g_next_task = -1;
    g_system_ticks = 0;
    os_create_task(idle_task, "idle", IDLE_STACK_SIZE);
}

int os_create_task(void (*entry)(void), const char *name, uint32_t stack_size)
{
    if (g_task_count >= MAX_TASKS) return -1;
    int id = g_task_count;
    TCB *task = &g_tasks[id];
    task->id = id;
    task->state = TASK_READY;
    task->priority = id;
    task->entry = entry;
    task->wake_tick = 0;
    task->cpu_ticks = 0;
    strncpy(task->name, name, sizeof(task->name) - 1);
    if (stack_size == 0) {
        stack_size = DEFAULT_STACK_SIZE;
    }
    uint32_t words = (stack_size + 3) / 4;
    if (g_stack_used + words > STACK_POOL_SIZE / 4) {
        return -1;          // プールが尽きた。黙って縮めず、失敗として返す
    }
    g_stack_used += words;
    task->stack_size = stack_size;
    task->stack_base = &g_stack_pool[g_stack_used];   // スタックは上端から下へ伸びる
    task->sp = init_task_stack(task->stack_base, entry);
    g_task_count++;
    return id;
}

void os_schedule(void)
{
    for (int i = 0; i < g_task_count; i++) {
        if (g_tasks[i].state == TASK_BLOCKED && g_system_ticks >= g_tasks[i].wake_tick) {
            g_tasks[i].state = TASK_READY;
        }
    }

    int next = g_current_task;
    do {
        next = (next + 1) % g_task_count;
        if (g_tasks[next].state == TASK_READY || g_tasks[next].state == TASK_RUNNING) break;
    } while (next != g_current_task);

    if (next != g_current_task || g_current_task < 0) {
        if (g_current_task >= 0 && g_tasks[g_current_task].state == TASK_RUNNING) {
            g_tasks[g_current_task].state = TASK_READY;
        }
        g_tasks[next].state = TASK_RUNNING;
        g_next_task = next;
        SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
    }
}

void os_yield(void) { os_schedule(); }
void os_sleep(uint32_t ms) {
    if (g_current_task > 0) {
        g_tasks[g_current_task].state = TASK_BLOCKED;
        g_tasks[g_current_task].wake_tick = g_system_ticks + ms;
        os_schedule();
    }
}
void os_suspend_task(int id) {
    if (id > 0 && id < g_task_count) {
        g_tasks[id].state = TASK_SUSPENDED;
        if (id == g_current_task) os_schedule();
    }
}
void os_resume_task(int id) {
    if (id > 0 && id < g_task_count && g_tasks[id].state == TASK_SUSPENDED) {
        g_tasks[id].state = TASK_READY;
    }
}
void os_terminate_task(int id) {
    if (id > 0 && id < g_task_count) {
        g_tasks[id].state = TASK_TERMINATED;
        if (id == g_current_task) os_schedule();
    }
}

uint32_t os_get_tick(void) { return g_system_ticks; }
int os_get_task_count(void) { return g_task_count; }
const char* os_get_task_name(int id) { return (id >= 0 && id < g_task_count) ? g_tasks[id].name : ""; }
TaskState os_get_task_state(int id) { return (id >= 0 && id < g_task_count) ? g_tasks[id].state : TASK_TERMINATED; }
int os_get_current_task(void) { return g_current_task; }

void os_start(void)
{
    NVIC_SetPriority(PendSV_IRQn, 0xFF);
    SysTick_Config(SystemCoreClock / 1000 * TIME_SLICE_MS);
    g_current_task = -1;
    g_next_task = 0;
    // Do NOT switch CONTROL to PSP here — PSP is not yet initialized.
    // PendSV returns with EXC_RETURN=0xFFFFFFFD, which automatically
    // switches thread mode to PSP after loading the first task's context.
    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
    while (1) { }
}

extern "C" void SysTick_Handler(void) {
    // 実際に経過した時間を、いま走っていたタスクの取り分として計上する。
    // os_schedule() 側で数えると os_yield() の回数まで混ざってしまう。
    if (g_current_task >= 0) {
        g_tasks[g_current_task].cpu_ticks += TIME_SLICE_MS;
    }
    g_system_ticks += TIME_SLICE_MS;
    os_schedule();
}

extern "C" __attribute__((naked)) void PendSV_Handler(void)
{
    __asm volatile (
        "   MRS     R0, PSP              \n"
        "   LDR     R1, =g_current_task  \n"
        "   LDR     R2, [R1]             \n"
        "   CMP     R2, #0               \n"
        "   BLT     _skip_save           \n"
        // FPU を使って中断したタスク（EXC_RETURN の bit4 が 0）は、
        // ハードウェアが積まない S16-S31 もここで退避する
        "   TST     LR, #0x10            \n"
        "   IT      EQ                   \n"
        "   VSTMDBEQ R0!, {S16-S31}      \n"
        // R4-R11 と EXC_RETURN(LR) を保存する
        "   STMDB   R0!, {R4-R11, LR}    \n"
        "   LDR     R3, =g_tasks         \n"
        "   LDR     R4, =g_tcb_size      \n"
        "   LDR     R4, [R4]             \n"
        "   MUL     R2, R2, R4           \n"
        "   ADD     R3, R3, R2           \n"
        "   STR     R0, [R3]             \n"
        "_skip_save:                     \n"
        "   LDR     R1, =g_next_task     \n"
        "   LDR     R2, [R1]             \n"
        "   LDR     R1, =g_current_task  \n"
        "   STR     R2, [R1]             \n"
        "   LDR     R3, =g_tasks         \n"
        "   LDR     R4, =g_tcb_size      \n"
        "   LDR     R4, [R4]             \n"
        "   MUL     R2, R2, R4           \n"
        "   ADD     R3, R3, R2           \n"
        "   LDR     R0, [R3]             \n"
        "   LDMIA   R0!, {R4-R11, LR}    \n"
        // FPU を使って中断していたタスクなら S16-S31 も復元する
        "   TST     LR, #0x10            \n"
        "   IT      EQ                   \n"
        "   VLDMIAEQ R0!, {S16-S31}      \n"
        "   MSR     PSP, R0              \n"
        "   BX      LR                   \n"
    );
}

// CPU使用率を取得（パーセント）
// 直前にこの関数を呼んだ時点からの差分で計算する。
// ps のようにタスクごとにループで呼ぶため、基準時刻もタスクごとに持つ。
int os_get_cpu_usage(int task_id)
{
    if (task_id < 0 || task_id >= g_task_count) {
        return -1;
    }

    static uint32_t last_total[MAX_TASKS] = {0};
    static uint32_t last_task_ticks[MAX_TASKS] = {0};

    uint32_t total = g_system_ticks;
    uint32_t delta_total = total - last_total[task_id];

    if (delta_total == 0) {
        return 0;
    }

    uint32_t delta_task = g_tasks[task_id].cpu_ticks - last_task_ticks[task_id];

    last_total[task_id] = total;
    last_task_ticks[task_id] = g_tasks[task_id].cpu_ticks;

    return (delta_task * 100) / delta_total;
}

src/os_svc.cpp — 95 lines

Open on GitHub

// os_svc.cpp
//
// 本書 応用編 第2章 2.5「SVC_Handler の実装」。
//
// SVC 番号は命令そのものに埋め込まれているので、ハンドラからは
// 「スタックに積まれた戻り先 PC の 2 バイト手前」を読んで取り出す。

#include <Arduino.h>
#include "os_syscall.h"
#include "os_kernel.h"

// ---- カーネル側のシステムコール実体 ----
//
// 本文では svc_dispatch_c から呼ぶ形だけを示しているので、中身はここで書く。
// いずれも「ユーザーが直接触れないもの」をカーネルが代行する形になっている。

void sys_putchar(char c)
{
    Serial.write(c);
}

void sys_yield(void)
{
    os_yield();
}

void sys_sleep(uint32_t ms)
{
    os_sleep(ms);
}

void sys_exit(int code)
{
    (void)code;
    os_terminate_task(os_get_current_task());
}

int sys_getpid(void)
{
    return os_get_current_task();
}

// ---- ディスパッチャ ----

// 引数 sp は SVC を発行したコードのスタックフレーム先頭を指す
extern "C" void svc_dispatch_c(uint32_t *sp)
{
    // sp[0]=R0, sp[1]=R1, sp[2]=R2, sp[3]=R3
    // sp[6]=元の PC (SVC の次の命令)
    uint8_t  *pc      = (uint8_t *)sp[6];
    uint8_t   svc_num = pc[-2];          // svc 命令の下位バイト = 番号

    uint32_t arg0 = sp[0];
    uint32_t ret  = 0;

    switch (svc_num) {
    case SYS_putchar:
        sys_putchar((char)arg0);
        break;
    case SYS_yield:
        sys_yield();
        break;
    case SYS_sleep:
        sys_sleep(arg0);  // ms 単位
        break;
    case SYS_exit:
        sys_exit((int)arg0);
        break;
    case SYS_getpid:
        ret = (uint32_t)sys_getpid();
        break;
    default:
        ret = (uint32_t)-1;  // ENOSYS 相当
        break;
    }

    // 戻り値は R0 (sp[0]) に書き戻す
    sp[0] = ret;
}

// ---- 例外ハンドラ本体 ----
//
// SVC が特権スレッドから来たか非特権スレッドから来たかで、引数が積まれている
// スタックが MSP か PSP かが変わる。EXC_RETURN の bit2 で見分ける。

extern "C" __attribute__((naked)) void SVC_Handler(void)
{
    __asm volatile (
        "TST   lr, #4            \n"  // EXC_RETURN bit2: 0=MSP, 1=PSP
        "ITE   EQ                \n"
        "MRSEQ r0, MSP           \n"  // bit2==0 なら MSP
        "MRSNE r0, PSP           \n"  // bit2==1 なら PSP
        "B     svc_dispatch_c    \n"  // R0 = スタックポインタ
    );
}

On the board#

../_images/adv2_svc.gif — `[A]` and `[B]` keep alternating, each character going out through `svc` (Supervisor Call, the instruction user code uses to call into the OS). The `[probe]` lines at the start scrolled by before the monitor opened.#

Pitfalls#

(Collected from reader questions — ask via GitHub Issues.)