Deep Dive: How Klipper 1.0 Implements Step Pulse Generation with Raspberry Pi 5 Real-Time OS

/**
 * Klipper 1.0 RPi 5 RTOS Shared Memory Queue Implementation
 * SPDX-License-Identifier: GPL-3.0-only
 * 
 * Manages lock-free single-producer single-consumer (SPSC) queue between
 * host userspace (producer) and RT core (consumer) for step pulse commands.
 * Uses cache-aligned memory regions to avoid false sharing between cores.
 */
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "shmem.h"

#define SHMEM_SIZE 4096  // 4KB shared memory region, cache-aligned to RPi 5 L2 cache line (64B)
#define QUEUE_DEPTH 256   // Maximum number of pending step commands
#define CACHE_LINE_SIZE 64

// Step command structure, aligned to cache line to prevent false sharing
struct __attribute__((aligned(CACHE_LINE_SIZE))) step_cmd {
    uint32_t step_pin;    // GPIO pin number for step pulse
    uint32_t dir_pin;     // GPIO pin number for direction
    uint32_t pulse_us;    // Pulse width in microseconds
    uint32_t interval_us; // Interval between pulses in microseconds
    uint64_t timestamp_ns;// Host timestamp for jitter measurement
};

// SPSC queue structure, aligned to cache line
struct __attribute__((aligned(CACHE_LINE_SIZE))) spsc_queue {
    volatile uint32_t head; // Producer index (host writes, RT reads)
    volatile uint32_t tail; // Consumer index (RT writes, host reads)
    struct step_cmd cmds[QUEUE_DEPTH];
};

// Shared memory region structure
struct shmem_region {
    struct spsc_queue queue;
    volatile uint32_t watchdog_counter; // Incremented by RT core every 10ms
    volatile uint8_t rt_core_online;    // Set to 1 when RT core initializes
};

int shmem_init(struct shmem_region **region, int is_producer) {
    int fd = open("/dev/klipper-shmem", O_RDWR | O_CREAT, 0666);
    if (fd < 0) {
        fprintf(stderr, "shmem_init: failed to open /dev/klipper-shmem: %s\n", strerror(errno));
        return -1;
    }

    // Truncate to fixed size
    if (ftruncate(fd, SHMEM_SIZE) < 0) {
        fprintf(stderr, "shmem_init: ftruncate failed: %s\n", strerror(errno));
        close(fd);
        return -1;
    }

    // Map shared memory with cache disabling for RT core access (via mmap flags)
    int mmap_flags = MAP_SHARED;
    if (is_producer) {
        mmap_flags |= MAP_LOCKED; // Lock pages in RAM to prevent swap for producer
    }
    *region = mmap(NULL, SHMEM_SIZE, PROT_READ | PROT_WRITE, mmap_flags, fd, 0);
    if (*region == MAP_FAILED) {
        fprintf(stderr, "shmem_init: mmap failed: %s\n", strerror(errno));
        close(fd);
        return -1;
    }

    // Initialize region only if producer (host) and region is uninitialized
    if (is_producer && (*region)->queue.head == 0 && (*region)->queue.tail == 0) {
        memset(*region, 0, SHMEM_SIZE);
        __sync_synchronize(); // Full memory barrier to ensure init is visible to RT core
    }

    close(fd);
    return 0;
}

int shmem_enqueue(struct shmem_region *region, struct step_cmd *cmd) {
    struct spsc_queue *q = ®ion->queue;
    uint32_t head = q->head;
    uint32_t next_head = (head + 1) % QUEUE_DEPTH;

    // Check if queue is full
    if (next_head == q->tail) {
        fprintf(stderr, "shmem_enqueue: queue full, dropping step command\n");
        return -1;
    }

    // Copy command to queue with memory barrier to ensure order
    memcpy(&q->cmds[head], cmd, sizeof(struct step_cmd));
    __sync_synchronize();
    q->head = next_head;
    return 0;
}

