手元にあったラズパイ4で、Yocto組込みLinuxのリアルタイム性能を測ってみた

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <time.h>
#include <sys/timerfd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sched.h>
#include <errno.h>
#include <stdint.h>
#include <signal.h>
#include <inttypes.h>
#include <math.h>
 
#define GPIO_NUM "530"   // GPIO18 (ピン12) 例 [512(BASE) + 18 = 530]
#define PERIOD_MS 1     // 1ms周期タイマ
 
static volatile sig_atomic_t g_stop = 0;
 
static void handle_stop_signal(int sig)
{
    (void)sig;
    g_stop = 1;
}
 
// トグル間隔の実測値(clock_gettime差分)を期待周期(1ms)と比べたずれを
// 1本の連続した尺度として扱い、us単位/ms単位のバケットに分けて集計する。
// (1ms以上の遅れがus側とms側の両方で二重にカウントされないよう、
//  バケットは重複なく1つの数直線として並べている)
typedef struct {
    uint64_t total;
    uint64_t errored;
    uint64_t max_expirations;
 
    uint64_t count;
    double sum_ns;
    double sum_sq_ns;
    int64_t min_ns;
    int64_t max_ns;
 
    uint64_t lt_neg100us;   // jitter < -100us
    uint64_t lt_neg10us;    // -100us  <= jitter < -10us
    uint64_t lt_10us;       //  -10us  <= jitter <  10us
    uint64_t lt_20us;       //   10us  <= jitter <  20us
    uint64_t lt_30us;       //   20us  <= jitter <  30us
    uint64_t lt_50us;       //   30us  <= jitter <  50us
    uint64_t lt_100us;      //   50us  <= jitter < 100us
    uint64_t lt_200us;      //  100us  <= jitter < 200us
    uint64_t lt_500us;      //  200us  <= jitter < 500us
    uint64_t lt_1ms;        //  500us  <= jitter <   1ms
    uint64_t lt_10ms;       //    1ms  <= jitter <  10ms
    uint64_t lt_100ms;      //   10ms  <= jitter < 100ms
    uint64_t lt_200ms;      //  100ms  <= jitter < 200ms
    uint64_t lt_500ms;      //  200ms  <= jitter < 500ms
    uint64_t lt_1000ms;     //  500ms  <= jitter < 1000ms
    uint64_t ge_1000ms;     // jitter >= 1000ms
} JitterStat;
 
// write()自体の所要時間(最短/平均/最長)をざっくり見るための集計
typedef struct {
    uint64_t count;
    double sum_ns;
    int64_t min_ns;
    int64_t max_ns;
} WriteTimeStat;
 
static void write_time_stat_add(WriteTimeStat *ws, int64_t elapsed_ns)
{
    ws->count++;
    ws->sum_ns += (double)elapsed_ns;
    if (elapsed_ns < ws->min_ns) ws->min_ns = elapsed_ns;
    if (elapsed_ns > ws->max_ns) ws->max_ns = elapsed_ns;
}
 
static uint64_t ts_to_ns(const struct timespec *ts)
{
    return (uint64_t)ts->tv_sec * 1000000000ULL + (uint64_t)ts->tv_nsec;
}
 
static void jitter_stat_add(JitterStat *js, int64_t jitter_ns)
{
    js->count++;
    js->sum_ns += (double)jitter_ns;
    js->sum_sq_ns += (double)jitter_ns * (double)jitter_ns;
    if (jitter_ns < js->min_ns) js->min_ns = jitter_ns;
    if (jitter_ns > js->max_ns) js->max_ns = jitter_ns;
 
    if      (jitter_ns < -100000)    js->lt_neg100us++;
    else if (jitter_ns < -10000)     js->lt_neg10us++;
    else if (jitter_ns < 10000)      js->lt_10us++;
    else if (jitter_ns < 20000)      js->lt_20us++;
    else if (jitter_ns < 30000)      js->lt_30us++;
    else if (jitter_ns < 50000)      js->lt_50us++;
    else if (jitter_ns < 100000)     js->lt_100us++;
    else if (jitter_ns < 200000)     js->lt_200us++;
    else if (jitter_ns < 500000)     js->lt_500us++;
    else if (jitter_ns < 1000000)    js->lt_1ms++;
    else if (jitter_ns < 10000000)   js->lt_10ms++;
    else if (jitter_ns < 100000000)  js->lt_100ms++;
    else if (jitter_ns < 200000000)  js->lt_200ms++;
    else if (jitter_ns < 500000000)  js->lt_500ms++;
    else if (jitter_ns < 1000000000) js->lt_1000ms++;
    else                              js->ge_1000ms++;
}
 
