code:
/*
 * Copyright (c) 2018 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 * This implementation is based on the paper "Extending RCU for Realtime
 * and Embedded Workloads" by Paul E. McKenney, Ingo Molnar, Dipankar Sarma,
 * and Suparna Bhattacharya. Beside the mechanisms not implemented yet,
 * such as priority boosting, the differences are described below.
 *
 * First, this implementation uses scalable reference counters provided
 * by the sref module instead of per-CPU counters as described in the paper.
 * The main benefit of this approach is the centralization of most scalability
 * improvements in the sref module, which should propagate to all sref users,
 * including RCU.
 *
 * In addition, this implementation introduces the concept of windows, where
 * a window is a range in time to which readers may be linked. Here, a
 * grace period is defined as the time range at the end of a window where
 * various synchronization steps are performed to enforce the RCU guarantees.
 * The minimum duration of a window acts as a knob allowing users to tune
 * the behavior of the RCU system.
 *
 * Finally, the state machine described in the paper is updated to accommodate
 * for windows, since grace periods don't run back-to-back to each other.
 * Windows are regularly checked and flipped if the previous one isn't
 * active any more. From that moment, processors may notice the global flip
 * and perform a local flip of their work window ID. Once all processors
 * have acknowleged the flip, it is certain that no new work may be queued
 * on the previous window. At this point, the same occurs for the
 * processor-local reader window ID, and once all processors have
 * acknowleged that flip, there can be no new reader linked to the previous
 * window. The RCU system then releases its own reference to the previous
 * window and waits for the window reference counter to drop to 0, indicating
 * that all readers linked to the previous window have left their read-side
 * critical section. When this global event occurs, processors are requested
 * to flush the works queued for the previous window, and once they all have
 * acknowleged their flush, the window ends and becomes inactive, allowing
 * a new grace period to occur later on.
 *
 * Here is an informal diagram describing this process :
 *
 * t ---->
 *
 *    reader window flip ---+     +--- no more readers
 * work window flip ------+ |     | +- works flushed
 * (grace period start)   | |     | |  (grace period / window end)
 *                        v v     v v
 *         +--------------+-+-----+-+
 *         |              . .     . |
 *         |  window 0    . . gp  . |
 *         |      removal . .     . | reclamation
 *         +--------------+-+-----+-+-----+----+
 *                        |               .    |
 *                        |  window 1     . gp |
 *                        |       removal .    | reclamation
 *                        +---------------+----+--------
 *                                        |
 *                                        |  window 2   ...
 *                                        |
 *                                        +-------------
 *
 * On each processor, work window flips are separate from reader window
 * flips in order to correctly handle situations such as this one, where
 * "wf" denotes a window flip for both works and readers :
 *
 * t ---->
 *
 * CPU0   wf load                           flush
 * CPU1            wf                       flush
 * global          no-new-reader ... no-ref       loaded value now invalid
 *
 * After its window flip, CPU0 may load data from the previous window with
 * a reader linked to the current window, because it doesn't know that there
 * may still be new works queued on the previous window.
 *
 * TODO Improve atomic acknowledgment scalability.
 * TODO Handle large amounts of deferred works.
 * TODO Priority boosting of slow readers.
 * TODO CPU registration for dyntick-friendly behavior.
 */

#include <assert.h>
#include <stdalign.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdio.h>

#include <kern/atomic.h>
#include <kern/clock.h>
#include <kern/init.h>
#include <kern/macros.h>
#include <kern/rcu.h>
#include <kern/panic.h>
#include <kern/percpu.h>
#include <kern/spinlock.h>
#include <kern/sref.h>
#include <kern/syscnt.h>
#include <kern/thread.h>
#include <kern/timer.h>
#include <kern/work.h>
#include <machine/cpu.h>

/*
 * Negative close to 0 so that an overflow occurs early.
 */
#define RCU_WINDOW_ID_INIT_VALUE ((unsigned int)-500)

/*
 * Interval (in milliseconds) between window checking.
 *
 * When windows are checked, a flip occurs if the previous window isn't
 * active any more.
 */
#define RCU_WINDOW_CHECK_INTERVAL CONFIG_RCU_WINDOW_CHECK_INTERVAL

/*
 * Grace period states.
 *
 * These states are only used to trigger per-CPU processing that is
 * globally acknowleged by decrementing a global atomic counter. They
 * do not completely represent the actual state of a grace period.
 */
enum rcu_gp_state {
    RCU_GP_STATE_WORK_WINDOW_FLIP,
    RCU_GP_STATE_READER_WINDOW_FLIP,
    RCU_GP_STATE_WORK_FLUSH,
};

/*
 * Per-CPU view of a window.
 *
 * Deferred works are scheduled when the window ends.
 */
struct rcu_cpu_window {
    struct work_queue works;
};

/*
 * Per-CPU RCU data.
 *
 * Each processor maintains two local window IDs. One is used as the current
 * window ID when deferring work, the other when detecting a reader. A local
 * flip occurs when a processor notices that the global grace period state
 * no longer matches the local grace period state. These checks only occur
 * on periodic events.
 *
 * Interrupts and preemption must be disabled when accessing local CPU data.
 */
struct rcu_cpu_data {
    enum rcu_gp_state gp_state;
    unsigned int work_wid;
    unsigned int reader_wid;
    struct rcu_cpu_window windows[2];
    struct syscnt sc_nr_detected_readers;
};

/*
 * Global window.
 *
 * A window is a time range that tracks read-side references. Conceptually,
 * each reader adds a reference to the current window. In practice, references
 * are only added when readers are detected, which occurs on a context switch
 * (to track preempted threads) or a reader window flip (to prevent currently
 * running readers to be linked to the next window).
 *
 * When a window is started, its scalable reference counter is initialized
 * with a reference owned by the RCU system. That reference guarantees that
 * the window remains active as long as new readers may add references,
 * since it prevents the counter from dropping to 0. After a reader window
 * flip, there may not be new references to the window, and the initial
 * reference is dropped, allowing the counter to reach 0 once all detected
 * readers leave their critical section and unreference the window they're
 * linked to.
 */
struct rcu_window {
    struct sref_counter nr_refs;
    uint64_t start_ts;
    bool active;
};

/*
 * Global data.
 *
 * Processors regularly check the grace period state against their own,
 * locally cached grace period state, and take action whenever they differ.
 * False sharing is avoided by making the global grace period state fill an
 * entire cache line on SMP.
 *
 * After processors notice a grace period state change, they acknowledge
 * noticing this change by decrementing the atomic acknowledgment counter,
 * which also fills a complete cache line on SMP in order to restrict cache
 * line bouncing. Atomic operations on this counter are done with
 * acquire-release ordering to enforce the memory ordering guarantees
 * required by the implementation, as well as those provided by the public
 * interface.
 *
 * In addition to the global window ID and the windows themselves, the data
 * include a timer, used to trigger the end of windows, i.e. grace periods.
 * Since the timer function, atomic acknowledgments, and window no-reference
 * function chain each other, there is currently no need for a global lock.
 */
struct rcu_data {
    struct {
        alignas(CPU_L1_SIZE) enum rcu_gp_state gp_state;
    };
    struct {
        alignas(CPU_L1_SIZE) unsigned int nr_acks;
    };

    unsigned int wid;
    struct rcu_window windows[2];
    struct timer timer;
    struct syscnt sc_nr_windows;
    struct syscnt sc_last_window_ms;
    struct syscnt sc_longest_window_ms;
};

/*
 * Structure used to implement rcu_wait().
 */
struct rcu_waiter {
    struct work work;
    struct spinlock lock;
    struct thread *thread;
    bool done;
};

static struct rcu_data rcu_data;
static struct rcu_cpu_data rcu_cpu_data __percpu;

static struct rcu_cpu_data *
rcu_get_cpu_data(void)
{
    assert(!cpu_intr_enabled());
    assert(!thread_preempt_enabled());

    return cpu_local_ptr(rcu_cpu_data);
}

static enum rcu_gp_state
rcu_data_get_gp_state(const struct rcu_data *data)
{
    return data->gp_state;
}

static unsigned int
rcu_data_get_wid(const struct rcu_data *data)
{
    return data->wid;
}

static struct rcu_window *
rcu_data_get_window_from_index(struct rcu_data *data, size_t index)
{
    assert(index < ARRAY_SIZE(data->windows));
    return &data->windows[index];
}

static struct rcu_window *
rcu_data_get_window(struct rcu_data *data, unsigned int wid)
{
    return rcu_data_get_window_from_index(data, wid & 1);
}

static void
rcu_data_update_gp_state(struct rcu_data *data, enum rcu_gp_state gp_state)
{
    assert(data->nr_acks  == 0);

    switch (gp_state) {
    case RCU_GP_STATE_WORK_WINDOW_FLIP:
        assert(data->gp_state == RCU_GP_STATE_WORK_FLUSH);
        break;
    case RCU_GP_STATE_READER_WINDOW_FLIP:
        assert(data->gp_state == RCU_GP_STATE_WORK_WINDOW_FLIP);
        break;
    case RCU_GP_STATE_WORK_FLUSH:
        assert(data->gp_state == RCU_GP_STATE_READER_WINDOW_FLIP);
        break;
    default:
        panic("rcu: invalid grace period state");
    }

    data->nr_acks = cpu_count();
    atomic_store(&data->gp_state, gp_state, ATOMIC_RELEASE);
}

static bool
rcu_data_check_gp_state(const struct rcu_data *data,
                        enum rcu_gp_state local_gp_state,
                        enum rcu_gp_state *global_gp_state)
{
    *global_gp_state = atomic_load(&data->gp_state, ATOMIC_RELAXED);

    if (unlikely(local_gp_state != *global_gp_state)) {
        atomic_fence(ATOMIC_ACQUIRE);
        return true;
    }