int shmem_dequeue(struct shmem_region *region, struct step_cmd *cmd) {
    struct spsc_queue *q = ®ion->queue;
    uint32_t tail = q->tail;

    // Check if queue is empty
    if (tail == q->head) {
        return -1; // No commands available
    }

    // Copy command from queue with memory barrier
    memcpy(cmd, &q->cmds[tail], sizeof(struct step_cmd));
    __sync_synchronize();
    q->tail = (tail + 1) % QUEUE_DEPTH;
    return 0;
}

/**
 * Klipper 1.0 RPi 5 RTOS Step Pulse Generator
 * Runs on isolated Core 3 with PREEMPT_RT kernel, handles GPIO toggling
 * with nanosecond-precision timing via clock_gettime(CLOCK_MONOTONIC_RAW)
 */
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "shmem.h"
#include "gpio.h"

#define RT_CORE_ID 3         // Isolated core for RT step generation
#define RT_PRIORITY 99       // Max real-time priority (SCHED_FIFO)
#define WATCHDOG_TIMEOUT_MS 100 // Reset if RT core stops updating watchdog
#define GPIO_BASE 0xFE200000 // RPi 5 GPIO controller MMIO base address

// GPIO controller registers (from RPi 5 datasheet)
struct gpio_regs {
    volatile uint32_t gpfsel[6];  // Function select registers
    volatile uint32_t gpset[2];   // Pin set registers
    volatile uint32_t gpclr[2];   // Pin clear registers
    volatile uint32_t gplev[2];   // Pin level registers
    // ... other registers omitted for brevity
};

static struct gpio_regs *gpio_map = NULL;
static struct shmem_region *shmem = NULL;
static volatile int rt_running = 1;

// Set GPIO pin to output mode
static int gpio_set_output(uint32_t pin) {
    if (pin > 53) { // RPi 5 has 54 GPIO pins
        fprintf(stderr, "gpio_set_output: invalid pin %u\n", pin);
        return -1;
    }
    uint32_t reg_idx = pin / 10;
    uint32_t bit_idx = (pin % 10) * 3;
    uint32_t mask = ~(0x7 << bit_idx);
    uint32_t val = 0x1 << bit_idx; // Output mode is 001
    gpio_map->gpfsel[reg_idx] = (gpio_map->gpfsel[reg_idx] & mask) | val;
    __sync_synchronize();
    return 0;
}

// Toggle step pin with precise timing
static void step_toggle(uint32_t step_pin, uint32_t pulse_us) {
    struct timespec start, now;
    uint64_t elapsed_ns = 0;

    // Set step pin high
    gpio_map->gpset[step_pin / 32] = 1 << (step_pin % 32);
    __sync_synchronize();

    // Busy-wait for pulse width (microsecond precision, busy-wait avoids scheduler latency)
    clock_gettime(CLOCK_MONOTONIC_RAW, &start);
    while (elapsed_ns < (pulse_us * 1000)) {
        clock_gettime(CLOCK_MONOTONIC_RAW, &now);
        elapsed_ns = (now.tv_sec - start.tv_sec) * 1e9 + (now.tv_nsec - start.tv_nsec);
    }

    // Set step pin low
    gpio_map->gpclr[step_pin / 32] = 1 << (step_pin % 32);
    __sync_synchronize();
}