static void print_jitter_stats(const JitterStat *js)
{
    printf("\\n--- timerfd stats (total=%" PRIu64 ", errored=%" PRIu64 ", max_expirations=%" PRIu64 ") ---\\n",
           js->total, js->errored, js->max_expirations);
 
    if (js->count == 0) return;
    double mean = js->sum_ns / (double)js->count;
    double variance = js->sum_sq_ns / (double)js->count - mean * mean;
    if (variance < 0) variance = 0; // 丸め誤差対策
    double stddev = sqrt(variance);
 
    printf("\\n--- jitter stats (samples=%" PRIu64 ") ---\\n", js->count);
    printf("mean                    : %8.2f us\\n", mean / 1000.0);
    printf("min                     : %8.2f us\\n", (double)js->min_ns / 1000.0);
    printf("max                     : %8.2f us\\n", (double)js->max_ns / 1000.0);
    printf("stddev                  : %8.2f us\\n", stddev / 1000.0);
    printf("jitter <  -100us        : %" PRIu64 "\\n", js->lt_neg100us);
    printf("-100us <= jitter < -10us: %" PRIu64 "\\n", js->lt_neg10us);
    printf(" -10us <= jitter <  10us: %" PRIu64 "\\n", js->lt_10us);
    printf("  10us <= jitter <  20us: %" PRIu64 "\\n", js->lt_20us);
    printf("  20us <= jitter <  30us: %" PRIu64 "\\n", js->lt_30us);
    printf("  30us <= jitter <  50us: %" PRIu64 "\\n", js->lt_50us);
    printf("  50us <= jitter < 100us: %" PRIu64 "\\n", js->lt_100us);
    printf(" 100us <= jitter < 200us: %" PRIu64 "\\n", js->lt_200us);
    printf(" 200us <= jitter < 500us: %" PRIu64 "\\n", js->lt_500us);
    printf(" 500us <= jitter <   1ms: %" PRIu64 "\\n", js->lt_1ms);
    printf("   1ms <= jitter <  10ms: %" PRIu64 "\\n", js->lt_10ms);
    printf("  10ms <= jitter < 100ms: %" PRIu64 "\\n", js->lt_100ms);
    printf(" 100ms <= jitter < 200ms: %" PRIu64 "\\n", js->lt_200ms);
    printf(" 200ms <= jitter < 500ms: %" PRIu64 "\\n", js->lt_500ms);
    printf(" 500ms <= jitter <1000ms: %" PRIu64 "\\n", js->lt_1000ms);
    printf(" jitter >= 1000ms       : %" PRIu64 "\\n", js->ge_1000ms);
}
 
static void print_write_time_stats(const WriteTimeStat *ws)
{
    printf("\\n--- gpio write() time (samples=%" PRIu64 ") ---\\n", ws->count);
    if (ws->count == 0) return;
    double wmean = ws->sum_ns / (double)ws->count;
    printf("min                     : %8.2f us\\n", (double)ws->min_ns / 1000.0);
    printf("mean                    : %8.2f us\\n", wmean / 1000.0);
    printf("max                     : %8.2f us\\n", (double)ws->max_ns / 1000.0);
}
 
static int gpio_export(const char *num)
{
    int fd = open("/sys/class/gpio/export", O_WRONLY);
    if (fd < 0) return -1;
    if (write(fd, num, strlen(num)) < 0) { close(fd); return -1; }
    close(fd);
    return 0;
}
 