    return false;
}

static void
rcu_window_end(struct rcu_window *window)
{
    assert(window->active);
    window->active = false;
}

static void
rcu_window_ref(struct rcu_window *window)
{
    sref_counter_inc(&window->nr_refs);
}

static void
rcu_window_unref(struct rcu_window *window)
{
    sref_counter_dec(&window->nr_refs);
}

static uint64_t
rcu_window_get_start_ts(const struct rcu_window *window)
{
    return window->start_ts;
}

static void
rcu_window_flush(struct sref_counter *counter)
{
    (void)counter;

    rcu_data_update_gp_state(&rcu_data, RCU_GP_STATE_WORK_FLUSH);
}

static void __init
rcu_window_init(struct rcu_window *window)
{
    window->active = false;
}

static void
rcu_window_start(struct rcu_window *window)
{
    assert(!window->active);

    sref_counter_init(&window->nr_refs, 1, NULL, rcu_window_flush);
    window->start_ts = clock_get_time();
    window->active = true;
}

static bool
rcu_window_active(const struct rcu_window *window)
{
    return window->active;
}

static void
rcu_data_end_prev_window(struct rcu_data *data, uint64_t now)
{
    struct rcu_window *window;
    uint64_t duration;

    window = rcu_data_get_window(data, data->wid - 1);
    duration = clock_ticks_to_ms(now - rcu_window_get_start_ts(window));
    syscnt_set(&data->sc_last_window_ms, duration);

    if (duration > syscnt_read(&data->sc_longest_window_ms)) {
        syscnt_set(&data->sc_longest_window_ms, duration);
    }

    rcu_window_end(window);
}

static void
rcu_data_schedule_timer(struct rcu_data *data, uint64_t now)
{
    uint64_t ticks;

    ticks = clock_ticks_from_ms(RCU_WINDOW_CHECK_INTERVAL);
    timer_schedule(&data->timer, now + ticks);
}

static void
rcu_data_ack_cpu(struct rcu_data *data)
{
    struct rcu_window *window;
    unsigned int prev_nr_acks;
    uint64_t now;

    prev_nr_acks = atomic_fetch_sub(&data->nr_acks, 1, ATOMIC_ACQ_REL);

    if (prev_nr_acks != 1) {
        assert(prev_nr_acks != 0);
        return;
    }

    switch (data->gp_state) {
    case RCU_GP_STATE_WORK_WINDOW_FLIP:
        rcu_data_update_gp_state(data, RCU_GP_STATE_READER_WINDOW_FLIP);
        break;
    case RCU_GP_STATE_READER_WINDOW_FLIP:
        window = rcu_data_get_window(data, data->wid - 1);
        rcu_window_unref(window);
        break;
    case RCU_GP_STATE_WORK_FLUSH:
        now = clock_get_time();
        rcu_data_end_prev_window(data, now);
        rcu_data_schedule_timer(data, now);
        break;
    default:
        panic("rcu: invalid grace period state");
    }
}

static bool
rcu_data_flip_windows(struct rcu_data *data)
{
    struct rcu_window *window;

    window = rcu_data_get_window(data, data->wid - 1);

    if (rcu_window_active(window)) {
        return false;
    }

    rcu_window_start(window);
    syscnt_inc(&data->sc_nr_windows);
    data->wid++;
    rcu_data_update_gp_state(data, RCU_GP_STATE_WORK_WINDOW_FLIP);
    return true;
}

static void
rcu_data_check_windows(struct timer *timer)
{
    struct rcu_data *data;
    bool flipped;

    data = &rcu_data;
    flipped = rcu_data_flip_windows(data);

    if (!flipped) {
        rcu_data_schedule_timer(data, timer_get_time(timer));
    }
}

static void __init
rcu_data_init(struct rcu_data *data)
{
    data->gp_state = RCU_GP_STATE_WORK_FLUSH;
    data->nr_acks = 0;
    data->wid = RCU_WINDOW_ID_INIT_VALUE;

    for (size_t i = 0; i < ARRAY_SIZE(data->windows); i++) {
        rcu_window_init(rcu_data_get_window_from_index(data, i));
    }

    rcu_window_start(rcu_data_get_window(data, data->wid));

    timer_init(&data->timer, rcu_data_check_windows, 0);
    rcu_data_schedule_timer(data, clock_get_time());

    syscnt_register(&data->sc_nr_windows, "rcu_nr_windows");
    syscnt_register(&data->sc_last_window_ms, "rcu_last_window_ms");
    syscnt_register(&data->sc_longest_window_ms, "rcu_longest_window_ms");
}

static void __init
rcu_cpu_window_init(struct rcu_cpu_window *cpu_window)
{
    work_queue_init(&cpu_window->works);
}

static void
rcu_cpu_window_queue(struct rcu_cpu_window *cpu_window, struct work *work)
{
    work_queue_push(&cpu_window->works, work);
}

static void
rcu_cpu_window_flush(struct rcu_cpu_window *cpu_window)
{
    work_queue_schedule(&cpu_window->works, 0);
    work_queue_init(&cpu_window->works);
}

static unsigned int
rcu_cpu_data_get_reader_wid(const struct rcu_cpu_data *cpu_data)
{
    return cpu_data->reader_wid;
}

static struct rcu_cpu_window *
rcu_cpu_data_get_window_from_index(struct rcu_cpu_data *cpu_data, size_t index)
{
    assert(index < ARRAY_SIZE(cpu_data->windows));
    return &cpu_data->windows[index];
}

static struct rcu_cpu_window *
rcu_cpu_data_get_window(struct rcu_cpu_data *cpu_data, unsigned int wid)
{
    return rcu_cpu_data_get_window_from_index(cpu_data, wid & 1);
}

static void __init
rcu_cpu_data_init(struct rcu_cpu_data *cpu_data, unsigned int cpu)
{
    struct rcu_data *data;
    char name[SYSCNT_NAME_SIZE];

    data = &rcu_data;

    cpu_data->gp_state = rcu_data_get_gp_state(data);
    cpu_data->work_wid = rcu_data_get_wid(data);
    cpu_data->reader_wid = cpu_data->work_wid;

    for (size_t i = 0; i < ARRAY_SIZE(cpu_data->windows); i++) {
        rcu_cpu_window_init(rcu_cpu_data_get_window_from_index(cpu_data, i));
    }

    snprintf(name, sizeof(name), "rcu_nr_detected_readers/%u", cpu);
    syscnt_register(&cpu_data->sc_nr_detected_readers, name);
}

static void
rcu_cpu_data_queue(struct rcu_cpu_data *cpu_data, struct work *work)
{
    struct rcu_cpu_window *cpu_window;

    cpu_window = rcu_cpu_data_get_window(cpu_data, cpu_data->work_wid);
    rcu_cpu_window_queue(cpu_window, work);
}

static void
rcu_cpu_data_flush(struct rcu_cpu_data *cpu_data)
{
    struct rcu_cpu_window *cpu_window;

    assert(cpu_data->work_wid == cpu_data->reader_wid);

    cpu_window = rcu_cpu_data_get_window(cpu_data, cpu_data->work_wid - 1);
    rcu_cpu_window_flush(cpu_window);
}

void
rcu_reader_init(struct rcu_reader *reader)
{
    reader->level = 0;
    reader->linked = false;
}

static void
rcu_reader_link(struct rcu_reader *reader, struct rcu_cpu_data *cpu_data)
{
    assert(!cpu_intr_enabled());
    assert(reader == thread_rcu_reader(thread_self()));
    assert(!rcu_reader_linked(reader));

    reader->wid = rcu_cpu_data_get_reader_wid(cpu_data);
    reader->linked = true;
}

static void
rcu_reader_unlink(struct rcu_reader *reader)
{
    assert(reader->level == 0);
    reader->linked = false;
}

static void
rcu_reader_enter(struct rcu_reader *reader, struct rcu_cpu_data *cpu_data)
{
    struct rcu_window *window;
    struct rcu_data *data;
    unsigned int wid;

    if (rcu_reader_linked(reader)) {
        return;
    }

    data = &rcu_data;
    wid = rcu_cpu_data_get_reader_wid(cpu_data);
    window = rcu_data_get_window(data, wid);

    rcu_reader_link(reader, cpu_data);
    rcu_window_ref(window);

    syscnt_inc(&cpu_data->sc_nr_detected_readers);
}

void
rcu_reader_leave(struct rcu_reader *reader)
{
    struct rcu_window *window;
    struct rcu_data *data;

    data = &rcu_data;

    window = rcu_data_get_window(data, reader->wid);
    rcu_window_unref(window);
    rcu_reader_unlink(reader);
}

static void
rcu_reader_account(struct rcu_reader *reader, struct rcu_cpu_data *cpu_data)
{
    if (rcu_reader_in_cs(reader)) {
        rcu_reader_enter(reader, cpu_data);
    }
}

static void
rcu_cpu_data_flip_work_wid(struct rcu_cpu_data *cpu_data)
{
    assert(!cpu_intr_enabled());
    assert(!thread_preempt_enabled());

    cpu_data->work_wid++;
}

static void
rcu_cpu_data_flip_reader_wid(struct rcu_cpu_data *cpu_data)
{
    assert(!cpu_intr_enabled());
    assert(!thread_preempt_enabled());

    rcu_reader_account(thread_rcu_reader(thread_self()), cpu_data);
    cpu_data->reader_wid++;
}