// RT core main loop
static void *rt_step_loop(void *arg) {
    struct step_cmd cmd;
    struct timespec next_step;
    uint64_t jitter_ns = 0;

    // Set thread to real-time scheduling policy
    struct sched_param param = { .sched_priority = RT_PRIORITY };
    if (sched_setscheduler(0, SCHED_FIFO, ¶m) < 0) {
        fprintf(stderr, "rt_step_loop: failed to set SCHED_FIFO: %s\n", strerror(errno));
        return NULL;
    }

    // Pin thread to isolated core
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(RT_CORE_ID, &cpuset);
    if (pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset) < 0) {
        fprintf(stderr, "rt_step_loop: failed to pin to core %d: %s\n", RT_CORE_ID, strerror(errno));
        return NULL;
    }

    // Map GPIO MMIO region
    int fd = open("/dev/mem", O_RDWR | O_SYNC);
    if (fd < 0) {
        fprintf(stderr, "rt_step_loop: failed to open /dev/mem: %s\n", strerror(errno));
        return NULL;
    }
    gpio_map = mmap(NULL, sizeof(struct gpio_regs), PROT_READ | PROT_WRITE, MAP_SHARED, fd, GPIO_BASE);
    if (gpio_map == MAP_FAILED) {
        fprintf(stderr, "rt_step_loop: mmap GPIO failed: %s\n", strerror(errno));
        close(fd);
        return NULL;
    }
    close(fd);

    // Initialize shared memory as consumer (RT core)
    if (shmem_init(&shmem, 0) < 0) {
        fprintf(stderr, "rt_step_loop: shmem_init failed\n");
        return NULL;
    }
    shmem->rt_core_online = 1;
    __sync_synchronize();

    // Main loop: dequeue commands and generate pulses
    while (rt_running) {
        // Update watchdog counter every 10ms
        static uint64_t last_watchdog_ns = 0;
        struct timespec now;
        clock_gettime(CLOCK_MONOTONIC_RAW, &now);
        uint64_t now_ns = now.tv_sec * 1e9 + now.tv_nsec;
        if (now_ns - last_watchdog_ns >= 10e6) { // 10ms
            shmem->watchdog_counter++;
            last_watchdog_ns = now_ns;
        }

        // Dequeue step command
        if (shmem_dequeue(shmem, &cmd) == 0) {
            // Set direction pin if needed (omitted for brevity, would check dir_pin state)
            // Generate step pulse
            step_toggle(cmd.step_pin, cmd.pulse_us);

            // Calculate jitter: difference between expected and actual interval
            if (next_step.tv_sec != 0) {
                clock_gettime(CLOCK_MONOTONIC_RAW, &now);
                int64_t expected_ns = cmd.interval_us * 1000;
                int64_t actual_ns = (now.tv_sec - next_step.tv_sec) * 1e9 + (now.tv_nsec - next_step.tv_nsec);
                jitter_ns = llabs(actual_ns - expected_ns);
                // Log jitter if over threshold (1μs)
                if (jitter_ns > 1000) {
                    fprintf(stderr, "step jitter: %ld ns (cmd interval %u us)\n", jitter_ns, cmd.interval_us);
                }
            }
            clock_gettime(CLOCK_MONOTONIC_RAW, &next_step);
        } else {
            // No commands, busy-wait 1μs to avoid spinning too hard
            struct timespec sleep = {0, 1000};
            clock_nanosleep(CLOCK_MONOTONIC_RAW, 0, &sleep, NULL);
        }
    }

    shmem->rt_core_online = 0;
    munmap(gpio_map, sizeof(struct gpio_regs));
    return NULL;
}

"""Klipper 1.0 Host Layer Motion Planner Step Enqueue
Runs in userspace, parses G-code motion commands into step pulses,
enqueues to shared memory for RT core.
"""
import time
import numpy as np
from . import shmem
from . import stepper

# Stepper configuration for a typical NEMA17 with 16x microstepping
STEP_PER_MM = 80  # 1.8° stepper: 200 steps/rev * 16 microsteps / 40mm belt pitch = 80 steps/mm
MAX_ACCEL = 3000  # mm/s²
MAX_VELOCITY = 300  # mm/s
SHMEM_REGION = None

def init_motion_planner(shmem_region):
    """Initialize motion planner with shared memory region from RT core."""
    global SHMEM_REGION
    SHMEM_REGION = shmem_region
    if not SHMEM_REGION.rt_core_online:
        raise RuntimeError("RT core not online, cannot initialize motion planner")
    # Initialize stepper objects for all configured steppers
    stepper.init_steppers()
    print(f"Motion planner initialized, RT core online: {SHMEM_REGION.rt_core_online}")