static int gpio_set_dir(const char *num, const char *dir)
{
    char path[64];
    snprintf(path, sizeof(path), "/sys/class/gpio/gpio%s/direction", num);
    int fd = open(path, O_WRONLY);
    if (fd < 0) return -1;
    if (write(fd, dir, strlen(dir)) < 0) { close(fd); return -1; }
    close(fd);
    return 0;
}
 
static int gpio_open_value(const char *num)
{
    char path[64];
    snprintf(path, sizeof(path), "/sys/class/gpio/gpio%s/value", num);
    return open(path, O_WRONLY);
}
 
int main(void)
{
    struct sched_param sp;
    int tfd, gfd;
    struct itimerspec its;
    uint64_t expirations;
    int level = 0;
    JitterStat js;
    WriteTimeStat ws;
    uint64_t prev_ns = 0;
    int have_prev = 0;
 
    memset(&js, 0, sizeof(js));
    js.min_ns = INT64_MAX;
    js.max_ns = INT64_MIN;
    memset(&ws, 0, sizeof(ws));
    ws.min_ns = INT64_MAX;
    ws.max_ns = INT64_MIN;
 
    // Ctrl-C/SIGTERMで統計を表示してから終了できるようにする
    struct sigaction sa;
    memset(&sa, 0, sizeof(sa));
    sa.sa_handler = handle_stop_signal;
    sigaction(SIGINT, &sa, NULL);
    sigaction(SIGTERM, &sa, NULL);
 
    // SCHED_FIFO に変更
    memset(&sp, 0, sizeof(sp));
    sp.sched_priority = 80; // root前提
    if (sched_setscheduler(0, SCHED_FIFO, &sp) < 0) {
        perror("sched_setscheduler");
        return 1;
    }
 
    // GPIO準備
    gpio_export(GPIO_NUM);
    usleep(100000); // ちょっと待つ
    if (gpio_set_dir(GPIO_NUM, "out") < 0) {
        perror("gpio_set_dir");
        return 1;
    }
    gfd = gpio_open_value(GPIO_NUM);
    if (gfd < 0) {
        perror("gpio_open_value");
        return 1;
    }
 
    // timerfd 1ms周期
    tfd = timerfd_create(CLOCK_MONOTONIC, 0);
    if (tfd < 0) {
        perror("timerfd_create");
        return 1;
    }
 
    its.it_interval.tv_sec = 0;
    its.it_interval.tv_nsec = PERIOD_MS * 1000000;
    its.it_value.tv_sec = 0;
    its.it_value.tv_nsec = PERIOD_MS * 1000000;
 
    if (timerfd_settime(tfd, 0, &its, NULL) < 0) {	// 相対タイマーの場合
        perror("timerfd_settime");
        return 1;
    }
 
    printf("Running 1ms timer, toggling GPIO %s (2ms period square wave)\\n", GPIO_NUM);
 
    while (!g_stop) {
        ssize_t r = read(tfd, &expirations, sizeof(expirations));
        if (r != sizeof(expirations)) {
            js.total++;
            js.errored++;
            if (errno == EINTR) break;
            perror("read timerfd");
            break;
        }
        js.total++;
        if (expirations > js.max_expirations) js.max_expirations = expirations;
 
        // 1msごとにトグル → 2ms周期矩形波
        level = !level;
        const char *val = level ? "1" : "0";
 
        struct timespec wt0, wt1;
        clock_gettime(CLOCK_MONOTONIC, &wt0);
        ssize_t wr = write(gfd, val, 1);
        clock_gettime(CLOCK_MONOTONIC, &wt1);
        if (wr < 0) {
            perror("write gpio");
            break;
        }
        write_time_stat_add(&ws, (int64_t)(ts_to_ns(&wt1) - ts_to_ns(&wt0)));
 
        // write()完了直後の時刻を「実際にGPIOが変化したとみなせる時刻」として
        // 前回との差分(期待周期1msからのずれ)をusec単位のジッタとして記録する
        uint64_t now_ns = ts_to_ns(&wt1);
        if (have_prev) {
            int64_t delta_ns = (int64_t)(now_ns - prev_ns);
            int64_t jitter_ns = delta_ns - (int64_t)PERIOD_MS * 1000000;
            jitter_stat_add(&js, jitter_ns);
        } else {
            have_prev = 1;
        }
        prev_ns = now_ns;
    }
 
    close(gfd);
    close(tfd);
 
    print_jitter_stats(&js);
    print_write_time_stats(&ws);
 
    return 0;
}