static void
rcu_cpu_data_check_gp_state(struct rcu_cpu_data *cpu_data)
{
    enum rcu_gp_state local_gp_state, global_gp_state;
    struct rcu_data *data;
    bool diff;

    data = &rcu_data;

    /*
     * A loop is used to optimize the case where a processor is the last to
     * acknowledge a grace period state change, in which case the latter
     * also immediately changes and can be acknowleged right away. As a
     * result, this loop may never run more than twice.
     */
    for (unsigned int i = 0; /* no condition */; i++) {
        local_gp_state = cpu_data->gp_state;
        diff = rcu_data_check_gp_state(data, local_gp_state, &global_gp_state);

        if (!diff) {
            break;
        }

        assert(i < 2);

        switch (global_gp_state) {
        case RCU_GP_STATE_WORK_WINDOW_FLIP:
            rcu_cpu_data_flip_work_wid(cpu_data);
            rcu_data_ack_cpu(data);
            break;
        case RCU_GP_STATE_READER_WINDOW_FLIP:
            rcu_cpu_data_flip_reader_wid(cpu_data);
            rcu_data_ack_cpu(data);
            break;
        case RCU_GP_STATE_WORK_FLUSH:
            rcu_cpu_data_flush(cpu_data);
            rcu_data_ack_cpu(data);
            break;
        default:
            panic("rcu: invalid grace period state");
        }

        cpu_data->gp_state = global_gp_state;
    }
}

void
rcu_report_context_switch(struct rcu_reader *reader)
{
    assert(!cpu_intr_enabled());
    assert(!thread_preempt_enabled());

    /*
     * Most readers don't need to be accounted for because their execution
     * doesn't overlap with a grace period. If a reader is preempted however,
     * it must be accounted in case a grace period starts while the reader
     * is preempted. Accounting also occurs when a grace period starts, and
     * more exactly, when the reader window ID of a processor is flipped.
     */
    rcu_reader_account(reader, rcu_get_cpu_data());
}

void
rcu_report_periodic_event(void)
{
    assert(!cpu_intr_enabled());
    assert(!thread_preempt_enabled());

    rcu_cpu_data_check_gp_state(rcu_get_cpu_data());
}

void
rcu_defer(struct work *work)
{
    struct rcu_cpu_data *cpu_data;
    unsigned long flags;

    assert(!rcu_reader_in_cs(thread_rcu_reader(thread_self())));

    thread_preempt_disable_intr_save(&flags);
    cpu_data = rcu_get_cpu_data();
    rcu_cpu_data_queue(cpu_data, work);
    thread_preempt_enable_intr_restore(flags);
}

static void
rcu_waiter_wakeup(struct work *work)
{
    struct rcu_waiter *waiter;

    waiter = structof(work, struct rcu_waiter, work);

    spinlock_lock(&waiter->lock);
    waiter->done = true;
    thread_wakeup(waiter->thread);
    spinlock_unlock(&waiter->lock);
}

static void
rcu_waiter_init(struct rcu_waiter *waiter, struct thread *thread)
{
    work_init(&waiter->work, rcu_waiter_wakeup);
    spinlock_init(&waiter->lock);
    waiter->thread = thread;
    waiter->done = false;
}

static void
rcu_waiter_wait(struct rcu_waiter *waiter)
{
    rcu_defer(&waiter->work);

    spinlock_lock(&waiter->lock);

    while (!waiter->done) {
        thread_sleep(&waiter->lock, waiter, "rcu_wait");
    }

    spinlock_unlock(&waiter->lock);
}

void
rcu_wait(void)
{
    struct rcu_waiter waiter;

    rcu_waiter_init(&waiter, thread_self()),
    rcu_waiter_wait(&waiter);
}

static int __init
rcu_bootstrap(void)
{
    rcu_data_init(&rcu_data);
    rcu_cpu_data_init(cpu_local_ptr(rcu_cpu_data), 0);
    return 0;
}

INIT_OP_DEFINE(rcu_bootstrap,
               INIT_OP_DEP(spinlock_setup, true),
               INIT_OP_DEP(sref_bootstrap, true),
               INIT_OP_DEP(syscnt_setup, true),
               INIT_OP_DEP(thread_bootstrap, true),
               INIT_OP_DEP(timer_bootstrap, true));

static int __init
rcu_setup(void)
{
    for (unsigned int i = 1; i < cpu_count(); i++) {
        rcu_cpu_data_init(percpu_ptr(rcu_cpu_data, i), i);
    }

    return 0;
}

INIT_OP_DEFINE(rcu_setup,
               INIT_OP_DEP(cpu_mp_probe, true),
               INIT_OP_DEP(rcu_bootstrap, true));


code:
/*
 * Copyright (c) 2018 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 * Read-Copy Update.
 *
 * This module provides a synchronization framework for read-only lockless
 * operations, best used on read-mostly data structures.
 *
 * To learn more about RCU, see "Is Parallel Programming Hard, And, If So,
 * What Can You Do About It ?" by Paul E. McKenney.
 *
 * A thorough discussion of RCU's requirements can be found at [1].
 * However, the memory model and terminology used in that document being
 * different from C11, the memory barrier guarantees are hereby translated
 * into memory ordering guarantees :
 *
 *  1. Release-acquire ordering is enforced between the end of all
 *     read-side critical sections starting before a grace period starts
 *     and the start of all works deferred before the same grace period
 *     starts.
 *  2. Release-acquire ordering is enforced between the start of all
 *     read-side critical sections ending after a grace period ends
 *     and all the work deferrals done before the same grace period
 *     starts.
 *  3. Release-acquire ordering is enforced between all the work deferrals
 *     done before a grace period starts and the start of all works deferred
 *     before the same grace period starts.
 *
 * [1] https://www.kernel.org/doc/Documentation/RCU/Design/Requirements/Requirements.html.
 */

#ifndef KERN_RCU_H
#define KERN_RCU_H

#include <kern/atomic.h>
#include <kern/init.h>
#include <kern/rcu_i.h>
#include <kern/rcu_types.h>
#include <kern/thread.h>
#include <kern/work.h>

/*
 * Thread-local RCU data.
 */
struct rcu_reader;

/*
 * Safely store a pointer.
 *
 * This macro enforces release ordering on the given pointer.
 */
#define rcu_store_ptr(ptr, value) atomic_store(&(ptr), value, ATOMIC_RELEASE)

/*
 * Safely load a pointer.
 *
 * This macro enforces consume ordering on the given pointer.
 */
#define rcu_load_ptr(ptr) atomic_load(&(ptr), ATOMIC_CONSUME)

/*
 * Read-side critical section functions.
 *
 * Critical sections are preemptible, may safely nest, and may safely
 * be used in interrupt context. It is not allowed to sleep inside a
 * read-side critical section. However, it is allowed to acquire locks
 * that don't sleep, such as spin locks.
 *
 * Entering or leaving a critical section doesn't provide any memory
 * ordering guarantee.
 */

static inline void
rcu_read_enter(void)
{
    rcu_reader_inc(thread_rcu_reader(thread_self()));
    barrier();
}

static inline void
rcu_read_leave(void)
{
    barrier();
    rcu_reader_dec(thread_rcu_reader(thread_self()));
}

/*
 * Initialize a RCU reader.
 */
void rcu_reader_init(struct rcu_reader *reader);

/*
 * Report a context switch on the current processor.
 *
 * The argument is the RCU reader of the preempted thread.
 *
 * Interrupts and preemption must be disabled when calling this function.
 */
void rcu_report_context_switch(struct rcu_reader *reader);

/*
 * Report a periodic event on the current processor.
 *
 * Interrupts and preemption must be disabled when calling this function.
 */
void rcu_report_periodic_event(void);

/*
 * Defer a work until all existing read-side references are dropped,
 * without blocking.
 *
 * The given work is scheduled when all existing read-side references
 * are dropped.
 */
void rcu_defer(struct work *work);

/*
 * Wait for all existing read-side references to be dropped.
 *
 * This function sleeps, and may do so for a moderately long duration,
 * at leat a few system timer ticks, sometimes a lot more.
 */
void rcu_wait(void);

/*
 * This init operation provides :
 *  - read-side critical sections usable
 */
INIT_OP_DECLARE(rcu_bootstrap);

#endif /* KERN_RCU_H */

license: mit


code:
/*
 * Copyright (c) 2018 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef KERN_RCU_I_H
#define KERN_RCU_I_H

#include <assert.h>
#include <stdbool.h>

#include <kern/macros.h>
#include <kern/rcu_types.h>
#include <kern/thread.h>

void rcu_reader_leave(struct rcu_reader *reader);

static inline bool
rcu_reader_in_cs(const struct rcu_reader *reader)
{
    return reader->level != 0;
}

static inline bool
rcu_reader_linked(const struct rcu_reader *reader)
{
    assert(reader == thread_rcu_reader(thread_self()));
    return reader->linked;
}

static inline void
rcu_reader_inc(struct rcu_reader *reader)
{
    reader->level++;
    assert(reader->level != 0);
}

static inline void
rcu_reader_dec(struct rcu_reader *reader)
{
    assert(reader->level != 0);
    reader->level--;

    if (unlikely(!rcu_reader_in_cs(reader) && rcu_reader_linked(reader))) {
        rcu_reader_leave(reader);
    }
}

#endif /* KERN_RCU_I_H */


code:
/*
 * Copyright (c) 2018 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 * Isolated type definition used to avoid inclusion circular dependencies.
 */

#ifndef KERN_RCU_TYPES_H
#define KERN_RCU_TYPES_H

#include <stdbool.h>

/*
 * Thread-local data used to track threads running read-side critical
 * sections.
 *
 * The window ID is valid if and only if the reader is linked.
 *
 * Interrupts and preemption must be disabled when accessing a reader.
 */
struct rcu_reader {
    unsigned int level;
    unsigned int wid;
    bool linked;
};

#endif /* KERN_RCU_TYPES_H */


code:
/*
 * Copyright (c) 2011-2018 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Upstream site with license notes :
 * http://git.sceen.net/rbraun/librbraun.git/
 */