def plan_linear_move(start_x, start_y, start_z, end_x, end_y, end_z, velocity, accel):
    """
    Plan a linear move between two points, generate step commands,
    enqueue to shared memory.
    Args:
        start_x/y/z: Start position in mm
        end_x/y/z: End position in mm
        velocity: Target velocity in mm/s (capped at MAX_VELOCITY)
        accel: Acceleration in mm/s² (capped at MAX_ACCEL)
    Returns:
        Number of step commands enqueued
    """
    if not SHMEM_REGION:
        raise RuntimeError("Motion planner not initialized")
    if not SHMEM_REGION.rt_core_online:
        raise RuntimeError("RT core went offline during move planning")

    # Cap velocity and acceleration to hardware limits
    velocity = min(velocity, MAX_VELOCITY)
    accel = min(accel, MAX_ACCEL)

    # Calculate move distance
    dx = end_x - start_x
    dy = end_y - start_y
    dz = end_z - start_z
    distance = np.sqrt(dx*dx + dy*dy + dz*dz)
    if distance < 0.001:  # No move needed
        return 0

    # Calculate motion profile: accelerate to target velocity, cruise, decelerate
    # Simplified trapezoidal velocity profile
    t_accel = velocity / accel
    d_accel = 0.5 * accel * t_accel * t_accel
    d_total_accel_decel = 2 * d_accel

    step_count = int(distance * STEP_PER_MM)
    if step_count == 0:
        return 0

    # Generate step intervals (simplified: constant interval for demo, real code uses velocity profile)
    # Interval between steps: 1e6 / (velocity * STEP_PER_MM) microseconds
    step_interval_us = int(1e6 / (velocity * STEP_PER_MM)) if velocity > 0 else 1e6
    step_pulse_us = 2  # 2μs pulse width, standard for most stepper drivers

    # Get current stepper pin mappings
    x_stepper = stepper.get_stepper("x")
    y_stepper = stepper.get_stepper("y")
    z_stepper = stepper.get_stepper("z")

    enqueued = 0
    for i in range(step_count):
        # Determine which axis to step (simplified: interleave X/Y/Z steps)
        axis = "x" if i % 3 == 0 else "y" if i % 3 == 1 else "z"
        curr_stepper = x_stepper if axis == "x" else y_stepper if axis == "y" else z_stepper

        # Create step command
        cmd = shmem.step_cmd(
            step_pin=curr_stepper.step_pin,
            dir_pin=curr_stepper.dir_pin,
            pulse_us=step_pulse_us,
            interval_us=step_interval_us,
            timestamp_ns=int(time.time_ns())
        )

        # Enqueue with retry on full queue (up to 3 retries)
        retry_count = 0
        while retry_count < 3:
            ret = shmem.shmem_enqueue(SHMEM_REGION, cmd)
            if ret == 0:
                enqueued += 1
                break
            else:
                # Queue full, wait 100μs and retry
                time.sleep(0.0001)
                retry_count += 1
                if retry_count == 3:
                    print(f"Failed to enqueue step command after 3 retries, dropping step {i}")
                    # Reduce velocity for next move to avoid queue overflow
                    velocity = max(velocity * 0.8, 10)
                    step_interval_us = int(1e6 / (velocity * STEP_PER_MM))

    return enqueued

def check_watchdog():
    """Check if RT core watchdog is still updating, reset if timeout."""
    if not SHMEM_REGION:
        return False
    last_counter = SHMEM_REGION.watchdog_counter
    time.sleep(0.15)  # Wait 150ms (more than WATCHDOG_TIMEOUT_MS)
    if SHMEM_REGION.watchdog_counter == last_counter:
        print("Watchdog timeout! RT core stopped responding.")
        return False
    return True

# Isolate Core 3, set Klipper RT thread to max priority
tuna --cpu=3 --isolate
tuna --thread=klipper-rt-stepgen --priority=99 --cpu=3

// MMIO write to set GPIO pin 17 high (from RT step generator)
gpio_map->gpset[17 / 32] = 1 << (17 % 32);
__sync_synchronize(); // Ensure write is visible to hardware

# Run cyclictest for 24 hours, measure latency on Core 3
cyclictest -c 3 -t 1 -p 99 -D 24h -m -q