#include <Arduino.h>
#include <STM32FreeRTOS.h>
#include <math.h>
 
// ---------------------------------------------------------------------------
// DWT->CYCCNT による高精度サイクルカウンタ
// (RPi4側の clock_gettime(CLOCK_MONOTONIC) に相当する時刻ソース)
// ---------------------------------------------------------------------------
static double g_nsPerCycle = 0.0;
 
static void dwtInit()
{
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    DWT->CYCCNT = 0;
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
    g_nsPerCycle = 1.0e9 / (double)SystemCoreClock;
}
 
static inline uint32_t dwtNow()
{
    return DWT->CYCCNT;
}
 
// CYCCNTは32bitでオーバーフローするが、差分を符号なし32bit演算のまま取れば
// ラップアラウンドしても正しい経過サイクル数になる。
static inline double cyclesToNs(uint32_t cycles)
{
    return (double)cycles * g_nsPerCycle;
}
 
// ---------------------------------------------------------------------------
// ジッタ統計(RPi4側 rt-gpio-square と同じ、1本の数直線上のバケット定義)
// ---------------------------------------------------------------------------
struct JitterStat
{
    uint32_t count = 0;
    double sum_ns = 0;
    double sum_sq_ns = 0;
    double min_ns = 1e18;
    double max_ns = -1e18;
 
    uint32_t lt_neg100us = 0; // jitter < -100us
    uint32_t lt_neg10us = 0;  // -100us <= jitter < -10us
    uint32_t lt_10us = 0;     //  -10us <= jitter <  10us
    uint32_t lt_20us = 0;     //   10us <= jitter <  20us
    uint32_t lt_30us = 0;     //   20us <= jitter <  30us
    uint32_t lt_50us = 0;     //   30us <= jitter <  50us
    uint32_t lt_100us = 0;    //   50us <= jitter < 100us
    uint32_t lt_200us = 0;    //  100us <= jitter < 200us
    uint32_t lt_500us = 0;    //  200us <= jitter < 500us
    uint32_t lt_1ms = 0;      //  500us <= jitter <   1ms
    uint32_t lt_10ms = 0;     //    1ms <= jitter <  10ms
    uint32_t lt_100ms = 0;    //   10ms <= jitter < 100ms
    uint32_t lt_200ms = 0;    //  100ms <= jitter < 200ms
    uint32_t lt_500ms = 0;    //  200ms <= jitter < 500ms
    uint32_t lt_1000ms = 0;   //  500ms <= jitter < 1000ms
    uint32_t ge_1000ms = 0;   // jitter >= 1000ms
};
 
// GPIO操作(digitalWrite)自体の所要時間統計(RPi4側の write() 計測に相当)
struct OpTimeStat
{
    uint32_t count = 0;
    double sum_ns = 0;
    double min_ns = 1e18;
    double max_ns = -1e18;
};
 
static void jitterStatAdd(JitterStat &js, double jitter_ns)
{
    js.count++;
    js.sum_ns += jitter_ns;
    js.sum_sq_ns += jitter_ns * jitter_ns;
    if (jitter_ns < js.min_ns) js.min_ns = jitter_ns;
    if (jitter_ns > js.max_ns) js.max_ns = jitter_ns;
 
    if      (jitter_ns < -100000.0)    js.lt_neg100us++;
    else if (jitter_ns < -10000.0)     js.lt_neg10us++;
    else if (jitter_ns < 10000.0)      js.lt_10us++;
    else if (jitter_ns < 20000.0)      js.lt_20us++;
    else if (jitter_ns < 30000.0)      js.lt_30us++;
    else if (jitter_ns < 50000.0)      js.lt_50us++;
    else if (jitter_ns < 100000.0)     js.lt_100us++;
    else if (jitter_ns < 200000.0)     js.lt_200us++;
    else if (jitter_ns < 500000.0)     js.lt_500us++;
    else if (jitter_ns < 1000000.0)    js.lt_1ms++;
    else if (jitter_ns < 10000000.0)   js.lt_10ms++;
    else if (jitter_ns < 100000000.0)  js.lt_100ms++;
    else if (jitter_ns < 200000000.0)  js.lt_200ms++;
    else if (jitter_ns < 500000000.0)  js.lt_500ms++;
    else if (jitter_ns < 1000000000.0) js.lt_1000ms++;
    else                                js.ge_1000ms++;
}
 