#include <assert.h>
#include <errno.h>
#include <limits.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>

#include <kern/init.h>
#include <kern/kmem.h>
#include <kern/macros.h>
#include <kern/rcu.h>
#include <kern/rdxtree.h>
#include <kern/rdxtree_i.h>
#include <kern/work.h>

/*
 * Mask applied on an entry to obtain its address.
 */
#define RDXTREE_ENTRY_ADDR_MASK (~0x3UL)

/*
 * Global properties used to shape radix trees.
 */
#define RDXTREE_RADIX       6
#define RDXTREE_RADIX_SIZE  (1UL << RDXTREE_RADIX)
#define RDXTREE_RADIX_MASK  (RDXTREE_RADIX_SIZE - 1)

#if RDXTREE_RADIX < 6
typedef unsigned long rdxtree_bm_t;
#define rdxtree_ffs(x) __builtin_ffsl(x)
#elif RDXTREE_RADIX == 6 /* RDXTREE_RADIX < 6 */
typedef unsigned long long rdxtree_bm_t;
#define rdxtree_ffs(x) __builtin_ffsll(x)
#else /* RDXTREE_RADIX < 6 */
#error "radix too high"
#endif /* RDXTREE_RADIX < 6 */

/*
 * Allocation bitmap size in bits.
 */
#define RDXTREE_BM_SIZE (sizeof(rdxtree_bm_t) * CHAR_BIT)

/*
 * Empty/full allocation bitmap words.
 */
#define RDXTREE_BM_EMPTY    ((rdxtree_bm_t)0)
#define RDXTREE_BM_FULL \
    ((~(rdxtree_bm_t)0) >> (RDXTREE_BM_SIZE - RDXTREE_RADIX_SIZE))

/*
 * Radix tree node.
 *
 * The height of a tree is the number of nodes to traverse until stored
 * pointers are reached. A height of 0 means the entries of a node (or the
 * tree root) directly point to stored pointers.
 *
 * The index is valid if and only if the parent isn't NULL.
 *
 * Concerning the allocation bitmap, a bit is set when the node it denotes,
 * or one of its children, can be used to allocate an entry. Conversely, a bit
 * is clear when the matching node and all of its children have no free entry.
 *
 * In order to support safe lockless lookups, in particular during a resize,
 * each node includes the height of its subtree, which is invariant during
 * the entire node lifetime. Since the tree height does vary, it can't be
 * used to determine whether the tree root is a node or a stored pointer.
 * This implementation assumes that all nodes and stored pointers are at least
 * 4-byte aligned, and uses the least significant bit of entries to indicate
 * the pointer type. This bit is set for internal nodes, and clear for stored
 * pointers so that they can be accessed from slots without conversion.
 */
struct rdxtree_node {
    union {
        struct {
            struct rdxtree_node *parent;
            unsigned short index;
        };

        /* Deferred destruction when unlinked */
        struct work work;
    };

    unsigned short height;
    unsigned short nr_entries;
    rdxtree_bm_t alloc_bm;
    void *entries[RDXTREE_RADIX_SIZE];
};

static struct kmem_cache rdxtree_node_cache;

#ifndef NDEBUG
static bool
rdxtree_alignment_valid(const void *ptr)
{
    return (((uintptr_t)ptr & ~RDXTREE_ENTRY_ADDR_MASK) == 0);
}
#endif /* NDEBUG */

static inline void *
rdxtree_entry_addr(void *entry)
{
    return (void *)((uintptr_t)entry & RDXTREE_ENTRY_ADDR_MASK);
}

static inline bool
rdxtree_entry_is_node(const void *entry)
{
    return ((uintptr_t)entry & 1) != 0;
}

static inline void *
rdxtree_node_to_entry(struct rdxtree_node *node)
{
    return (void *)((uintptr_t)node | 1);
}

static void
rdxtree_node_ctor(void *buf)
{
    struct rdxtree_node *node;

    node = buf;
    node->nr_entries = 0;
    node->alloc_bm = RDXTREE_BM_FULL;
    memset(node->entries, 0, sizeof(node->entries));
}

static int
rdxtree_node_create(struct rdxtree_node **nodep, unsigned short height)
{
    struct rdxtree_node *node;

    node = kmem_cache_alloc(&rdxtree_node_cache);

    if (node == NULL) {
        return ENOMEM;
    }

    assert(rdxtree_alignment_valid(node));
    node->parent = NULL;
    node->height = height;
    *nodep = node;
    return 0;
}

static void
rdxtree_node_destroy(struct rdxtree_node *node)
{
    /* See rdxtree_shrink() */
    if (node->nr_entries != 0) {
        assert(node->nr_entries == 1);
        assert(node->entries[0] != NULL);
        node->entries[0] = NULL;
        node->nr_entries = 0;
        node->alloc_bm = RDXTREE_BM_FULL;
    }

    kmem_cache_free(&rdxtree_node_cache, node);
}

static void
rdxtree_node_destroy_deferred(struct work *work)
{
    struct rdxtree_node *node;

    node = structof(work, struct rdxtree_node, work);
    rdxtree_node_destroy(node);
}

static void
rdxtree_node_schedule_destruction(struct rdxtree_node *node)
{
    assert(node->parent == NULL);

    work_init(&node->work, rdxtree_node_destroy_deferred);
    rcu_defer(&node->work);
}

static inline void
rdxtree_node_link(struct rdxtree_node *node, struct rdxtree_node *parent,
                  unsigned short index)
{
    node->parent = parent;
    node->index = index;
}

static inline void
rdxtree_node_unlink(struct rdxtree_node *node)
{
    assert(node->parent != NULL);
    node->parent = NULL;
}

static inline bool
rdxtree_node_full(struct rdxtree_node *node)
{
    return (node->nr_entries == ARRAY_SIZE(node->entries));
}

static inline bool
rdxtree_node_empty(struct rdxtree_node *node)
{
    return (node->nr_entries == 0);
}

static inline void
rdxtree_node_insert(struct rdxtree_node *node, unsigned short index,
                    void *entry)
{
    assert(index < ARRAY_SIZE(node->entries));
    assert(node->entries[index] == NULL);

    node->nr_entries++;
    rcu_store_ptr(node->entries[index], entry);
}

static inline void
rdxtree_node_insert_node(struct rdxtree_node *node, unsigned short index,
                         struct rdxtree_node *child)
{
    rdxtree_node_insert(node, index, rdxtree_node_to_entry(child));
}

static inline void
rdxtree_node_remove(struct rdxtree_node *node, unsigned short index)
{
    assert(index < ARRAY_SIZE(node->entries));
    assert(node->entries[index] != NULL);

    node->nr_entries--;
    rcu_store_ptr(node->entries[index], NULL);
}

static inline void *
rdxtree_node_find(struct rdxtree_node *node, unsigned short *indexp)
{
    unsigned short index;
    void *ptr;

    index = *indexp;

    while (index < ARRAY_SIZE(node->entries)) {
        ptr = rdxtree_entry_addr(rcu_load_ptr(node->entries[index]));

        if (ptr != NULL) {
            *indexp = index;
            return ptr;
        }

        index++;
    }

    return NULL;
}

static inline void
rdxtree_node_bm_set(struct rdxtree_node *node, unsigned short index)
{
    node->alloc_bm |= (rdxtree_bm_t)1 << index;
}

static inline void
rdxtree_node_bm_clear(struct rdxtree_node *node, unsigned short index)
{
    node->alloc_bm &= ~((rdxtree_bm_t)1 << index);
}

static inline bool
rdxtree_node_bm_is_set(struct rdxtree_node *node, unsigned short index)
{
    return (node->alloc_bm & ((rdxtree_bm_t)1 << index));
}

static inline bool
rdxtree_node_bm_empty(struct rdxtree_node *node)
{
    return (node->alloc_bm == RDXTREE_BM_EMPTY);
}

static inline unsigned short
rdxtree_node_bm_first(struct rdxtree_node *node)
{
    return rdxtree_ffs(node->alloc_bm) - 1;
}

static inline rdxtree_key_t
rdxtree_max_key(unsigned short height)
{
    size_t shift;

    shift = RDXTREE_RADIX * height;

    if (likely(shift < (sizeof(rdxtree_key_t) * CHAR_BIT))) {
        return ((rdxtree_key_t)1 << shift) - 1;
    } else {
        return ~((rdxtree_key_t)0);
    }
}

static inline bool
rdxtree_key_alloc_enabled(const struct rdxtree *tree)
{
    return (tree->flags & RDXTREE_KEY_ALLOC);
}

static void
rdxtree_shrink(struct rdxtree *tree)
{
    struct rdxtree_node *node;
    void *entry;

    while (tree->height > 0) {
        node = rdxtree_entry_addr(tree->root);

        if (node->nr_entries != 1) {
            break;
        }

        entry = node->entries[0];

        if (entry == NULL) {
            break;
        }

        tree->height--;

        if (tree->height > 0) {
            rdxtree_node_unlink(rdxtree_entry_addr(entry));
        }

        rcu_store_ptr(tree->root, entry);

        /*
         * There is still one valid entry (the first one) in this node. It
         * must remain valid as long as read-side references can exist so
         * that concurrent lookups can find the rest of the tree. Therefore,
         * this entry isn't reset before node destruction.
         */
        rdxtree_node_schedule_destruction(node);
    }
}

static int
rdxtree_grow(struct rdxtree *tree, rdxtree_key_t key)
{
    struct rdxtree_node *root, *node;
    unsigned short new_height;
    int error;

    new_height = tree->height + 1;

    while (key > rdxtree_max_key(new_height)) {
        new_height++;
    }

    if (tree->root == NULL) {
        tree->height = new_height;
        return 0;
    }

    root = rdxtree_entry_addr(tree->root);

    do {
        error = rdxtree_node_create(&node, tree->height);

        if (error) {
            rdxtree_shrink(tree);
            return error;
        }

        if (tree->height == 0) {
            if (rdxtree_key_alloc_enabled(tree)) {
                rdxtree_node_bm_clear(node, 0);
            }
        } else {
            rdxtree_node_link(root, node, 0);

            if (rdxtree_key_alloc_enabled(tree)
                && rdxtree_node_bm_empty(root)) {
                rdxtree_node_bm_clear(node, 0);
            }
        }

        rdxtree_node_insert(node, 0, tree->root);
        tree->height++;
        rcu_store_ptr(tree->root, rdxtree_node_to_entry(node));
        root = node;
    } while (new_height > tree->height);

    return 0;
}

static void
rdxtree_cleanup(struct rdxtree *tree, struct rdxtree_node *node)
{
    struct rdxtree_node *prev;

    for (;;) {
        if (likely(!rdxtree_node_empty(node))) {
            if (unlikely(node->parent == NULL)) {
                rdxtree_shrink(tree);
            }

            break;
        }

        if (node->parent == NULL) {
            tree->height = 0;
            rcu_store_ptr(tree->root, NULL);
            rdxtree_node_schedule_destruction(node);
            break;
        }

        prev = node;
        node = node->parent;
        rdxtree_node_unlink(prev);
        rdxtree_node_remove(node, prev->index);
        rdxtree_node_schedule_destruction(prev);
    }
}

static void
rdxtree_insert_bm_clear(struct rdxtree_node *node, unsigned short index)
{
    for (;;) {
        rdxtree_node_bm_clear(node, index);

        if (!rdxtree_node_full(node) || (node->parent == NULL)) {
            break;
        }

        index = node->index;
        node = node->parent;
    }
}

int
rdxtree_insert_common(struct rdxtree *tree, rdxtree_key_t key,
                      void *ptr, void ***slotp)
{
    struct rdxtree_node *node, *prev;
    unsigned short height, shift;
    unsigned short index = 0; /* GCC */
    int error;

    assert(ptr != NULL);
    assert(rdxtree_alignment_valid(ptr));

    if (unlikely(key > rdxtree_max_key(tree->height))) {
        error = rdxtree_grow(tree, key);

        if (error) {
            return error;
        }
    }

    height = tree->height;

    if (unlikely(height == 0)) {
        if (tree->root != NULL) {
            return EBUSY;
        }

        rcu_store_ptr(tree->root, ptr);

        if (slotp != NULL) {
            *slotp = &tree->root;
        }

        return 0;
    }

    node = rdxtree_entry_addr(tree->root);
    shift = (height - 1) * RDXTREE_RADIX;
    prev = NULL;

    do {
        if (node == NULL) {
            error = rdxtree_node_create(&node, height - 1);

            if (error) {
                if (prev == NULL) {
                    tree->height = 0;
                } else {
                    rdxtree_cleanup(tree, prev);
                }

                return error;
            }

            if (prev == NULL) {
                rcu_store_ptr(tree->root, rdxtree_node_to_entry(node));
            } else {
                rdxtree_node_link(node, prev, index);
                rdxtree_node_insert_node(prev, index, node);
            }
        }

        prev = node;
        index = (unsigned short)(key >> shift) & RDXTREE_RADIX_MASK;
        node = rdxtree_entry_addr(prev->entries[index]);
        shift -= RDXTREE_RADIX;
        height--;
    } while (height > 0);

    if (unlikely(node != NULL)) {
        return EBUSY;
    }

    rdxtree_node_insert(prev, index, ptr);

    if (rdxtree_key_alloc_enabled(tree)) {
        rdxtree_insert_bm_clear(prev, index);
    }

    if (slotp != NULL) {
        *slotp = &prev->entries[index];
    }

    return 0;
}

int
rdxtree_insert_alloc_common(struct rdxtree *tree, void *ptr,
                            rdxtree_key_t *keyp, void ***slotp)
{
    struct rdxtree_node *node, *prev;
    unsigned short height, shift;
    unsigned short index = 0; /* GCC */
    rdxtree_key_t key;
    int error;

    assert(rdxtree_key_alloc_enabled(tree));
    assert(ptr != NULL);
    assert(rdxtree_alignment_valid(ptr));

    height = tree->height;

    if (unlikely(height == 0)) {
        if (tree->root == NULL) {
            rcu_store_ptr(tree->root, ptr);
            *keyp = 0;

            if (slotp != NULL) {
                *slotp = &tree->root;
            }

            return 0;
        }

        goto grow;
    }

    node = rdxtree_entry_addr(tree->root);
    key = 0;
    shift = (height - 1) * RDXTREE_RADIX;
    prev = NULL;

    do {
        if (node == NULL) {
            error = rdxtree_node_create(&node, height - 1);

            if (error) {
                rdxtree_cleanup(tree, prev);
                return error;
            }

            rdxtree_node_link(node, prev, index);
            rdxtree_node_insert_node(prev, index, node);
        }

        prev = node;
        index = rdxtree_node_bm_first(node);

        if (index == (unsigned short)-1) {
            goto grow;
        }

        key |= (rdxtree_key_t)index << shift;
        node = rdxtree_entry_addr(node->entries[index]);
        shift -= RDXTREE_RADIX;
        height--;
    } while (height > 0);

    rdxtree_node_insert(prev, index, ptr);
    rdxtree_insert_bm_clear(prev, index);

    if (slotp != NULL) {
        *slotp = &prev->entries[index];
    }

    goto out;

grow:
    key = rdxtree_max_key(height) + 1;
    error = rdxtree_insert_common(tree, key, ptr, slotp);

    if (error) {
        return error;
    }

out:
    *keyp = key;
    return 0;
}

static void
rdxtree_remove_bm_set(struct rdxtree_node *node, unsigned short index)
{
    do {
        rdxtree_node_bm_set(node, index);

        if (node->parent == NULL) {
            break;
        }

        index = node->index;
        node = node->parent;
    } while (!rdxtree_node_bm_is_set(node, index));
}

void *
rdxtree_remove(struct rdxtree *tree, rdxtree_key_t key)
{
    struct rdxtree_node *node, *prev;
    unsigned short height, shift, index;

    height = tree->height;

    if (unlikely(key > rdxtree_max_key(height))) {
        return NULL;
    }

    node = rdxtree_entry_addr(tree->root);

    if (unlikely(height == 0)) {
        rcu_store_ptr(tree->root, NULL);
        return node;
    }

    shift = (height - 1) * RDXTREE_RADIX;

    do {
        if (node == NULL) {
            return NULL;
        }

        prev = node;
        index = (unsigned short)(key >> shift) & RDXTREE_RADIX_MASK;
        node = rdxtree_entry_addr(node->entries[index]);
        shift -= RDXTREE_RADIX;
        height--;
    } while (height > 0);

    if (node == NULL) {
        return NULL;
    }

    if (rdxtree_key_alloc_enabled(tree)) {
        rdxtree_remove_bm_set(prev, index);
    }

    rdxtree_node_remove(prev, index);
    rdxtree_cleanup(tree, prev);
    return node;
}

void *
rdxtree_lookup_common(const struct rdxtree *tree, rdxtree_key_t key,
                      bool get_slot)
{
    struct rdxtree_node *node, *prev;
    unsigned short height, shift, index;
    void *entry;

    entry = rcu_load_ptr(tree->root);

    if (entry == NULL) {
        node = NULL;
        height = 0;
    } else {
        node = rdxtree_entry_addr(entry);
        height = rdxtree_entry_is_node(entry) ? node->height + 1 : 0;
    }

    if (key > rdxtree_max_key(height)) {
        return NULL;
    }

    if (height == 0) {
        if (node == NULL) {
            return NULL;
        }

        return get_slot ? (void *)&tree->root : node;
    }

    shift = (height - 1) * RDXTREE_RADIX;

    do {
        if (node == NULL) {
            return NULL;
        }

        prev = node;
        index = (unsigned short)(key >> shift) & RDXTREE_RADIX_MASK;
        entry = rcu_load_ptr(node->entries[index]);
        node = rdxtree_entry_addr(entry);
        shift -= RDXTREE_RADIX;
        height--;
    } while (height > 0);

    if (node == NULL) {
        return NULL;
    }

    return get_slot ? (void *)&prev->entries[index] : node;
}

void *
rdxtree_replace_slot(void **slot, void *ptr)
{
    void *old;

    assert(ptr != NULL);
    assert(rdxtree_alignment_valid(ptr));

    old = *slot;
    assert(old != NULL);
    assert(rdxtree_alignment_valid(old));
    rcu_store_ptr(*slot, ptr);
    return old;
}

static void *
rdxtree_walk_next(struct rdxtree *tree, struct rdxtree_iter *iter)
{
    struct rdxtree_node *root, *node, *prev;
    unsigned short height, shift, index, orig_index;
    rdxtree_key_t key;
    void *entry;

    entry = rcu_load_ptr(tree->root);

    if (entry == NULL) {
        return NULL;
    }

    if (!rdxtree_entry_is_node(entry)) {
        if (iter->key != (rdxtree_key_t)-1) {
            return NULL;
        } else {
            iter->key = 0;
            return rdxtree_entry_addr(entry);
        }
    }

    key = iter->key + 1;

    if ((key == 0) && (iter->node != NULL)) {
        return NULL;
    }

    root = rdxtree_entry_addr(entry);

restart:
    node = root;
    height = root->height + 1;

    if (key > rdxtree_max_key(height)) {
        return NULL;
    }

    shift = (height - 1) * RDXTREE_RADIX;

    do {
        prev = node;
        index = (key >> shift) & RDXTREE_RADIX_MASK;
        orig_index = index;
        node = rdxtree_node_find(node, &index);

        if (node == NULL) {
            shift += RDXTREE_RADIX;
            key = ((key >> shift) + 1) << shift;

            if (key == 0) {
                return NULL;
            }

            goto restart;
        }

        if (orig_index != index) {
            key = ((key >> shift) + (index - orig_index)) << shift;
        }

        shift -= RDXTREE_RADIX;
        height--;
    } while (height > 0);

    iter->node = prev;
    iter->key = key;
    return node;
}