static void opTimeStatAdd(OpTimeStat &ws, double elapsed_ns)
{
    ws.count++;
    ws.sum_ns += elapsed_ns;
    if (elapsed_ns < ws.min_ns) ws.min_ns = elapsed_ns;
    if (elapsed_ns > ws.max_ns) ws.max_ns = elapsed_ns;
}
 
static void printJitterStats(const JitterStat &js)
{
    Serial.println();
    Serial.print(F("--- jitter stats (samples="));
    Serial.print(js.count);
    Serial.println(F(") ---"));
    if (js.count == 0) return;
 
    double mean = js.sum_ns / (double)js.count;
    double variance = js.sum_sq_ns / (double)js.count - mean * mean;
    if (variance < 0) variance = 0; // 丸め誤差対策
    double stddev = sqrt(variance);
 
    Serial.print(F("mean                    : ")); Serial.print(mean / 1000.0, 2); Serial.println(F(" us"));
    Serial.print(F("min                     : ")); Serial.print(js.min_ns / 1000.0, 2); Serial.println(F(" us"));
    Serial.print(F("max                     : ")); Serial.print(js.max_ns / 1000.0, 2); Serial.println(F(" us"));
    Serial.print(F("stddev                  : ")); Serial.print(stddev / 1000.0, 2); Serial.println(F(" us"));
    Serial.print(F("jitter <  -100us        : ")); Serial.println(js.lt_neg100us);
    Serial.print(F("-100us <= jitter < -10us: ")); Serial.println(js.lt_neg10us);
    Serial.print(F(" -10us <= jitter <  10us: ")); Serial.println(js.lt_10us);
    Serial.print(F("  10us <= jitter <  20us: ")); Serial.println(js.lt_20us);
    Serial.print(F("  20us <= jitter <  30us: ")); Serial.println(js.lt_30us);
    Serial.print(F("  30us <= jitter <  50us: ")); Serial.println(js.lt_50us);
    Serial.print(F("  50us <= jitter < 100us: ")); Serial.println(js.lt_100us);
    Serial.print(F(" 100us <= jitter < 200us: ")); Serial.println(js.lt_200us);
    Serial.print(F(" 200us <= jitter < 500us: ")); Serial.println(js.lt_500us);
    Serial.print(F(" 500us <= jitter <   1ms: ")); Serial.println(js.lt_1ms);
    Serial.print(F("   1ms <= jitter <  10ms: ")); Serial.println(js.lt_10ms);
    Serial.print(F("  10ms <= jitter < 100ms: ")); Serial.println(js.lt_100ms);
    Serial.print(F(" 100ms <= jitter < 200ms: ")); Serial.println(js.lt_200ms);
    Serial.print(F(" 200ms <= jitter < 500ms: ")); Serial.println(js.lt_500ms);
    Serial.print(F(" 500ms <= jitter <1000ms: ")); Serial.println(js.lt_1000ms);
    Serial.print(F(" jitter >= 1000ms       : ")); Serial.println(js.ge_1000ms);
}
 
static void printOpTimeStats(const OpTimeStat &ws)
{
    Serial.println();
    Serial.print(F("--- gpio toggle time (samples="));
    Serial.print(ws.count);
    Serial.println(F(") ---"));
    if (ws.count == 0) return;
 
    double mean = ws.sum_ns / (double)ws.count;
    Serial.print(F("min                     : ")); Serial.print(ws.min_ns / 1000.0, 2); Serial.println(F(" us"));
    Serial.print(F("mean                    : ")); Serial.print(mean / 1000.0, 2); Serial.println(F(" us"));
    Serial.print(F("max                     : ")); Serial.print(ws.max_ns / 1000.0, 2); Serial.println(F(" us"));
}
 