void *
rdxtree_walk(struct rdxtree *tree, struct rdxtree_iter *iter)
{
    unsigned short index, orig_index;
    void *ptr;

    if (iter->node == NULL) {
        return rdxtree_walk_next(tree, iter);
    }

    index = (iter->key + 1) & RDXTREE_RADIX_MASK;

    if (index != 0) {
        orig_index = index;
        ptr = rdxtree_node_find(iter->node, &index);

        if (ptr != NULL) {
            iter->key += (index - orig_index) + 1;
            return ptr;
        }
    }

    return rdxtree_walk_next(tree, iter);
}

void
rdxtree_remove_all(struct rdxtree *tree)
{
    struct rdxtree_node *node, *parent;
    struct rdxtree_iter iter;

    if (tree->height == 0) {
        if (tree->root != NULL) {
            rcu_store_ptr(tree->root, NULL);
        }

        return;
    }

    for (;;) {
        rdxtree_iter_init(&iter);
        rdxtree_walk_next(tree, &iter);

        if (iter.node == NULL) {
            break;
        }

        node = iter.node;
        parent = node->parent;

        if (parent == NULL) {
            rdxtree_init(tree, tree->flags);
        } else {
            if (rdxtree_key_alloc_enabled(tree)) {
                rdxtree_remove_bm_set(parent, node->index);
            }

            rdxtree_node_remove(parent, node->index);
            rdxtree_cleanup(tree, parent);
            node->parent = NULL;
        }

        rdxtree_node_schedule_destruction(node);
    }
}

static int __init
rdxtree_setup(void)
{
    kmem_cache_init(&rdxtree_node_cache, "rdxtree_node",
                    sizeof(struct rdxtree_node), 0,
                    rdxtree_node_ctor, KMEM_CACHE_PAGE_ONLY);
    return 0;
}

INIT_OP_DEFINE(rdxtree_setup,
               INIT_OP_DEP(kmem_bootstrap, true),
               INIT_OP_DEP(rcu_bootstrap, true));


code:
/*
 * Copyright (c) 2011-2018 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Upstream site with license notes :
 * http://git.sceen.net/rbraun/librbraun.git/
 *
 *
 * Radix tree.
 *
 * In addition to the standard insertion operation, this implementation can
 * allocate keys for the caller at insertion time. It also allows lookups to
 * occur concurrently with updates using RCU.
 */

#ifndef KERN_RDXTREE_H
#define KERN_RDXTREE_H

#include <assert.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>

#include <kern/init.h>
#include <kern/rcu.h>

typedef uint64_t rdxtree_key_t;

/*
 * Radix tree initialization flags.
 */
#define RDXTREE_KEY_ALLOC 0x1 /* Enable key allocation */

/*
 * Radix tree.
 */
struct rdxtree;

/*
 * Radix tree iterator.
 */
struct rdxtree_iter;

/*
 * Static tree initializer.
 */
#define RDXTREE_INITIALIZER { 0, NULL }

#include <kern/rdxtree_i.h>

/*
 * Initialize a tree.
 */
static inline void
rdxtree_init(struct rdxtree *tree, unsigned short flags)
{
    assert((flags & ~RDXTREE_KEY_ALLOC) == 0);

    tree->height = 0;
    tree->flags = flags;
    tree->root = NULL;
}

/*
 * Insert a pointer in a tree.
 *
 * The ptr parameter must not be NULL.
 */
static inline int
rdxtree_insert(struct rdxtree *tree, rdxtree_key_t key, void *ptr)
{
    return rdxtree_insert_common(tree, key, ptr, NULL);
}

/*
 * Insert a pointer in a tree and obtain its slot.
 *
 * The ptr and slotp parameters must not be NULL. If successful, the slot of
 * the newly inserted pointer is stored at the address pointed to by the slotp
 * parameter.
 */
static inline int
rdxtree_insert_slot(struct rdxtree *tree, rdxtree_key_t key,
                    void *ptr, void ***slotp)
{
    return rdxtree_insert_common(tree, key, ptr, slotp);
}

/*
 * Insert a pointer in a tree, for which a new key is allocated.
 *
 * The ptr and keyp parameters must not be NULL. The newly allocated key is
 * stored at the address pointed to by the keyp parameter.
 */
static inline int
rdxtree_insert_alloc(struct rdxtree *tree, void *ptr, rdxtree_key_t *keyp)
{
    return rdxtree_insert_alloc_common(tree, ptr, keyp, NULL);
}

/*
 * Insert a pointer in a tree, for which a new key is allocated, and obtain
 * its slot.
 *
 * The ptr, keyp and slotp parameters must not be NULL. The newly allocated
 * key is stored at the address pointed to by the keyp parameter while the
 * slot of the inserted pointer is stored at the address pointed to by the
 * slotp parameter.
 */
static inline int
rdxtree_insert_alloc_slot(struct rdxtree *tree, void *ptr,
                          rdxtree_key_t *keyp, void ***slotp)
{
    return rdxtree_insert_alloc_common(tree, ptr, keyp, slotp);
}

/*
 * Remove a pointer from a tree.
 *
 * The matching pointer is returned if successful, NULL otherwise.
 */
void * rdxtree_remove(struct rdxtree *tree, rdxtree_key_t key);

/*
 * Look up a pointer in a tree.
 *
 * The matching pointer is returned if successful, NULL otherwise.
 */
static inline void *
rdxtree_lookup(const struct rdxtree *tree, rdxtree_key_t key)
{
    return rdxtree_lookup_common(tree, key, false);
}

/*
 * Look up a slot in a tree.
 *
 * A slot is a pointer to a stored pointer in a tree. It can be used as
 * a placeholder for fast replacements to avoid multiple lookups on the same
 * key.
 *
 * A slot for the matching pointer is returned if successful, NULL otherwise.
 *
 * See rdxtree_replace_slot().
 */
static inline void **
rdxtree_lookup_slot(const struct rdxtree *tree, rdxtree_key_t key)
{
    return rdxtree_lookup_common(tree, key, true);
}

static inline void *
rdxtree_load_slot(void **slot)
{
    return rcu_load_ptr(*slot);
}

/*
 * Replace a pointer in a tree.
 *
 * The ptr parameter must not be NULL. The previous pointer is returned.
 *
 * See rdxtree_lookup_slot().
 */
void * rdxtree_replace_slot(void **slot, void *ptr);

/*
 * Forge a loop to process all pointers of a tree.
 *
 * It is not safe to modify a tree from such a loop.
 */
#define rdxtree_for_each(tree, iter, ptr)                       \
for (rdxtree_iter_init(iter), ptr = rdxtree_walk(tree, iter);   \
     ptr != NULL;                                               \
     ptr = rdxtree_walk(tree, iter))

/*
 * Return the key of the current pointer from an iterator.
 */
static inline rdxtree_key_t
rdxtree_iter_key(const struct rdxtree_iter *iter)
{
    return iter->key;
}

/*
 * Remove all pointers from a tree.
 *
 * The common way to destroy a tree and its pointers is to loop over all
 * the pointers using rdxtree_for_each(), freeing them, then call this
 * function.
 */
void rdxtree_remove_all(struct rdxtree *tree);

/*
 * This init operation provides :
 *  - module fully initialized
 */
INIT_OP_DECLARE(rdxtree_setup);

#endif /* KERN_RDXTREE_H */


code:
/*
 * Copyright (c) 2011-2018 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Upstream site with license notes :
 * http://git.sceen.net/rbraun/librbraun.git/
 */

#ifndef KERN_RDXTREE_I_H
#define KERN_RDXTREE_I_H

#include <stdbool.h>
#include <stddef.h>

/*
 * Radix tree.
 */
struct rdxtree {
    unsigned short height;
    unsigned short flags;
    void *root;
};

/*
 * Radix tree iterator.
 *
 * The node member refers to the node containing the current pointer, if any.
 * The key member refers to the current pointer, and is valid if and only if
 * rdxtree_walk() has been called at least once on the iterator.
 */
struct rdxtree_iter {
    void *node;
    rdxtree_key_t key;
};

/*
 * Initialize an iterator.
 */
static inline void
rdxtree_iter_init(struct rdxtree_iter *iter)
{
    iter->node = NULL;
    iter->key = (rdxtree_key_t)-1;
}

int rdxtree_insert_common(struct rdxtree *tree, rdxtree_key_t key,
                          void *ptr, void ***slotp);

int rdxtree_insert_alloc_common(struct rdxtree *tree, void *ptr,
                                rdxtree_key_t *keyp, void ***slotp);

void * rdxtree_lookup_common(const struct rdxtree *tree, rdxtree_key_t key,
                             bool get_slot);

void * rdxtree_walk(struct rdxtree *tree, struct rdxtree_iter *iter);

#endif /* KERN_RDXTREE_I_H */


code:
/*
 * Copyright (c) 2017 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <assert.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>

#include <kern/atomic.h>
#include <kern/init.h>
#include <kern/macros.h>
#include <kern/rtmutex.h>
#include <kern/rtmutex_i.h>
#include <kern/rtmutex_types.h>
#include <kern/syscnt.h>
#include <kern/thread.h>
#include <kern/turnstile.h>

#ifdef CONFIG_MUTEX_DEBUG

enum {
    RTMUTEX_SC_WAIT_SUCCESSES,
    RTMUTEX_SC_WAIT_ERRORS,
    RTMUTEX_SC_DOWNGRADES,
    RTMUTEX_SC_ERROR_DOWNGRADES,
    RTMUTEX_SC_CANCELED_DOWNGRADES,
    RTMUTEX_NR_SCS
};

static struct syscnt rtmutex_sc_array[RTMUTEX_NR_SCS];

static void
rtmutex_register_sc(unsigned int index, const char *name)
{
    assert(index < ARRAY_SIZE(rtmutex_sc_array));
    syscnt_register(&rtmutex_sc_array[index], name);
}

static void
rtmutex_setup_debug(void)
{
    rtmutex_register_sc(RTMUTEX_SC_WAIT_SUCCESSES,
                        "rtmutex_wait_successes");
    rtmutex_register_sc(RTMUTEX_SC_WAIT_ERRORS,
                        "rtmutex_wait_errors");
    rtmutex_register_sc(RTMUTEX_SC_DOWNGRADES,
                        "rtmutex_downgrades");
    rtmutex_register_sc(RTMUTEX_SC_ERROR_DOWNGRADES,
                        "rtmutex_error_downgrades");
    rtmutex_register_sc(RTMUTEX_SC_CANCELED_DOWNGRADES,
                        "rtmutex_canceled_downgrades");
}

static void
rtmutex_inc_sc(unsigned int index)
{
    assert(index < ARRAY_SIZE(rtmutex_sc_array));
    syscnt_inc(&rtmutex_sc_array[index]);
}

#else /* CONFIG_MUTEX_DEBUG */
#define rtmutex_setup_debug()
#define rtmutex_inc_sc(x)
#endif /* CONFIG_MUTEX_DEBUG */

static struct thread *
rtmutex_get_thread(uintptr_t owner)
{
    return (struct thread *)(owner & RTMUTEX_OWNER_MASK);
}

static void
rtmutex_set_contended(struct rtmutex *rtmutex)
{
    atomic_or(&rtmutex->owner, RTMUTEX_CONTENDED, ATOMIC_RELEASE);
}

static int
rtmutex_lock_slow_common(struct rtmutex *rtmutex, bool timed, uint64_t ticks)
{
    struct turnstile *turnstile;
    uintptr_t self, owner;
    struct thread *thread;
    uintptr_t bits;
    int error;

    error = 0;
    self = (uintptr_t)thread_self();

    turnstile = turnstile_lend(rtmutex);

    rtmutex_set_contended(rtmutex);

    bits = RTMUTEX_CONTENDED;

    for (;;) {
        owner = atomic_cas(&rtmutex->owner, bits, self | bits, ATOMIC_ACQUIRE);
        assert((owner & bits) == bits);

        if (owner == bits) {
            break;
        }

        thread = rtmutex_get_thread(owner);

        if (!timed) {
            turnstile_wait(turnstile, "rtmutex", thread);
        } else {
            error = turnstile_timedwait(turnstile, "rtmutex", thread, ticks);

            if (error) {
                break;
            }
        }

        bits |= RTMUTEX_FORCE_WAIT;
    }

    if (error) {
        rtmutex_inc_sc(RTMUTEX_SC_WAIT_ERRORS);

        /*
         * Keep in mind more than one thread may have timed out on waiting.
         * These threads aren't considered waiters, making the turnstile
         * empty. The first to reacquire the turnstile clears the contention
         * bits, allowing the owner to unlock through the fast path.
         */
        if (turnstile_empty(turnstile)) {
            owner = atomic_load(&rtmutex->owner, ATOMIC_RELAXED);

            if (owner & RTMUTEX_CONTENDED) {
                rtmutex_inc_sc(RTMUTEX_SC_ERROR_DOWNGRADES);
                owner &= RTMUTEX_OWNER_MASK;
                atomic_store(&rtmutex->owner, owner, ATOMIC_RELAXED);
            } else {
                rtmutex_inc_sc(RTMUTEX_SC_CANCELED_DOWNGRADES);
            }
        }

        goto out;
    }

    rtmutex_inc_sc(RTMUTEX_SC_WAIT_SUCCESSES);

    turnstile_own(turnstile);

    if (turnstile_empty(turnstile)) {
        rtmutex_inc_sc(RTMUTEX_SC_DOWNGRADES);
        owner = atomic_swap(&rtmutex->owner, self, ATOMIC_RELAXED);
        assert(owner == (self | bits));
    }

out:
    turnstile_return(turnstile);

    /*
     * A lock owner should never perform priority propagation on itself,
     * because this process is done using its own priority, potentially
     * introducing unbounded priority inversion.
     * Instead, let new waiters do it, using their own priority.
     */

    return error;
}

void
rtmutex_lock_slow(struct rtmutex *rtmutex)
{
    int error;

    error = rtmutex_lock_slow_common(rtmutex, false, 0);
    assert(!error);
}

int
rtmutex_timedlock_slow(struct rtmutex *rtmutex, uint64_t ticks)
{
    return rtmutex_lock_slow_common(rtmutex, true, ticks);
}

void
rtmutex_unlock_slow(struct rtmutex *rtmutex)
{
    struct turnstile *turnstile;
    uintptr_t owner;

    for (;;) {
        turnstile = turnstile_acquire(rtmutex);

        if (turnstile != NULL) {
            break;
        }

        owner = rtmutex_unlock_fast(rtmutex);

        if (!(owner & RTMUTEX_CONTENDED)) {
            goto out;
        }
    }

    owner = atomic_swap(&rtmutex->owner,
                        RTMUTEX_FORCE_WAIT | RTMUTEX_CONTENDED,
                        ATOMIC_RELEASE);
    assert(rtmutex_get_thread(owner) == thread_self());

    turnstile_disown(turnstile);
    turnstile_signal(turnstile);

    turnstile_release(turnstile);

out:
    thread_propagate_priority();
}

static int
rtmutex_bootstrap(void)
{
    return 0;
}

INIT_OP_DEFINE(rtmutex_bootstrap,
               INIT_OP_DEP(thread_setup_booter, true));

static int
rtmutex_setup(void)
{
    rtmutex_setup_debug();
    return 0;
}

#ifdef CONFIG_MUTEX_DEBUG
#define RTMUTEX_DEBUG_INIT_OPS                  \
               INIT_OP_DEP(syscnt_setup, true),
#else /* CONFIG_MUTEX_DEBUG */
#define RTMUTEX_DEBUG_INIT_OPS
#endif /* CONFIG_MUTEX_DEBUG */

INIT_OP_DEFINE(rtmutex_setup,
               INIT_OP_DEP(rtmutex_bootstrap, true),
               RTMUTEX_DEBUG_INIT_OPS
);


code:
/*
 * Copyright (c) 2017 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 * Real-time mutual exclusion locks.
 *
 * A real-time mutex is similar to a regular mutex, except priority
 * inheritance is unconditionally enabled.
 */

#ifndef KERN_RTMUTEX_H
#define KERN_RTMUTEX_H

#include <assert.h>
#include <errno.h>
#include <stdbool.h>
#include <stdint.h>

#include <kern/atomic.h>
#include <kern/init.h>
#include <kern/macros.h>
#include <kern/rtmutex_i.h>
#include <kern/rtmutex_types.h>

struct rtmutex;

static inline bool
rtmutex_locked(const struct rtmutex *rtmutex)
{
    uintptr_t owner;

    owner = atomic_load(&rtmutex->owner, ATOMIC_RELAXED);
    return (owner != 0);
}

/*
 * Initialize a real-time mutex.
 */
static inline void
rtmutex_init(struct rtmutex *rtmutex)
{
    rtmutex->owner = 0;
}

/*
 * Attempt to lock the given real-time mutex.
 *
 * This function may not sleep.
 *
 * Return 0 on success, EBUSY if the mutex is already locked.
 */
static inline int
rtmutex_trylock(struct rtmutex *rtmutex)
{
    uintptr_t prev_owner;

    prev_owner = rtmutex_lock_fast(rtmutex);

    if (unlikely(prev_owner != 0)) {
        return EBUSY;
    }

    return 0;
}

/*
 * Lock a real-time mutex.
 *
 * If the mutex is already locked, the calling thread sleeps until the
 * mutex is unlocked, and its priority is propagated as needed to prevent
 * unbounded priority inversion.
 *
 * A mutex can only be locked once.
 *
 * This function may sleep.
 */
static inline void
rtmutex_lock(struct rtmutex *rtmutex)
{
    uintptr_t prev_owner;

    prev_owner = rtmutex_lock_fast(rtmutex);

    if (unlikely(prev_owner != 0)) {
        rtmutex_lock_slow(rtmutex);
    }
}

/*
 * Lock a real-time mutex, with a time boundary.
 *
 * The time boundary is an absolute time in ticks.
 *
 * If successful, the mutex is locked, otherwise an error is returned.
 * A mutex can only be locked once.
 *
 * This function may sleep.
 */
static inline int
rtmutex_timedlock(struct rtmutex *rtmutex, uint64_t ticks)
{
    uintptr_t prev_owner;

    prev_owner = rtmutex_lock_fast(rtmutex);

    if (unlikely(prev_owner != 0)) {
        return rtmutex_timedlock_slow(rtmutex, ticks);
    }

    return 0;
}

/*
 * Unlock a real-time mutex.
 *
 * The mutex must be locked, and must have been locked by the calling
 * thread.
 */
static inline void
rtmutex_unlock(struct rtmutex *rtmutex)
{
    uintptr_t prev_owner;

    prev_owner = rtmutex_unlock_fast(rtmutex);

    if (unlikely(prev_owner & RTMUTEX_CONTENDED)) {
        rtmutex_unlock_slow(rtmutex);
    }
}