// ---------------------------------------------------------------------------
// タスク
// ---------------------------------------------------------------------------
static JitterStat g_jitter;
static OpTimeStat g_opTime;
static TaskHandle_t g_toggleTaskHandle = NULL;
 
constexpr double kPeriodNs = 1000000.0; // 1ms
constexpr uint32_t kDebounceMs = 20;
 
void toggleTask(void *pvParameters)
{
    pinMode(LED_BUILTIN, OUTPUT);
 
    bool havePrev = false;
    uint32_t prevCycles = 0;
 
    for (;;)
    {
        vTaskDelay(pdMS_TO_TICKS(1)); // 1msごとにトグル → 2ms周期矩形波
 
        uint32_t t0 = dwtNow();
        digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
        uint32_t t1 = dwtNow();
 
        double opNs = cyclesToNs(t1 - t0);
 
        // digitalWrite完了直後の時刻を「実際にGPIOが変化したとみなせる時刻」として、
        // 前回との差分(期待周期1msからのずれ)をジッタとして記録する(RPi4側と同じ考え方)。
        double jitterNs = 0;
        bool haveJitter = false;
        if (havePrev)
        {
            double deltaNs = cyclesToNs(t1 - prevCycles);
            jitterNs = deltaNs - kPeriodNs;
            haveJitter = true;
        }
 
        taskENTER_CRITICAL();
        opTimeStatAdd(g_opTime, opNs);
        if (haveJitter) jitterStatAdd(g_jitter, jitterNs);
        taskEXIT_CRITICAL();
 
        prevCycles = t1;
        havePrev = true;
    }
}
 
// USER_BTN(Nucleoボードの青いボタン、B1/PC13、押下でLOW)が押されるまで計測を続け、
// 押されたらtoggleTaskを止めてそこまでの統計を表示する(RPi4側のCtrl-Cに相当)。
void buttonTask(void *pvParameters)
{
    pinMode(USER_BTN, INPUT);
 
    // 10ms周期でサンプリングし、3回連続で同じ論理値が読めたらその値を確定値とする
    // (確定値がLOW=押下になったら計測終了)。
    constexpr uint8_t kStableSamples = 3;
    int candidateLevel = digitalRead(USER_BTN);
    uint8_t matchCount = 1;
 
    for (;;)
    {
        vTaskDelay(pdMS_TO_TICKS(kDebounceMs));
        int level = digitalRead(USER_BTN);
 
        if (level == candidateLevel)
        {
            matchCount++;
        }
        else
        {
            candidateLevel = level;
            matchCount = 1;
        }
 
        if (matchCount >= kStableSamples && candidateLevel == LOW) break;
    }
 
    vTaskSuspend(g_toggleTaskHandle); // これ以降サンプルを追加させない
 
    JitterStat jitterCopy;
    OpTimeStat opTimeCopy;
    taskENTER_CRITICAL();
    jitterCopy = g_jitter;
    opTimeCopy = g_opTime;
    taskEXIT_CRITICAL();
 
    Serial.println();
    Serial.println(F("--- button pressed, measurement stopped ---"));
    printJitterStats(jitterCopy);
    printOpTimeStats(opTimeCopy);
 
    vTaskSuspend(NULL); // ボタンタスク自身もここで停止(リセットするまで待ち)
}
 
void setup()
{
    Serial.begin(115200);
    dwtInit();
 
    Serial.println(F("Running 1ms FreeRTOS task, toggling LED (2ms period square wave)"));
    Serial.println(F("Press the user button (B1) to stop measurement and print stats."));
 
    xTaskCreate(toggleTask, "toggle", 256, NULL, 2, &g_toggleTaskHandle);
    xTaskCreate(buttonTask, "button", 256, NULL, 1, NULL);
    vTaskStartScheduler();
}
 
void loop() {}