/*
 * Mutex init operations. See kern/mutex.h.
 */
INIT_OP_DECLARE(rtmutex_bootstrap);
INIT_OP_DECLARE(rtmutex_setup);

#endif /* KERN_RTMUTEX_H */


code:
/*
 * Copyright (c) 2017 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef KERN_RTMUTEX_I_H
#define KERN_RTMUTEX_I_H

#include <assert.h>
#include <stdbool.h>
#include <stdint.h>

#include <kern/atomic.h>
#include <kern/rtmutex_types.h>
#include <kern/thread.h>

/*
 * Real-time mutex flags.
 *
 * The "contended" flag indicates that threads are waiting for the mutex
 * to be unlocked. It forces threads trying to lock the mutex as well as
 * the owner to take the slow path.
 *
 * The "force-wait" flag prevents "stealing" a mutex. When a contended
 * mutex is unlocked, a thread may concurrently try to lock it. Without
 * this flag, it may succeed, and in doing so, it would prevent a
 * potentially higher priority thread from locking the mutex. The flag
 * forces all threads to not only take the slow path, but to also call
 * the turnstile wait function so that only the highest priority thread
 * may lock the mutex.
 */
#define RTMUTEX_CONTENDED   ((uintptr_t)0x1)
#define RTMUTEX_FORCE_WAIT  ((uintptr_t)0x2)

#define RTMUTEX_OWNER_MASK  (~((uintptr_t)(RTMUTEX_FORCE_WAIT \
                                           | RTMUTEX_CONTENDED)))

static inline bool
rtmutex_owner_aligned(uintptr_t owner)
{
    return (((owner) & ~RTMUTEX_OWNER_MASK) == 0);
}

static inline uintptr_t
rtmutex_lock_fast(struct rtmutex *rtmutex)
{
    uintptr_t owner;

    owner = (uintptr_t)thread_self();
    assert(rtmutex_owner_aligned(owner));
    return atomic_cas(&rtmutex->owner, 0, owner, ATOMIC_ACQUIRE);
}

static inline uintptr_t
rtmutex_unlock_fast(struct rtmutex *rtmutex)
{
    uintptr_t owner, prev_owner;

    owner = (uintptr_t)thread_self();
    assert(rtmutex_owner_aligned(owner));
    prev_owner = atomic_cas(&rtmutex->owner, owner, 0, ATOMIC_RELEASE);
    assert((prev_owner & RTMUTEX_OWNER_MASK) == owner);
    return prev_owner;
}

void rtmutex_lock_slow(struct rtmutex *rtmutex);

int rtmutex_timedlock_slow(struct rtmutex *rtmutex, uint64_t ticks);

void rtmutex_unlock_slow(struct rtmutex *rtmutex);

#endif /* KERN_RTMUTEX_I_H */


code:
/*
 * Copyright (c) 2017 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 * Isolated type definition used to avoid inclusion circular dependencies.
 */

#ifndef KERN_RTMUTEX_TYPES_H
#define KERN_RTMUTEX_TYPES_H

#include <stdint.h>

struct rtmutex {
    uintptr_t owner;
};

#endif /* KERN_RTMUTEX_TYPES_H */


code:
/*
 * Copyright (c) 2017 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <assert.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>

#include <kern/semaphore.h>
#include <kern/semaphore_i.h>
#include <kern/sleepq.h>

static int
semaphore_wait_slow_common(struct semaphore *semaphore,
                           bool timed, uint64_t ticks)
{
    struct sleepq *sleepq;
    unsigned long flags;
    unsigned int prev;
    int error;

    error = 0;

    sleepq = sleepq_lend_intr_save(semaphore, false, &flags);

    for (;;) {
        prev = semaphore_dec(semaphore);

        if (prev != 0) {
            break;
        }

        if (!timed) {
            sleepq_wait(sleepq, "sem");
        } else {
            error = sleepq_timedwait(sleepq, "sem", ticks);

            if (error) {
                break;
            }
        }
    }

    sleepq_return_intr_restore(sleepq, flags);

    return error;
}

void
semaphore_wait_slow(struct semaphore *semaphore)
{
    int error;

    error = semaphore_wait_slow_common(semaphore, false, 0);
    assert(!error);
}

int
semaphore_timedwait_slow(struct semaphore *semaphore, uint64_t ticks)
{
    return semaphore_wait_slow_common(semaphore, true, ticks);
}

void
semaphore_post_slow(struct semaphore *semaphore)
{
    struct sleepq *sleepq;
    unsigned long flags;

    sleepq = sleepq_acquire_intr_save(semaphore, false, &flags);

    if (sleepq == NULL) {
        return;
    }

    sleepq_signal(sleepq);

    sleepq_release_intr_restore(sleepq, flags);
}


code:
/*
 * Copyright (c) 2017 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 * Semaphores are resource-counting sleeping synchronization objects.
 * They are used to synchronize access to resources and signal events.
 *
 * The main operations supported by semaphores are waiting and signalling.
 * A semaphore is implemented as an atomic integer with an initial value.
 * Waiting on a semaphore means decrementing that integer, whereas signalling
 * means incrementing it. Waiting can only succeed if the semaphore value
 * is strictly greater than 0.
 *
 * The use of semaphores is generally discouraged. Mutexes are recommended
 * to implement preemptible critical sections, and spinlocks combined with
 * calls to thread_sleep() and thread_wakeup() are recommended for
 * non-preemptible critical sections. The reason is that a semaphore
 * internally already uses a spinlock, but that internal lock may not be
 * used to serialize access to anything else. This means that the only case
 * where a semaphore may be an efficient synchronization mechanism is
 * real-time signalling, e.g. an interrupt handler signalling a thread.
 * Here, "real-time" means that there is a guarantee that the thread has
 * always consumed the data produced by the interrupt handler before the
 * latter runs again.
 *
 * Since the kernel is an incomplete program without applications, it is
 * impossible to perform an analysis providing the real-time guarantee.
 * As a result, semaphores may only be used by application code.
 */

#ifndef KERN_SEMAPHORE_H
#define KERN_SEMAPHORE_H

#include <assert.h>
#include <errno.h>
#include <stdint.h>

#include <kern/atomic.h>

#define SEMAPHORE_VALUE_MAX 32768

#include <kern/semaphore_i.h>

struct semaphore;

/*
 * Initialize a semaphore.
 */
static inline void
semaphore_init(struct semaphore *semaphore, unsigned int value)
{
    assert(value <= SEMAPHORE_VALUE_MAX);
    semaphore->value = value;
}

/*
 * Attempt to decrement a semaphore.
 *
 * This function may not sleep.
 *
 * Return 0 on success, EAGAIN if the semaphore could not be decremented.
 */
static inline int
semaphore_trywait(struct semaphore *semaphore)
{
    unsigned int prev;

    prev = semaphore_dec(semaphore);

    if (prev == 0) {
        return EAGAIN;
    }

    return 0;
}

/*
 * Wait on a semaphore.
 *
 * If the semaphore value cannot be decremented, the calling thread sleeps
 * until the semaphore value is incremented.
 */
static inline void
semaphore_wait(struct semaphore *semaphore)
{
    unsigned int prev;

    prev = semaphore_dec(semaphore);

    if (prev == 0) {
        semaphore_wait_slow(semaphore);
    }
}

/*
 * Wait on a semaphore, with a time boundary.
 *
 * The time boundary is an absolute time in ticks.
 *
 * If successful, the semaphore is decremented, otherwise an error is returned.
 */
static inline int
semaphore_timedwait(struct semaphore *semaphore, uint64_t ticks)
{
    unsigned int prev;

    prev = semaphore_dec(semaphore);

    if (prev == 0) {
        return semaphore_timedwait_slow(semaphore, ticks);
    }

    return 0;
}

/*
 * Signal a semaphore.
 *
 * If the semaphore value becomes strictly greater than 0, a thread waiting
 * on the semaphore is awaken.
 *
 * A semaphore may be signalled from interrupt context.
 */
static inline void
semaphore_post(struct semaphore *semaphore)
{
    unsigned int prev;

    prev = semaphore_inc(semaphore);

    if (prev == 0) {
        semaphore_post_slow(semaphore);
    }
}

/*
 * Get the value of a semaphore.
 */
static inline unsigned int
semaphore_getvalue(const struct semaphore *semaphore)
{
    return atomic_load(&semaphore->value, ATOMIC_RELAXED);
}

#endif /* KERN_SEMAPHORE_H */


code:
/*
 * Copyright (c) 2017 Richard Braun.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef KERN_SEMAPHORE_I_H
#define KERN_SEMAPHORE_I_H

#include <assert.h>
#include <stdint.h>

#include <kern/atomic.h>

struct semaphore {
    unsigned int value;
};

static inline unsigned int
semaphore_dec(struct semaphore *semaphore)
{
    unsigned int prev, value;

    do {
        value = atomic_load(&semaphore->value, ATOMIC_RELAXED);

        if (value == 0) {
            break;
        }

        prev = atomic_cas(&semaphore->value, value, value - 1, ATOMIC_ACQUIRE);
    } while (prev != value);

    return value;
}

static inline unsigned int
semaphore_inc(struct semaphore *semaphore)
{
    unsigned int prev;

    prev = atomic_fetch_add(&semaphore->value, 1, ATOMIC_RELEASE);
    assert(prev != SEMAPHORE_VALUE_MAX);
    return prev;
}

void semaphore_wait_slow(struct semaphore *semaphore);

int semaphore_timedwait_slow(struct semaphore *semaphore, uint64_t ticks);

void semaphore_post_slow(struct semaphore *semaphore);

#endif /* KERN_SEMAPHORE_I_H */