code:
/*
 * Copyright (c) 2017-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_THREAD_I_H
#define KERN_THREAD_I_H

#include <stdalign.h>
#include <stdbool.h>

#include <kern/atomic.h>
#include <kern/cpumap.h>
#include <kern/list_types.h>
#include <kern/perfmon_types.h>
#include <kern/rcu_types.h>
#include <kern/spinlock_types.h>
#include <kern/turnstile_types.h>
#include <machine/cpu.h>
#include <machine/tcb.h>

/*
 * Forward declarations.
 */
struct sleepq;

struct thread_runq;
struct thread_fs_runq;

/*
 * Thread flags.
 */
#define THREAD_YIELD    0x1UL /* Must yield the processor ASAP */
#define THREAD_DETACHED 0x2UL /* Resources automatically released on exit */

/*
 * Scheduling data for a real-time thread.
 */
struct thread_rt_data {
    struct list node;
    unsigned short time_slice;
};

/*
 * Scheduling data for a fair-scheduling thread.
 */
struct thread_fs_data {
    struct list group_node;
    struct list runq_node;
    struct thread_fs_runq *fs_runq;
    unsigned long round;
    unsigned short weight;
    unsigned short work;
};

/*
 * Thread structure.
 *
 * Threads don't have their own lock. Instead, the associated run queue
 * lock is used for synchronization. A number of members are thread-local
 * and require no synchronization. Others must be accessed with atomic
 * instructions.
 *
 * Locking keys :
 * (r) run queue
 * (t) turnstile_td
 * (T) task
 * (j) join_lock
 * (a) atomic
 * (-) thread-local
 * ( ) read-only
 *
 * (*) The runq member is used to determine which run queue lock must be
 * held to serialize access to the relevant members. However, it is only
 * updated while the associated run queue is locked. As a result, atomic
 * reads are only necessary outside critical sections.
 */
struct thread {
    alignas(CPU_L1_SIZE) struct tcb tcb; /* (r) */

    unsigned long nr_refs;  /* (a) */
    unsigned long flags;    /* (a) */

    /* Sleep/wake-up synchronization members */
    struct thread_runq *runq;   /* (r,*) */
    bool in_runq;               /* (r)   */
    const void *wchan_addr;     /* (r)   */
    const char *wchan_desc;     /* (r)   */
    int wakeup_error;           /* (r)   */
    unsigned int state;         /* (a,r) */

    /* Sleep queue available for lending */
    struct sleepq *priv_sleepq; /* (-) */

    /* Turnstile available for lending */
    struct turnstile *priv_turnstile;   /* (-) */

    struct turnstile_td turnstile_td;   /* (t) */

    /* True if priority must be propagated when preemption is reenabled */
    bool propagate_priority;    /* (-) */

    /* Preemption level, preemption is enabled if 0 */
    unsigned short preempt_level;   /* (-) */

    /* Pin level, migration is allowed if 0 */
    unsigned short pin_level;   /* (-) */

    /* Interrupt level, in thread context if 0 */
    unsigned short intr_level;  /* (-) */

    /* RCU per-thread data */
    struct rcu_reader rcu_reader;   /* (-) */

    /* Processors on which this thread is allowed to run */
    struct cpumap cpumap;   /* (r) */

    struct thread_sched_data user_sched_data;   /* (r,t) */
    struct thread_sched_data real_sched_data;   /* (r,t) */

    /*
     * True if the real scheduling data are not the user scheduling data.
     *
     * Note that it doesn't provide any information about priority inheritance.
     * A thread may be part of a priority inheritance chain without its
     * priority being boosted.
     */
    bool boosted;   /* (r,t) */

    /* True if the thread is marked to suspend */
    bool suspend;   /* (r) */

    union {
        struct thread_rt_data rt_data;  /* (r) */
        struct thread_fs_data fs_data;  /* (r) */
    };

    /*
     * Thread-specific data.
     *
     * TSD are reserved for application use.
     */
#if CONFIG_THREAD_MAX_TSD_KEYS != 0
    void *tsd[CONFIG_THREAD_MAX_TSD_KEYS];
#endif /* CONFIG_THREAD_MAX_TSD_KEYS != 0 */

    /*
     * Members related to termination.
     *
     * The termination protocol is made of two steps :
     *  1/ The thread exits, thereby releasing its self reference, and
     *     sets its state to dead before calling the scheduler.
     *  2/ Another thread must either already be joining, or join later.
     *     When the thread reference counter drops to zero, the terminating
     *     flag is set, and the joining thread is awaken, if any. After that,
     *     the join operation polls the state until it sees the target thread
     *     as dead, and then releases its resources.
     */
    struct thread *join_waiter;     /* (j) */
    struct spinlock join_lock;
    bool terminating;               /* (j) */

    struct task *task;              /* (T) */
    struct list task_node;          /* (T) */
    void *stack;                    /* (-) */
    char name[THREAD_NAME_SIZE];    /* ( ) */

#ifdef CONFIG_PERFMON
    struct perfmon_td perfmon_td;   /* ( ) */
#endif
};

#define THREAD_ATTR_DETACHED 0x1

void thread_terminate(struct thread *thread);

/*
 * Flag access functions.
 */

static inline void
thread_set_flag(struct thread *thread, unsigned long flag)
{
    atomic_or(&thread->flags, flag, ATOMIC_RELEASE);
}

static inline void
thread_clear_flag(struct thread *thread, unsigned long flag)
{
    atomic_and(&thread->flags, ~flag, ATOMIC_RELEASE);
}

static inline int
thread_test_flag(struct thread *thread, unsigned long flag)
{
    return (atomic_load(&thread->flags, ATOMIC_ACQUIRE) & flag) != 0;
}

#endif /* KERN_THREAD_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/>.
 *
 *
 * This implementation is based on "Hashed and Hierarchical Timing Wheels:
 * Efficient Data Structures for Implementing a Timer Facility" by George
 * Varghese and Tony Lauck. Specifically, it implements scheme 6.1.2.
 *
 * TODO Analyse hash parameters.
 */

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

#include <kern/atomic.h>
#include <kern/clock.h>
#include <kern/error.h>
#include <kern/init.h>
#include <kern/hlist.h>
#include <kern/macros.h>
#include <kern/panic.h>
#include <kern/percpu.h>
#include <kern/spinlock.h>
#include <kern/thread.h>
#include <kern/timer.h>
#include <kern/timer_i.h>
#include <kern/work.h>
#include <machine/boot.h>
#include <machine/cpu.h>

/*
 * Timer states.
 */
#define TIMER_TS_READY          1
#define TIMER_TS_SCHEDULED      2
#define TIMER_TS_RUNNING        3
#define TIMER_TS_DONE           4

/*
 * Timer flags.
 */
#define TIMER_TF_DETACHED       0x1
#define TIMER_TF_INTR           0x2
#define TIMER_TF_HIGH_PRIO      0x4
#define TIMER_TF_CANCELED       0x8

#define TIMER_INVALID_CPU ((unsigned int)-1)

#define TIMER_HTABLE_SIZE 2048

#if !ISP2(TIMER_HTABLE_SIZE)
#error "hash table size must be a power of two"
#endif /* !ISP2(TIMER_HTABLE_SIZE) */

#define TIMER_HTABLE_MASK (TIMER_HTABLE_SIZE - 1)

struct timer_bucket {
    struct hlist timers;
};

/*
 * The hash table bucket matching the last time member has already been
 * processed, and the next periodic event resumes from the next bucket.
 *
 * Locking order: interrupts -> timer_cpu_data.
 */
struct timer_cpu_data {
    unsigned int cpu;
    struct spinlock lock;
    uint64_t last_time;
    struct timer_bucket htable[TIMER_HTABLE_SIZE];
};

static struct timer_cpu_data timer_cpu_data __percpu;

static struct timer_cpu_data *
timer_cpu_data_acquire(unsigned long *flags)
{
    struct timer_cpu_data *cpu_data;

    thread_preempt_disable();
    cpu_data = cpu_local_ptr(timer_cpu_data);
    spinlock_lock_intr_save(&cpu_data->lock, flags);
    thread_preempt_enable_no_resched();

    return cpu_data;
}

static struct timer_cpu_data *
timer_lock_cpu_data(struct timer *timer, unsigned long *flags)
{
    struct timer_cpu_data *cpu_data;
    unsigned int cpu;

    for (;;) {
        cpu = atomic_load(&timer->cpu, ATOMIC_RELAXED);

        if (cpu == TIMER_INVALID_CPU) {
            return NULL;
        }

        cpu_data = percpu_ptr(timer_cpu_data, cpu);

        spinlock_lock_intr_save(&cpu_data->lock, flags);

        if (cpu == atomic_load(&timer->cpu, ATOMIC_RELAXED)) {
            return cpu_data;
        }

        spinlock_unlock_intr_restore(&cpu_data->lock, *flags);
    }
}

static void
timer_unlock_cpu_data(struct timer_cpu_data *cpu_data, unsigned long flags)
{
    spinlock_unlock_intr_restore(&cpu_data->lock, flags);
}

/*
 * Timer state functions.
 */

static bool
timer_ready(const struct timer *timer)
{
    return timer->state == TIMER_TS_READY;
}

static void
timer_set_ready(struct timer *timer)
{
    timer->state = TIMER_TS_READY;
}

static bool
timer_scheduled(const struct timer *timer)
{
    return timer->state == TIMER_TS_SCHEDULED;
}

static void
timer_set_scheduled(struct timer *timer, unsigned int cpu)
{
    atomic_store(&timer->cpu, cpu, ATOMIC_RELAXED);
    timer->state = TIMER_TS_SCHEDULED;
}

static bool
timer_running(const struct timer *timer)
{
    return timer->state == TIMER_TS_RUNNING;
}

static void
timer_set_running(struct timer *timer)
{
    timer->state = TIMER_TS_RUNNING;
}

static bool
timer_done(const struct timer *timer)
{
    return timer->state == TIMER_TS_DONE;
}

static void
timer_set_done(struct timer *timer)
{
    timer->state = TIMER_TS_DONE;
}

/*
 * Timer flags functions.
 */

static bool
timer_detached(const struct timer *timer)
{
    return timer->flags & TIMER_TF_DETACHED;
}

static void
timer_set_detached(struct timer *timer)
{
    timer->flags |= TIMER_TF_DETACHED;
}

static bool
timer_is_intr(const struct timer *timer)
{
    return timer->flags & TIMER_TF_INTR;
}

static void
timer_set_intr(struct timer *timer)
{
    timer->flags |= TIMER_TF_INTR;
}

static bool
timer_is_high_prio(const struct timer *timer)
{
    return timer->flags & TIMER_TF_HIGH_PRIO;
}

static void
timer_set_high_prio(struct timer *timer)
{
    timer->flags |= TIMER_TF_HIGH_PRIO;
}

static bool
timer_canceled(const struct timer *timer)
{
    return timer->flags & TIMER_TF_CANCELED;
}

static void
timer_set_canceled(struct timer *timer)
{
    timer->flags |= TIMER_TF_CANCELED;
}

static void
timer_set_time(struct timer *timer, uint64_t ticks)
{
    timer->ticks = ticks;
}

static bool
timer_occurred(const struct timer *timer, uint64_t ref)
{
    return clock_time_occurred(timer_get_time(timer), ref);
}

static uintptr_t
timer_hash(uint64_t ticks)
{
    return ticks;
}

static void
timer_run(struct timer *timer)
{
    struct timer_cpu_data *cpu_data;
    unsigned long cpu_flags;

    assert(timer_running(timer));

    timer->fn(timer);

    if (timer_detached(timer)) {
        return;
    }

    cpu_data = timer_lock_cpu_data(timer, &cpu_flags);

    /*
     * The timer handler may have :
     *  - rescheduled itself
     *  - been canceled
     *  - none of the above
     *
     * If the handler didn't call a timer function, or if the timer was
     * canceled, set the state to done and wake up the joiner, if any.
     *
     * If the handler rescheduled the timer, nothing must be done. This
     * is also true if the timer was canceled after being rescheduled by
     * the handler (in this case, cancellation won't wait for a signal).
     * These cases can be identified by checking if the timer state is
     * different from running.
     */

    if (timer_running(timer)) {
        timer_set_done(timer);
        thread_wakeup(timer->joiner);
    }

    timer_unlock_cpu_data(cpu_data, cpu_flags);
}

static void
timer_run_work(struct work *work)
{
    struct timer *timer;

    timer = structof(work, struct timer, work);
    timer_run(timer);
}

static void
timer_process(struct timer *timer)
{
    int work_flags;

    if (timer_is_intr(timer)) {
        timer_run(timer);
        return;
    }

    if (timer_is_high_prio(timer)) {
        work_flags = TIMER_TF_HIGH_PRIO;
    } else {
        work_flags = 0;
    }

    work_init(&timer->work, timer_run_work);
    work_schedule(&timer->work, work_flags);
}

static void
timer_bucket_init(struct timer_bucket *bucket)
{
    hlist_init(&bucket->timers);
}

static void
timer_bucket_add(struct timer_bucket *bucket, struct timer *timer)
{
    hlist_insert_head(&bucket->timers, &timer->node);
}

static void
timer_bucket_remove(struct timer_bucket *bucket, struct timer *timer)
{
    (void)bucket;
    hlist_remove(&timer->node);
}

static void
timer_cpu_data_init(struct timer_cpu_data *cpu_data, unsigned int cpu)
{
    cpu_data->cpu = cpu;
    spinlock_init(&cpu_data->lock);

    /* See periodic event handling */
    cpu_data->last_time = clock_get_time() - 1;

    for (size_t i = 0; i < ARRAY_SIZE(cpu_data->htable); i++) {
        timer_bucket_init(&cpu_data->htable[i]);
    }
}

static struct timer_bucket *
timer_cpu_data_get_bucket(struct timer_cpu_data *cpu_data, uint64_t ticks)
{
    uintptr_t index;

    index = timer_hash(ticks) & TIMER_HTABLE_MASK;
    assert(index < ARRAY_SIZE(cpu_data->htable));
    return &cpu_data->htable[index];
}

static void
timer_cpu_data_add(struct timer_cpu_data *cpu_data, struct timer *timer)
{
    struct timer_bucket *bucket;

    assert(timer_ready(timer));

    bucket = timer_cpu_data_get_bucket(cpu_data, timer->ticks);
    timer_bucket_add(bucket, timer);
}

static void
timer_cpu_data_remove(struct timer_cpu_data *cpu_data, struct timer *timer)
{
    struct timer_bucket *bucket;

    assert(timer_scheduled(timer));

    bucket = timer_cpu_data_get_bucket(cpu_data, timer->ticks);
    timer_bucket_remove(bucket, timer);
}

static void
timer_bucket_filter(struct timer_bucket *bucket, uint64_t now,
                    struct hlist *timers)
{
    struct timer *timer, *tmp;

    hlist_for_each_entry_safe(&bucket->timers, timer, tmp, node) {
        assert(timer_scheduled(timer));

        if (!timer_occurred(timer, now)) {
            continue;
        }

        hlist_remove(&timer->node);
        timer_set_running(timer);
        hlist_insert_head(timers, &timer->node);
    }
}

static int __init
timer_bootstrap(void)
{
    timer_cpu_data_init(cpu_local_ptr(timer_cpu_data), 0);
    return 0;
}

INIT_OP_DEFINE(timer_bootstrap,
               INIT_OP_DEP(cpu_setup, true),
               INIT_OP_DEP(spinlock_setup, true));

static int __init
timer_setup(void)
{
    for (unsigned int cpu = 1; cpu < cpu_count(); cpu++) {
        timer_cpu_data_init(percpu_ptr(timer_cpu_data, cpu), cpu);
    }

    return 0;
}

INIT_OP_DEFINE(timer_setup,
               INIT_OP_DEP(cpu_mp_probe, true),
               INIT_OP_DEP(spinlock_setup, true));

void
timer_init(struct timer *timer, timer_fn_t fn, int flags)
{
    timer->fn = fn;
    timer->cpu = TIMER_INVALID_CPU;
    timer->state = TIMER_TS_READY;
    timer->flags = 0;
    timer->joiner = NULL;

    if (flags & TIMER_DETACHED) {
        timer_set_detached(timer);
    }

    if (flags & TIMER_INTR) {
        timer_set_intr(timer);
    } else if (flags & TIMER_HIGH_PRIO) {
        timer_set_high_prio(timer);
    }
}

void
timer_schedule(struct timer *timer, uint64_t ticks)
{
    struct timer_cpu_data *cpu_data;
    unsigned long cpu_flags;

    cpu_data = timer_lock_cpu_data(timer, &cpu_flags);

    if (cpu_data == NULL) {
        cpu_data = timer_cpu_data_acquire(&cpu_flags);
    } else {
        if (timer_canceled(timer)) {
            goto out;
        }

        /*
         * If called from the handler, the timer is running. If rescheduled
         * after completion, it's done.
         */
        if (timer_running(timer) || timer_done(timer)) {
            timer_set_ready(timer);
        }
    }

    timer_set_time(timer, ticks);

    if (timer_occurred(timer, cpu_data->last_time)) {
        ticks = cpu_data->last_time + 1;
    }

    timer_cpu_data_add(cpu_data, timer);
    timer_set_scheduled(timer, cpu_data->cpu);

out:
    timer_unlock_cpu_data(cpu_data, cpu_flags);
}

void
timer_cancel(struct timer *timer)
{
    struct timer_cpu_data *cpu_data;
    unsigned long cpu_flags;

    assert(!timer_detached(timer));

    cpu_data = timer_lock_cpu_data(timer, &cpu_flags);

    assert(timer->joiner == NULL);

    timer_set_canceled(timer);

    if (timer_scheduled(timer)) {
        timer_cpu_data_remove(cpu_data, timer);
    } else {
        timer->joiner = thread_self();

        while (!timer_done(timer)) {
            if (timer_is_intr(timer)) {
                timer_unlock_cpu_data(cpu_data, cpu_flags);
                cpu_pause();
                cpu_data = timer_lock_cpu_data(timer, &cpu_flags);
            } else {
                thread_sleep(&cpu_data->lock, timer, "tmr_cncl");
            }
        }

        assert(timer_done(timer));

        timer->joiner = NULL;
    }

    timer_set_ready(timer);

    timer_unlock_cpu_data(cpu_data, cpu_flags);
}

void
timer_report_periodic_event(void)
{
    struct timer_cpu_data *cpu_data;
    struct timer_bucket *bucket;
    struct timer *timer;
    struct hlist timers;
    uint64_t ticks, now;

    assert(thread_check_intr_context());

    now = clock_get_time();
    hlist_init(&timers);
    cpu_data = cpu_local_ptr(timer_cpu_data);

    spinlock_lock(&cpu_data->lock);

    for (ticks = cpu_data->last_time + 1;
         clock_time_occurred(ticks, now);
         ticks++) {
        bucket = timer_cpu_data_get_bucket(cpu_data, ticks);
        timer_bucket_filter(bucket, now, &timers);
    }

    cpu_data->last_time = now;

    spinlock_unlock(&cpu_data->lock);

    while (!hlist_empty(&timers)) {
        timer = hlist_first_entry(&timers, struct timer, node);
        hlist_remove(&timer->node);
        timer_process(timer);
    }
}

license: mit


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/>.
 *
 *
 * Low resolution timer system.
 */

#ifndef KERN_TIMER_H
#define KERN_TIMER_H

#include <stdint.h>

#include <kern/init.h>

/*
 * Scheduling flags.
 */
#define TIMER_DETACHED      0x1     /* Timer completion isn't synchronized */
#define TIMER_INTR          0x2     /* Handler is run from interrupt context */
#define TIMER_HIGH_PRIO     0x4     /* Handler is run in high priority thread */

struct timer;

/*
 * Type for timer functions.
 */
typedef void (*timer_fn_t)(struct timer *);

#include <kern/timer_i.h>

/*
 * Return the absolute expiration time of the timer, in ticks.
 *
 * This function may not be called while another thread is scheduling the
 * timer.
 */
static inline uint64_t
timer_get_time(const struct timer *timer)
{
    return timer->ticks;
}

/*
 * Initialize a timer.
 *
 * Timers that are reponsible for releasing their own resources must
 * be detached.
 */
void timer_init(struct timer *timer, timer_fn_t fn, int flags);

/*
 * Schedule a timer.
 *
 * The time of expiration is an absolute time in ticks.
 *
 * Timers may safely be rescheduled after completion. Periodic timers are
 * implemented by rescheduling from the handler.
 *
 * If the timer has been canceled, this function does nothing. A
 * canceled timer must be reinitialized before being scheduled again.
 *
 * This function may safely be called in interrupt context.
 */
void timer_schedule(struct timer *timer, uint64_t ticks);

/*
 * Cancel a timer.
 *
 * The given timer must not be detached.
 *
 * If the timer has already expired, this function waits until the timer
 * function completes, or returns immediately if the function has already
 * completed.
 *
 * This function may safely be called from the timer handler, but not on
 * the current timer. Canceling a timer from the handler is achieved by
 * simply not rescheduling it.
 */
void timer_cancel(struct timer *timer);

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

/*
 * This init operation provides :
 *  - timer initialization and scheduling
 */
INIT_OP_DECLARE(timer_bootstrap);

#endif /* KERN_TIMER_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_TIMER_I_H
#define KERN_TIMER_I_H

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

#include <kern/hlist.h>
#include <kern/work.h>

/*
 * Locking keys :
 * (c) cpu_data
 * (a) atomic
 *
 * (*) The cpu member is used to determine which lock serializes access to
 * the structure. It must be accessed atomically, but updated while the
 * timer is locked.
 */
struct timer {
    union {
        struct hlist_node node; /* (c)      */
        struct work work;
    };

    uint64_t ticks;             /* (c)     */
    timer_fn_t fn;
    unsigned int cpu;           /* (c,a,*) */
    unsigned short state;       /* (c)     */
    unsigned short flags;       /* (c)     */
    struct thread *joiner;      /* (c)     */
};

#endif /* KERN_TIMER_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/>.
 *
 *
 * This implementation is based on "Solaris(tm) Internals: Solaris 10 and
 * OpenSolaris Kernel Architecture, Second Edition" by Richard McDougall
 * and Jim Mauro. See "Part Six: Platform Specifics, Chapter 17: Locking
 * and Synchronization, Section 7 Turnstiles and Priority Inheritance".
 *
 * The differences are outlined below.
 *
 * This implementation doesn't support read/write locks, only mutual
 * exclusion, because ownership doesn't apply well to read/write locks.
 *
 * Instead of using an external sleep queue object, this module implements
 * that functionality itself. The reasons behind this decision are :
 *  - the use of expensive priority lists used to queue threads, that
 *    a simpler sleep queue implementation shouldn't use
 *  - increased coupling with the scheduler
 *
 * Locking order : bucket (turnstile) -> turnstile_td -> thread_runq
 *
 * This order allows changing the owner of a turnstile without unlocking it
 * which is important because a turnstile is normally used to synchronize
 * access to the owner. Unlocking a turnstile would allow the owner to
 * change and make complex transient states visible. The drawback is that
 * a thread cannot be requeued atomically when its priority is changed.
 * That deferred requeue is done during priority propagation.
 *
 * TODO Analyse hash parameters.
 */

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

#include <kern/hlist.h>
#include <kern/init.h>
#include <kern/kmem.h>
#include <kern/list.h>
#include <kern/macros.h>
#include <kern/plist.h>
#include <kern/spinlock.h>
#include <kern/thread.h>
#include <kern/turnstile.h>
#include <kern/turnstile_types.h>
#include <machine/cpu.h>

/*
 * Locking keys :
 * (b) bucket
 */
struct turnstile_bucket {
    alignas(CPU_L1_SIZE) struct spinlock lock;
    struct hlist turnstiles;    /* (b) */
};

/*
 * Adding/removing waiters to/from a turnstile are performed while
 * holding both the waiter's thread data and the turnstile locks.
 *
 * Changing the owner of a turnstile and linking/unlinking the turnstile
 * into/from the owner's list of owned turnstiles are done atomically,
 * while holding both the owner's thread data and the turnstile locks.
 *
 * Locking keys :
 * (b) bucket
 * (t) turnstile_td
 *
 * (*) A turnstile is referenced in thread data after/before being
 * added/removed to/from its bucket. Referencing a turnstile in
 * thread data requires holding the thread data lock. This implies
 * that, while holding the thread data lock, if the referenced
 * turnstile isn't NULL, the bucket pointer is also not NULL and
 * stable.
 */
struct turnstile {
    struct turnstile_bucket *bucket;        /* (b*)  */
    struct hlist_node node;                 /* (b)   */
    const void *sync_obj;                   /* (b)   */
    struct plist waiters;                   /* (b,t) */
    struct turnstile *next_free;            /* (b)   */
    struct turnstile_waiter *top_waiter;    /* (b,t) */
    struct thread *owner;                   /* (b,t) */
    struct plist_node td_node;              /* (t)   */
};

/*
 * Locking keys :
 * (b) bucket
 * (t) turnstile_td
 */
struct turnstile_waiter {
    struct plist_node node; /* (b,t) */
    struct thread *thread;  /* (b,t) */
    bool awaken;            /* (b)   */
};

#define TURNSTILE_HTABLE_SIZE 128

#if !ISP2(TURNSTILE_HTABLE_SIZE)
#error "hash table size must be a power of two"
#endif /* !ISP2(TURNSTILE_HTABLE_SIZE) */

#define TURNSTILE_HTABLE_MASK (TURNSTILE_HTABLE_SIZE - 1)

static struct turnstile_bucket turnstile_htable[TURNSTILE_HTABLE_SIZE];

static struct kmem_cache turnstile_cache;

static uintptr_t
turnstile_hash(const void *addr)
{
    return ((uintptr_t)addr >> 8) ^ (uintptr_t)addr;
}

static void
turnstile_waiter_init(struct turnstile_waiter *waiter, struct thread *thread)
{
    plist_node_init(&waiter->node, thread_real_global_priority(thread));
    waiter->thread = thread;
    waiter->awaken = false;
}

static unsigned int
turnstile_waiter_priority(const struct turnstile_waiter *waiter)
{
    return plist_node_priority(&waiter->node);
}

static bool
turnstile_waiter_awaken(const struct turnstile_waiter *waiter)
{
    return waiter->awaken;
}

static void
turnstile_waiter_set_awaken(struct turnstile_waiter *waiter)
{
    waiter->awaken = true;
}

static void
turnstile_waiter_clear_awaken(struct turnstile_waiter *waiter)
{
    waiter->awaken = false;
}

static void
turnstile_waiter_wakeup(struct turnstile_waiter *waiter)
{
    if (turnstile_waiter_awaken(waiter)) {
        return;
    }

    thread_wakeup(waiter->thread);
    turnstile_waiter_set_awaken(waiter);
}

static void
turnstile_update_top_waiter(struct turnstile *turnstile)
{
    if (turnstile_empty(turnstile)) {
        turnstile->top_waiter = NULL;
        return;
    }

    turnstile->top_waiter = plist_last_entry(&turnstile->waiters,
                                             struct turnstile_waiter, node);
}

static void
turnstile_add_waiter(struct turnstile *turnstile,
                     struct turnstile_waiter *waiter)
{
    assert(!turnstile_waiter_awaken(waiter));
    plist_add(&turnstile->waiters, &waiter->node);
    turnstile_update_top_waiter(turnstile);
}

static void
turnstile_remove_waiter(struct turnstile *turnstile,
                        struct turnstile_waiter *waiter)
{
    plist_remove(&turnstile->waiters, &waiter->node);
    turnstile_update_top_waiter(turnstile);
}

static void
turnstile_requeue_waiter(struct turnstile *turnstile,
                         struct turnstile_waiter *waiter)
{
    unsigned int global_priority;

    global_priority = thread_real_global_priority(waiter->thread);
    assert(global_priority != plist_node_priority(&waiter->node));

    plist_remove(&turnstile->waiters, &waiter->node);
    plist_node_set_priority(&waiter->node, global_priority);
    plist_add(&turnstile->waiters, &waiter->node);
    turnstile_update_top_waiter(turnstile);
}

static void
turnstile_td_set_waiter(struct turnstile_td *td,
                        struct turnstile_waiter *waiter)
{
    td->waiter = waiter;
}

static struct turnstile_waiter *
turnstile_td_get_waiter(const struct turnstile_td *td)
{
    return td->waiter;
}

static void
turnstile_td_update_top_priority(struct turnstile_td *td)
{
    struct turnstile_waiter *top_waiter;
    struct turnstile *top_turnstile;

    if (plist_empty(&td->owned_turnstiles)) {
        td->top_global_priority = 0;
        return;
    }

    top_turnstile = plist_last_entry(&td->owned_turnstiles,
                                     struct turnstile, td_node);
    top_waiter = top_turnstile->top_waiter;

    if (top_waiter == NULL) {
        td->top_global_priority = 0;
    } else {
        td->top_global_priority = turnstile_waiter_priority(top_waiter);
        td->top_sched_policy = thread_real_sched_policy(top_waiter->thread);
        td->top_priority = thread_real_priority(top_waiter->thread);
    }
}

static void
turnstile_td_own(struct turnstile_td *td, struct turnstile *turnstile)
{
    struct turnstile_waiter *top_waiter;
    unsigned int top_priority;

    assert(turnstile->owner == NULL);

    top_waiter = turnstile->top_waiter;
    assert(top_waiter != NULL);
    top_priority = thread_real_global_priority(top_waiter->thread);
    plist_node_init(&turnstile->td_node, top_priority);
    plist_add(&td->owned_turnstiles, &turnstile->td_node);
    turnstile_td_update_top_priority(td);

    turnstile->owner = structof(td, struct thread, turnstile_td);
}

static void
turnstile_td_disown(struct turnstile_td *td, struct turnstile *turnstile)
{
    assert(turnstile->owner == structof(td, struct thread, turnstile_td));

    assert(!plist_node_unlinked(&turnstile->td_node));
    plist_remove(&td->owned_turnstiles, &turnstile->td_node);
    turnstile_td_update_top_priority(td);

    turnstile->owner = NULL;
}

/*
 * A turnstile must be "reowned" whenever its top waiter has changed.
 */
static void
turnstile_td_reown(struct turnstile_td *td, struct turnstile *turnstile)
{
    assert(turnstile->owner == structof(td, struct thread, turnstile_td));
    assert(!plist_node_unlinked(&turnstile->td_node));
    plist_remove(&td->owned_turnstiles, &turnstile->td_node);
    turnstile->owner = NULL;
    turnstile_td_own(td, turnstile);
}

/*
 * The caller must hold the turnstile thread data lock and no turnstile
 * locks when calling this function. The thread data are unlocked on return.
 *
 * In addition, this function drops a reference on the thread associated
 * with the given thread data.
 */
static void
turnstile_td_propagate_priority_loop(struct turnstile_td *td)
{
    unsigned int user_priority, real_priority, top_priority;
    struct turnstile_waiter *waiter;
    struct turnstile *turnstile;
    struct thread *thread;
    unsigned short priority;
    unsigned char policy;
    int error;

    /*
     * At the very least, this function must make sure that :
     *  - the given thread has its intended priority, which is the
     *    highest among its own and all the waiters in the turnstiles
     *    it owns, and
     *  - the thread is at its intended position in the turnstile it's
     *    waiting on, if any.
     */

    for (;;) {
        thread = structof(td, struct thread, turnstile_td);
        user_priority = thread_user_global_priority(thread);
        real_priority = thread_real_global_priority(thread);
        top_priority = td->top_global_priority;

        if (top_priority > user_priority) {
            policy = td->top_sched_policy;
            priority = td->top_priority;
        } else {
            top_priority = user_priority;
            policy = thread_user_sched_policy(thread);
            priority = thread_user_priority(thread);
        }

        if (top_priority != real_priority) {
            thread_pi_setscheduler(thread, policy, priority);
        }

        waiter = turnstile_td_get_waiter(td);

        if ((waiter == NULL)
            || (top_priority == turnstile_waiter_priority(waiter))) {
            spinlock_unlock(&td->lock);
            thread_unref(thread);
            return;
        }

        turnstile = turnstile_td_get_turnstile(td);
        assert(turnstile != NULL);

        error = spinlock_trylock(&turnstile->bucket->lock);

        if (error) {
            spinlock_unlock(&td->lock);
            spinlock_lock(&td->lock);
            continue;
        }

        /*
         * This couldn't be done while changing the thread's priority
         * because of locking restrictions. Do it now.
         */
        turnstile_requeue_waiter(turnstile, waiter);

        spinlock_unlock(&td->lock);
        thread_unref(thread);

        thread = turnstile->owner;

        if (thread == NULL) {
            break;
        }

        td = thread_turnstile_td(thread);

        thread_ref(thread);
        spinlock_lock(&td->lock);

        turnstile_td_reown(td, turnstile);

        spinlock_unlock(&turnstile->bucket->lock);
    }

    spinlock_unlock(&turnstile->bucket->lock);
}

void
turnstile_td_propagate_priority(struct turnstile_td *td)
{
    struct thread *thread;

    thread = structof(td, struct thread, turnstile_td);

    thread_ref(thread);
    spinlock_lock(&td->lock);
    turnstile_td_propagate_priority_loop(td);
}

static bool
turnstile_init_state_valid(const struct turnstile *turnstile)
{
    return (turnstile->bucket == NULL)
           && (turnstile->sync_obj == NULL)
           && (plist_empty(&turnstile->waiters))
           && (turnstile->next_free == NULL)
           && (turnstile->top_waiter == NULL);
}

static void
turnstile_use(struct turnstile *turnstile, const void *sync_obj)
{
    assert(turnstile->sync_obj == NULL);
    turnstile->sync_obj = sync_obj;
}

static void
turnstile_unuse(struct turnstile *turnstile)
{
    assert(turnstile->sync_obj != NULL);
    turnstile->sync_obj = NULL;
}

static bool
turnstile_in_use(const struct turnstile *turnstile)
{
    return turnstile->sync_obj != NULL;
}

static bool
turnstile_in_use_by(const struct turnstile *turnstile, const void *sync_obj)
{
    return turnstile->sync_obj == sync_obj;
}

static void
turnstile_bucket_init(struct turnstile_bucket *bucket)
{
    spinlock_init(&bucket->lock);
    hlist_init(&bucket->turnstiles);
}

static struct turnstile_bucket *
turnstile_bucket_get(const void *sync_obj)
{
    uintptr_t index;

    index = turnstile_hash(sync_obj) & TURNSTILE_HTABLE_MASK;
    assert(index < ARRAY_SIZE(turnstile_htable));
    return &turnstile_htable[index];
}

static void
turnstile_bucket_add(struct turnstile_bucket *bucket,
                     struct turnstile *turnstile)
{
    assert(turnstile->bucket == NULL);
    turnstile->bucket = bucket;
    hlist_insert_head(&bucket->turnstiles, &turnstile->node);
}

static void
turnstile_bucket_remove(struct turnstile_bucket *bucket,
                        struct turnstile *turnstile)
{
    assert(turnstile->bucket == bucket);
    turnstile->bucket = NULL;
    hlist_remove(&turnstile->node);
}

static struct turnstile *
turnstile_bucket_lookup(const struct turnstile_bucket *bucket,
                        const void *sync_obj)
{
    struct turnstile *turnstile;

    hlist_for_each_entry(&bucket->turnstiles, turnstile, node) {
        if (turnstile_in_use_by(turnstile, sync_obj)) {
            return turnstile;
        }
    }

    return NULL;
}

static void
turnstile_ctor(void *ptr)
{
    struct turnstile *turnstile;

    turnstile = ptr;
    turnstile->bucket = NULL;
    turnstile->sync_obj = NULL;
    plist_init(&turnstile->waiters);
    turnstile->next_free = NULL;
    turnstile->top_waiter = NULL;
    turnstile->owner = NULL;
}

static int __init
turnstile_setup(void)
{
    unsigned int i;

    for (i = 0; i < ARRAY_SIZE(turnstile_htable); i++) {
        turnstile_bucket_init(&turnstile_htable[i]);
    }

    kmem_cache_init(&turnstile_cache, "turnstile", sizeof(struct turnstile),
                    CPU_L1_SIZE, turnstile_ctor, 0);
    return 0;
}

INIT_OP_DEFINE(turnstile_setup,
               INIT_OP_DEP(kmem_setup, true));

struct turnstile *
turnstile_create(void)
{
    struct turnstile *turnstile;

    turnstile = kmem_cache_alloc(&turnstile_cache);

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

    assert(turnstile_init_state_valid(turnstile));
    return turnstile;
}

void
turnstile_destroy(struct turnstile *turnstile)
{
    assert(turnstile_init_state_valid(turnstile));
    kmem_cache_free(&turnstile_cache, turnstile);
}

struct turnstile *
turnstile_acquire(const void *sync_obj)
{
    struct turnstile_bucket *bucket;
    struct turnstile *turnstile;

    assert(sync_obj != NULL);

    bucket = turnstile_bucket_get(sync_obj);

    spinlock_lock(&bucket->lock);

    turnstile = turnstile_bucket_lookup(bucket, sync_obj);

    if (turnstile == NULL) {
        spinlock_unlock(&bucket->lock);
        return NULL;
    }

    return turnstile;
}

void
turnstile_release(struct turnstile *turnstile)
{
    spinlock_unlock(&turnstile->bucket->lock);
}

static void
turnstile_push_free(struct turnstile *turnstile,
                    struct turnstile *free_turnstile)
{
    assert(free_turnstile->next_free == NULL);
    free_turnstile->next_free = turnstile->next_free;
    turnstile->next_free = free_turnstile;
}

static struct turnstile *
turnstile_pop_free(struct turnstile *turnstile)
{
    struct turnstile *free_turnstile;

    free_turnstile = turnstile->next_free;

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

    turnstile->next_free = free_turnstile->next_free;
    free_turnstile->next_free = NULL;
    return free_turnstile;
}

struct turnstile *
turnstile_lend(const void *sync_obj)
{
    struct turnstile_bucket *bucket;
    struct turnstile *turnstile, *prev;
    struct turnstile_td *td;

    assert(sync_obj != NULL);

    turnstile = thread_turnstile_lend();
    assert(turnstile_init_state_valid(turnstile));

    td = thread_turnstile_td(thread_self());
    bucket = turnstile_bucket_get(sync_obj);

    spinlock_lock(&bucket->lock);

    prev = turnstile_bucket_lookup(bucket, sync_obj);

    if (prev == NULL) {
        turnstile_use(turnstile, sync_obj);
        turnstile_bucket_add(bucket, turnstile);
    } else {
        turnstile_push_free(prev, turnstile);
        turnstile = prev;
    }

    spinlock_lock(&td->lock);
    turnstile_td_set_turnstile(td, turnstile);
    spinlock_unlock(&td->lock);

    return turnstile;
}

void
turnstile_return(struct turnstile *turnstile)
{
    struct turnstile_bucket *bucket;
    struct turnstile *free_turnstile;
    struct turnstile_td *td;

    assert(turnstile_in_use(turnstile));

    td = thread_turnstile_td(thread_self());

    spinlock_lock(&td->lock);
    turnstile_td_set_turnstile(td, NULL);
    spinlock_unlock(&td->lock);

    bucket = turnstile->bucket;
    free_turnstile = turnstile_pop_free(turnstile);

    if (free_turnstile == NULL) {
        turnstile_bucket_remove(bucket, turnstile);
        turnstile_unuse(turnstile);
        free_turnstile = turnstile;
    }

    spinlock_unlock(&bucket->lock);

    assert(turnstile_init_state_valid(turnstile));
    thread_turnstile_return(free_turnstile);
}

bool
turnstile_empty(const struct turnstile *turnstile)
{
    return plist_empty(&turnstile->waiters);
}

static void
turnstile_update_owner(struct turnstile *turnstile, struct thread *owner)
{
    struct turnstile_td *td;

    assert(owner != NULL);
    assert((turnstile->owner == NULL) || (turnstile->owner == owner));

    td = thread_turnstile_td(owner);

    thread_ref(owner);
    spinlock_lock(&td->lock);

    if (turnstile_empty(turnstile)) {
        if (turnstile->owner != NULL) {
            turnstile_td_disown(td, turnstile);
        }
    } else if (turnstile->owner == NULL) {
        turnstile_td_own(td, turnstile);
    } else {
        turnstile_td_reown(td, turnstile);
    }

    spinlock_unlock(&turnstile->bucket->lock);

    turnstile_td_propagate_priority_loop(td);

    spinlock_lock(&turnstile->bucket->lock);
}

static int
turnstile_wait_common(struct turnstile *turnstile, const char *wchan,
                      struct thread *owner, bool timed, uint64_t ticks)
{
    struct turnstile_waiter waiter;
    struct turnstile_td *td;
    struct thread *thread;
    int error;

    error = 0;
    thread = thread_self();
    assert(thread != owner);

    td = thread_turnstile_td(thread);

    spinlock_lock(&td->lock);
    turnstile_waiter_init(&waiter, thread);
    turnstile_add_waiter(turnstile, &waiter);
    turnstile_td_set_waiter(td, &waiter);
    spinlock_unlock(&td->lock);

    if (owner == NULL) {
        if (turnstile->top_waiter == &waiter) {
            turnstile_waiter_set_awaken(&waiter);
        }
    } else {
        /* This function temporarily unlocks the turnstile */
        turnstile_update_owner(turnstile, owner);
    }

    for (;;) {
        if (!turnstile_waiter_awaken(&waiter)) {
            if (!timed) {
                thread_sleep(&turnstile->bucket->lock,
                             turnstile->sync_obj, wchan);
            } else {
                error = thread_timedsleep(&turnstile->bucket->lock,
                                          turnstile->sync_obj, wchan, ticks);

                if (error) {
                    if (turnstile_waiter_awaken(&waiter)) {
                        error = 0;
                    } else {
                        break;
                    }
                }
            }
        }

        /* Handle spurious wakeups */
        if (!turnstile_waiter_awaken(&waiter)) {
            continue;
        }

        /*
         * The real priority of a thread may change between waking up
         * and reacquiring the turnstile.
         */
        if (turnstile->top_waiter == &waiter) {
            break;
        }

        /* Otherwise, make sure the new top waiter is awaken */
        turnstile_waiter_wakeup(turnstile->top_waiter);
        turnstile_waiter_clear_awaken(&waiter);
    }

    spinlock_lock(&td->lock);
    turnstile_td_set_waiter(td, NULL);
    turnstile_remove_waiter(turnstile, &waiter);
    spinlock_unlock(&td->lock);

    if (error && (turnstile->owner != NULL)) {
        /* This function temporarily unlocks the turnstile */
        turnstile_update_owner(turnstile, turnstile->owner);
    }

    return error;
}

void
turnstile_wait(struct turnstile *turnstile, const char *wchan,
               struct thread *owner)
{
    int error;

    error = turnstile_wait_common(turnstile, wchan, owner, false, 0);
    assert(!error);
}

int
turnstile_timedwait(struct turnstile *turnstile, const char *wchan,
                    struct thread *owner, uint64_t ticks)
{
    return turnstile_wait_common(turnstile, wchan, owner, true, ticks);
}

void
turnstile_signal(struct turnstile *turnstile)
{
    struct turnstile_waiter *waiter;

    if (turnstile_empty(turnstile)) {
        return;
    }

    waiter = plist_last_entry(&turnstile->waiters,
                              struct turnstile_waiter, node);
    turnstile_waiter_wakeup(waiter);
}

void
turnstile_own(struct turnstile *turnstile)
{
    struct turnstile_td *td;
    struct thread *owner;
    unsigned int top_priority;

    assert(turnstile->owner == NULL);

    if (turnstile_empty(turnstile)) {
        return;
    }

    owner = thread_self();
    top_priority = turnstile_waiter_priority(turnstile->top_waiter);
    assert(thread_real_global_priority(owner) >= top_priority);
    td = thread_turnstile_td(owner);

    spinlock_lock(&td->lock);
    turnstile_td_own(td, turnstile);
    spinlock_unlock(&td->lock);
}

void
turnstile_disown(struct turnstile *turnstile)
{
    struct turnstile_td *td;
    struct thread *owner;

    if (turnstile->owner == NULL) {
        return;
    }

    owner = thread_self();
    assert(turnstile->owner == owner);
    assert(!turnstile_empty(turnstile));

    td = thread_turnstile_td(owner);

    spinlock_lock(&td->lock);
    turnstile_td_disown(td, turnstile);
    spinlock_unlock(&td->lock);
}


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/>.
 *
 *
 * Priority propagation capable sleep queues.
 *
 * Turnstiles are used to build sleeping synchronization primitives where
 * ownership applies, such as mutexes. They allow threads with different
 * priorities to contend on the same synchronization object without
 * unbounded priority inversion.
 */

#ifndef KERN_TURNSTILE_H
#define KERN_TURNSTILE_H

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

#include <kern/init.h>
#include <kern/plist.h>
#include <kern/spinlock.h>
#include <kern/thread.h>
#include <kern/turnstile_types.h>

struct turnstile;

/*
 * Turnstile thread data.
 */
struct turnstile_td;

static inline bool
turnstile_td_locked(const struct turnstile_td *td)
{
    return spinlock_locked(&td->lock);
}

/*
 * Initialize turnstile thread data.
 */
static inline void
turnstile_td_init(struct turnstile_td *td)
{
    spinlock_init(&td->lock);
    td->turnstile = NULL;
    td->waiter = NULL;
    plist_init(&td->owned_turnstiles);
    td->top_global_priority = 0;
}

/*
 * Turnstile thread data locking functions.
 */

static inline void
turnstile_td_lock(struct turnstile_td *td)
{
    spinlock_lock(&td->lock);
}

static inline int
turnstile_td_trylock(struct turnstile_td *td)
{
    return spinlock_trylock(&td->lock);
}

static inline void
turnstile_td_unlock(struct turnstile_td *td)
{
    spinlock_unlock(&td->lock);
}

/*
 * Functions managing the turnstile a thread is sleeping in.
 */

static inline void
turnstile_td_set_turnstile(struct turnstile_td *td,
                           struct turnstile *turnstile)
{
    td->turnstile = turnstile;
}

static inline struct turnstile *
turnstile_td_get_turnstile(const struct turnstile_td *td)
{
    return td->turnstile;
}

/*
 * Propagate priority starting at the thread containing the given thread data.
 */
void turnstile_td_propagate_priority(struct turnstile_td *td);

/*
 * Create/destroy a turnstile.
 */
struct turnstile * turnstile_create(void);
void turnstile_destroy(struct turnstile *turnstile);

/*
 * Acquire/release a turnstile.
 *
 * Acquiring a turnstile serializes all access and disables preemption.
 */
struct turnstile * turnstile_acquire(const void *sync_obj);
void turnstile_release(struct turnstile *turnstile);

/*
 * Lend/return a turnstile.
 *
 * A thread lends its private turnstile to the turnstile module in
 * order to prepare its sleep. The turnstile obtained on lending
 * is either the thread's turnstile, or an already existing turnstile
 * for this synchronization object if another thread is waiting.
 *
 * When multiple threads are waiting on the same turnstile, the extra
 * turnstiles lent are kept in an internal free list, used when threads
 * are awaken to return a turnstile to them.
 *
 * Note that the turnstile returned may not be the one lent.
 *
 * The turnstile obtained when lending is automatically acquired.
 */
struct turnstile * turnstile_lend(const void *sync_obj);
void turnstile_return(struct turnstile *turnstile);

/*
 * Return true if the given turnstile has no waiters.
 *
 * The turnstile must be acquired when calling this function.
 */
bool turnstile_empty(const struct turnstile *turnstile);

/*
 * Wait for a wake up on the given turnstile.
 *
 * The turnstile must be lent when calling this function. It is
 * released and later reacquired before returning from this function.
 *
 * The calling thread is considered a waiter as long as it didn't
 * reacquire the turnstile. This means that signalling a turnstile
 * has no visible effect on the number of waiters until the turnstile
 * is released, e.g. if a single thread is waiting and another signals
 * the turnstile, the turnstile is not immediately considered empty.
 *
 * If owner isn't NULL, it must refer to the thread currently owning
 * the associated synchronization object. The priority of the caller
 * is propagated to the chain of turnstiles and owners as necessary
 * to prevent unbounded priority inversion.
 *
 * When bounding the duration of the wait, the caller must pass an absolute
 * time in ticks, and ETIMEDOUT is returned if that time is reached before
 * the turnstile is signalled. In addition, if a timeout occurs, the calling
 * thread temporarily releases the turnstile before returning, causing other
 * threads to consider the turnstile as empty.
 */
void turnstile_wait(struct turnstile *turnstile, const char *wchan,
                    struct thread *owner);
int turnstile_timedwait(struct turnstile *turnstile, const char *wchan,
                        struct thread *owner, uint64_t ticks);

/*
 * Wake up a thread waiting on the given turnstile, if any.
 *
 * The turnstile must be acquired when calling this function.
 * Since a turnstile must be lent (and in turn is automatically
 * acquired) when waiting, and acquired in order to signal it,
 * wake-ups are serialized and cannot be missed.
 */
void turnstile_signal(struct turnstile *turnstile);

/*
 * Own/disown a turnstile.
 *
 * The turnstile must be lent when taking ownership, acquired when
 * releasing it.
 *
 * Ownership must be updated atomically with regard to the ownership
 * of the associated synchronization object.
 */
void turnstile_own(struct turnstile *turnstile);
void turnstile_disown(struct turnstile *turnstile);

/*
 * This init operation provides :
 *  - turnstile creation
 *  - module fully initialized
 */
INIT_OP_DECLARE(turnstile_setup);

#endif /* KERN_TURNSTILE_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_TURNSTILE_TYPES_H
#define KERN_TURNSTILE_TYPES_H

#include <kern/plist_types.h>
#include <kern/spinlock_types.h>

struct turnstile;
struct turnstile_waiter;

/*
 * Per-thread turnstile data.
 *
 * The turnstile member indicates whether this thread is in a turnstile,
 * and is only valid if the thread is not running.
 *
 * The waiter points to the structure a thread uses to queue itself on
 * a turnstile. It is used to access a sleeping thread from another
 * thread, e.g. on wake-ups or priority updates.
 *
 * The list of owned turnstiles is used by priority propagation to
 * determine the top priority among all waiters, which is stored in
 * the thread data so that turnstiles are quickly unlocked.
 *
 * Locking keys :
 * (b) bucket
 * (t) turnstile_td
 */
struct turnstile_td {
    struct spinlock lock;
    struct turnstile *turnstile;                /* (t)   */
    struct turnstile_waiter *waiter;            /* (b,t) */
    struct plist owned_turnstiles;              /* (t)   */
    unsigned int top_global_priority;           /* (t)   */
    unsigned char top_sched_policy;             /* (t)   */
    unsigned short top_priority;                /* (t)   */
};

#endif /* KERN_TURNSTILE_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/>.
 *
 *
 * Machine-independent definitions for non-standard C types.
 */

#ifndef KERN_TYPES_H
#define KERN_TYPES_H

typedef long ssize_t;

#endif /* KERN_TYPES_H */


code:
/*
 * Copyright (c) 2013-2014 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 <errno.h>
#include <stdalign.h>
#include <stddef.h>
#include <stdio.h>

#include <kern/bitmap.h>
#include <kern/error.h>
#include <kern/init.h>
#include <kern/kmem.h>
#include <kern/list.h>
#include <kern/log.h>
#include <kern/macros.h>
#include <kern/panic.h>
#include <kern/percpu.h>
#include <kern/spinlock.h>
#include <kern/syscnt.h>
#include <kern/thread.h>
#include <kern/work.h>
#include <machine/cpu.h>

#define WORK_PRIO_NORMAL    THREAD_SCHED_FS_PRIO_DEFAULT
#define WORK_PRIO_HIGH      THREAD_SCHED_FS_PRIO_MAX

#define WORK_INVALID_CPU ((unsigned int)-1)

/*
 * Keep at least that many threads alive when a work pool is idle.
 */
#define WORK_THREADS_SPARE 4

/*
 * When computing the maximum number of worker threads, start with multiplying
 * the number of processors by the ratio below. If the result is greater than
 * the threshold, retry by decreasing the ratio until either the result is
 * less than the threshold or the ratio is 1.
 */
#define WORK_THREADS_RATIO      4
#define WORK_THREADS_THRESHOLD  512
#define WORK_MAX_THREADS        MAX(CONFIG_MAX_CPUS, WORK_THREADS_THRESHOLD)

/*
 * Work pool flags.
 */
#define WORK_PF_GLOBAL      0x1 /* System-wide work queue */
#define WORK_PF_HIGHPRIO    0x2 /* High priority worker threads */

struct work_thread {
    struct list node;
    struct thread *thread;
    struct work_pool *pool;
    unsigned int id;
};

/*
 * Pool of threads and works.
 *
 * Interrupts must be disabled when accessing a work pool. Holding the
 * lock is required for global pools only, whereas exclusive access on
 * per-processor pools is achieved by disabling preemption.
 *
 * There are two internal queues of pending works. When first scheduling
 * a work, it is inserted into queue0. After a periodic event, works still
 * present in queue0 are moved to queue1. If these works are still present
 * in queue1 at the next periodic event, it means they couldn't be processed
 * for a complete period between two periodic events, at which point it is
 * assumed that processing works on the same processor they were queued on
 * becomes less relevant. As a result, periodic events also trigger the
 * transfer of works from queue1 to the matching global pool. Global pools
 * only use one queue.
 */
struct work_pool {
    alignas(CPU_L1_SIZE) struct spinlock lock;
    int flags;
    struct work_queue queue0;
    struct work_queue queue1;
    struct work_thread *manager;
    struct syscnt sc_transfers;
    unsigned int cpu;
    unsigned int max_threads;
    unsigned int nr_threads;
    unsigned int nr_available_threads;
    struct list available_threads;
    struct list dead_threads;
    BITMAP_DECLARE(bitmap, WORK_MAX_THREADS);
};

static int work_thread_create(struct work_pool *pool, unsigned int id);

static struct work_pool work_pool_cpu_main __percpu;
static struct work_pool work_pool_cpu_highprio __percpu;
static struct work_pool work_pool_main;
static struct work_pool work_pool_highprio;

static struct kmem_cache work_thread_cache;

static unsigned int
work_pool_alloc_id(struct work_pool *pool)
{
    int bit;

    assert(pool->nr_threads < pool->max_threads);
    pool->nr_threads++;
    bit = bitmap_find_first_zero(pool->bitmap, pool->max_threads);
    assert(bit >= 0);
    bitmap_set(pool->bitmap, bit);
    return bit;
}

static void
work_pool_free_id(struct work_pool *pool, unsigned int id)
{
    assert(pool->nr_threads != 0);
    pool->nr_threads--;
    bitmap_clear(pool->bitmap, id);
}

static unsigned int
work_pool_cpu_id(const struct work_pool *pool)
{
    assert(!(pool->flags & WORK_PF_GLOBAL));
    return pool->cpu;
}

static unsigned int
work_pool_compute_max_threads(unsigned int nr_cpus)
{
    unsigned int max_threads, ratio;

    ratio = WORK_THREADS_RATIO;
    max_threads = nr_cpus * ratio;

    while ((ratio > 1) && (max_threads > WORK_THREADS_THRESHOLD)) {
        ratio--;
        max_threads = nr_cpus * ratio;
    }

    assert(max_threads != 0);
    assert(max_threads <= WORK_MAX_THREADS);
    return max_threads;
}

static void __init
work_pool_init(struct work_pool *pool)
{
    spinlock_init(&pool->lock);
    work_queue_init(&pool->queue0);
    work_queue_init(&pool->queue1);
    pool->manager = NULL;
}

static void __init
work_pool_build(struct work_pool *pool, unsigned int cpu, int flags)
{
    char name[SYSCNT_NAME_SIZE];
    const char *suffix;
    unsigned int id, nr_cpus, max_threads;
    int error;

    pool->flags = flags;

    if (flags & WORK_PF_GLOBAL) {
        nr_cpus = cpu_count();
        pool->cpu = WORK_INVALID_CPU;
    } else {
        nr_cpus = 1;
        suffix = (flags & WORK_PF_HIGHPRIO) ? "h" : "";
        snprintf(name, sizeof(name), "work_transfers/%u%s", cpu, suffix);
        syscnt_register(&pool->sc_transfers, name);
        pool->cpu = cpu;
    }

    max_threads = work_pool_compute_max_threads(nr_cpus);

    pool->max_threads = max_threads;
    pool->nr_threads = 0;
    pool->nr_available_threads = 0;
    list_init(&pool->available_threads);
    list_init(&pool->dead_threads);
    bitmap_zero(pool->bitmap, WORK_MAX_THREADS);

    id = work_pool_alloc_id(pool);
    error = work_thread_create(pool, id);

    if (error) {
        goto error_thread;
    }

    return;

error_thread:
    panic("work: unable to create initial worker thread");
}

static struct work_pool *
work_pool_cpu_select(int flags)
{
    return (flags & WORK_HIGHPRIO)
           ? cpu_local_ptr(work_pool_cpu_highprio)
           : cpu_local_ptr(work_pool_cpu_main);
}

static void
work_pool_acquire(struct work_pool *pool, unsigned long *flags)
{
    if (pool->flags & WORK_PF_GLOBAL) {
        spinlock_lock_intr_save(&pool->lock, flags);
    } else {
        thread_preempt_disable_intr_save(flags);
    }
}

static void
work_pool_release(struct work_pool *pool, unsigned long flags)
{
    if (pool->flags & WORK_PF_GLOBAL) {
        spinlock_unlock_intr_restore(&pool->lock, flags);
    } else {
        thread_preempt_enable_intr_restore(flags);
    }
}

static int
work_pool_nr_works(const struct work_pool *pool)
{
    return (work_queue_nr_works(&pool->queue0)
            + work_queue_nr_works(&pool->queue1));
}

static struct work *
work_pool_pop_work(struct work_pool *pool)
{
    if (!(pool->flags & WORK_PF_GLOBAL)) {
        if (work_queue_nr_works(&pool->queue1) != 0) {
            return work_queue_pop(&pool->queue1);
        }
    }

    return work_queue_pop(&pool->queue0);
}

static void
work_pool_wakeup_manager(struct work_pool *pool)
{
    if (work_pool_nr_works(pool) == 0) {
        return;
    }

    if (pool->manager != NULL) {
        thread_wakeup(pool->manager->thread);
    }
}

static void
work_pool_shift_queues(struct work_pool *pool, struct work_queue *old_queue)
{
    assert(!(pool->flags & WORK_PF_GLOBAL));

    work_queue_transfer(old_queue, &pool->queue1);
    work_queue_transfer(&pool->queue1, &pool->queue0);
    work_queue_init(&pool->queue0);

    if (work_queue_nr_works(old_queue) != 0) {
        syscnt_inc(&pool->sc_transfers);
    }
}

static void
work_pool_push_work(struct work_pool *pool, struct work *work)
{
    work_queue_push(&pool->queue0, work);
    work_pool_wakeup_manager(pool);
}

static void
work_pool_concat_queue(struct work_pool *pool, struct work_queue *queue)
{
    work_queue_concat(&pool->queue0, queue);
    work_pool_wakeup_manager(pool);
}

static void
work_thread_destroy(struct work_thread *worker)
{
    thread_join(worker->thread);
    kmem_cache_free(&work_thread_cache, worker);
}

static void
work_process(void *arg)
{
    struct work_thread *self, *worker;
    struct work_pool *pool;
    struct work *work;
    struct spinlock *lock;
    unsigned long flags;
    unsigned int id;
    int error;

    self = arg;
    pool = self->pool;
    lock = (pool->flags & WORK_PF_GLOBAL) ? &pool->lock : NULL;

    work_pool_acquire(pool, &flags);

    for (;;) {
        if (pool->manager != NULL) {
            list_insert_tail(&pool->available_threads, &self->node);
            pool->nr_available_threads++;

            do {
                thread_sleep(lock, pool, "work_spr");
            } while (pool->manager != NULL);

            list_remove(&self->node);
            pool->nr_available_threads--;
        }

        if (!list_empty(&pool->dead_threads)) {
            worker = list_first_entry(&pool->dead_threads,
                                      struct work_thread, node);
            list_remove(&worker->node);
            work_pool_release(pool, flags);

            id = worker->id;
            work_thread_destroy(worker);

            /*
             * Release worker ID last so that, if the pool is full, no new
             * worker can be created unless all the resources of the worker
             * being destroyed have been freed. This is important to enforce
             * a strict boundary on the total amount of resources allocated
             * for a pool at any time.
             */
            work_pool_acquire(pool, &flags);
            work_pool_free_id(pool, id);
            continue;
        }

        if (work_pool_nr_works(pool) == 0) {
            if (pool->nr_threads > WORK_THREADS_SPARE) {
                break;
            }

            pool->manager = self;

            do {
                thread_sleep(lock, pool, "work_mgr");
            } while (work_pool_nr_works(pool) == 0);

            pool->manager = NULL;
        }

        work = work_pool_pop_work(pool);

        if (work_pool_nr_works(pool) != 0) {
            if (pool->nr_available_threads != 0) {
                worker = list_first_entry(&pool->available_threads,
                                          struct work_thread, node);
                thread_wakeup(worker->thread);
            } else if (pool->nr_threads < pool->max_threads) {
                id = work_pool_alloc_id(pool);
                work_pool_release(pool, flags);

                error = work_thread_create(pool, id);

                work_pool_acquire(pool, &flags);

                if (error) {
                    work_pool_free_id(pool, id);
                    log_warning("work: unable to create worker thread");
                }
            }
        }

        work_pool_release(pool, flags);

        work->fn(work);

        work_pool_acquire(pool, &flags);
    }

    list_insert_tail(&pool->dead_threads, &self->node);
    work_pool_release(pool, flags);
}

static int
work_thread_create(struct work_pool *pool, unsigned int id)
{
    char name[THREAD_NAME_SIZE];
    struct thread_attr attr;
    struct cpumap *cpumap;
    struct work_thread *worker;
    const char *suffix;
    unsigned short priority;
    int error;

    worker = kmem_cache_alloc(&work_thread_cache);

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

    worker->pool = pool;
    worker->id = id;

    if (pool->flags & WORK_PF_HIGHPRIO) {
        suffix = "h";
        priority = WORK_PRIO_HIGH;
    } else {
        suffix = "";
        priority = WORK_PRIO_NORMAL;
    }

    if (pool->flags & WORK_PF_GLOBAL) {
        cpumap = NULL;
        snprintf(name, sizeof(name),
                 THREAD_KERNEL_PREFIX "work_process/g:%u%s",
                 worker->id, suffix);
    } else {
        unsigned int pool_id;

        error = cpumap_create(&cpumap);

        if (error) {
            goto error_cpumap;
        }

        pool_id = work_pool_cpu_id(pool);
        cpumap_zero(cpumap);
        cpumap_set(cpumap, pool_id);
        snprintf(name, sizeof(name),
                 THREAD_KERNEL_PREFIX "work_process/%u:%u%s",
                 pool_id, worker->id, suffix);
    }

    thread_attr_init(&attr, name);
    thread_attr_set_priority(&attr, priority);

    if (cpumap != NULL) {
        thread_attr_set_cpumap(&attr, cpumap);
    }

    error = thread_create(&worker->thread, &attr, work_process, worker);

    if (cpumap != NULL) {
        cpumap_destroy(cpumap);
    }

    if (error) {
        goto error_thread;
    }

    return 0;

error_thread:
error_cpumap:
    kmem_cache_free(&work_thread_cache, worker);
    return error;
}

static int __init
work_bootstrap(void)
{
    work_pool_init(cpu_local_ptr(work_pool_cpu_main));
    work_pool_init(cpu_local_ptr(work_pool_cpu_highprio));
    return 0;
}

INIT_OP_DEFINE(work_bootstrap,
               INIT_OP_DEP(cpu_setup, true),
               INIT_OP_DEP(spinlock_setup, true),
               INIT_OP_DEP(thread_bootstrap, true));

static int __init
work_setup(void)
{
    kmem_cache_init(&work_thread_cache, "work_thread",
                    sizeof(struct work_thread), 0, NULL, 0);

    for (unsigned int i = 1; i < cpu_count(); i++) {
        work_pool_init(percpu_ptr(work_pool_cpu_main, i));
        work_pool_init(percpu_ptr(work_pool_cpu_highprio, i));
    }

    work_pool_init(&work_pool_main);
    work_pool_init(&work_pool_highprio);

    for (unsigned int i = 0; i < cpu_count(); i++) {
        work_pool_build(percpu_ptr(work_pool_cpu_main, i), i, 0);
        work_pool_build(percpu_ptr(work_pool_cpu_highprio, i), i,
                        WORK_PF_HIGHPRIO);
    }

    work_pool_build(&work_pool_main, WORK_INVALID_CPU, WORK_PF_GLOBAL);
    work_pool_build(&work_pool_highprio, WORK_INVALID_CPU,
                    WORK_PF_GLOBAL | WORK_PF_HIGHPRIO);

    log_info("work: threads per pool (per-cpu/global): %u/%u, spare: %u",
             percpu_var(work_pool_cpu_main.max_threads, 0),
             work_pool_main.max_threads, WORK_THREADS_SPARE);

    return 0;
}

INIT_OP_DEFINE(work_setup,
               INIT_OP_DEP(cpu_mp_probe, true),
               INIT_OP_DEP(cpumap_setup, true),
               INIT_OP_DEP(kmem_setup, true),
               INIT_OP_DEP(log_setup, true),
               INIT_OP_DEP(spinlock_setup, true),
               INIT_OP_DEP(syscnt_setup, true),
               INIT_OP_DEP(thread_setup, true),
               INIT_OP_DEP(work_bootstrap, true));

void
work_schedule(struct work *work, int flags)
{
    struct work_pool *pool;
    unsigned long cpu_flags;

    thread_pin();
    pool = work_pool_cpu_select(flags);
    work_pool_acquire(pool, &cpu_flags);
    work_pool_push_work(pool, work);
    work_pool_release(pool, cpu_flags);
    thread_unpin();
}

void
work_queue_schedule(struct work_queue *queue, int flags)
{
    struct work_pool *pool;
    unsigned long cpu_flags;

    thread_pin();
    pool = work_pool_cpu_select(flags);
    work_pool_acquire(pool, &cpu_flags);
    work_pool_concat_queue(pool, queue);
    work_pool_release(pool, cpu_flags);
    thread_unpin();
}

void
work_report_periodic_event(void)
{
    struct work_queue queue, highprio_queue;

    assert(thread_check_intr_context());

    work_pool_shift_queues(cpu_local_ptr(work_pool_cpu_main), &queue);
    work_pool_shift_queues(cpu_local_ptr(work_pool_cpu_highprio),
                           &highprio_queue);

    if (work_queue_nr_works(&queue) != 0) {
        spinlock_lock(&work_pool_main.lock);
        work_pool_concat_queue(&work_pool_main, &queue);
        spinlock_unlock(&work_pool_main.lock);
    }

    if (work_queue_nr_works(&highprio_queue) != 0) {
        spinlock_lock(&work_pool_highprio.lock);
        work_pool_concat_queue(&work_pool_highprio, &highprio_queue);
        spinlock_unlock(&work_pool_highprio.lock);
    }
}


code:
/*
 * Copyright (c) 2013-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/>.
 *
 *
 * Deferred work queues.
 *
 * Works, like threads, are scheduled activities, but they are much shorter
 * and (usually) consume a lot less resources. They are allowed to block
 * and must run in thread context. This module provides thread pools to
 * concurrently handle queued works.
 */

#ifndef KERN_WORK_H
#define KERN_WORK_H

#include <kern/init.h>

/*
 * Work scheduling flags.
 */
#define WORK_HIGHPRIO   0x1 /* Use a high priority worker thread */

/*
 * Deferred work.
 *
 * This structure should be embedded in objects related to the work. It
 * stores the work function and is passed to it as its only parameter.
 * The function can then find the containing object with the structof macro.
 */
struct work;

/*
 * Queue of deferred works for batch scheduling.
 */
struct work_queue;

/*
 * Type for work functions.
 */
typedef void (*work_fn_t)(struct work *);

#include <kern/work_i.h>

static inline void
work_queue_init(struct work_queue *queue)
{
    queue->first = NULL;
    queue->last = NULL;
    queue->nr_works = 0;
}

static inline unsigned int
work_queue_nr_works(const struct work_queue *queue)
{
    return queue->nr_works;
}

static inline void
work_queue_push(struct work_queue *queue, struct work *work)
{
    work->next = NULL;

    if (queue->last == NULL) {
        queue->first = work;
    } else {
        queue->last->next = work;
    }

    queue->last = work;
    queue->nr_works++;
}

static inline struct work *
work_queue_pop(struct work_queue *queue)
{
    struct work *work;

    work = queue->first;
    queue->first = work->next;

    if (queue->last == work) {
        queue->last = NULL;
    }

    queue->nr_works--;
    return work;
}

static inline void
work_queue_transfer(struct work_queue *dest, struct work_queue *src)
{
    *dest = *src;
}

static inline void
work_queue_concat(struct work_queue *queue1, struct work_queue *queue2)
{
    if (queue2->nr_works == 0) {
        return;
    }

    if (queue1->nr_works == 0) {
        *queue1 = *queue2;
        return;
    }

    queue1->last->next = queue2->first;
    queue1->last = queue2->last;
    queue1->nr_works += queue2->nr_works;
}

static inline void
work_init(struct work *work, work_fn_t fn)
{
    work->fn = fn;
}

/*
 * Schedule work for deferred processing.
 *
 * After being scheduled, a work queue must be reinitialized before
 * it can be reused.
 *
 * This function may be called from interrupt context.
 */
void work_schedule(struct work *work, int flags);
void work_queue_schedule(struct work_queue *queue, int flags);

/*
 * Report a periodic event (normally the periodic timer interrupt) on the
 * current processor.
 *
 * Periodic events are used internally for optimizations.
 *
 * Interrupts and preemption must be disabled when calling this function.
 */
void work_report_periodic_event(void);

/*
 * This init operation provides :
 *  - work / work queue initialization and scheduling
 */
INIT_OP_DECLARE(work_bootstrap);

#endif /* KERN_WORK_H */


code:
/*
 * Copyright (c) 2013-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_WORK_I_H
#define KERN_WORK_I_H

struct work {
    struct work *next;
    work_fn_t fn;
};

struct work_queue {
    struct work *first;
    struct work *last;
    unsigned int nr_works;
};

#endif /* KERN_WORK_I_H */


code:
/*
 * Copyright (c) 2014-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/>.
 */

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

#include <kern/atomic.h>
#include <kern/init.h>
#include <kern/log.h>
#include <kern/macros.h>
#include <kern/percpu.h>
#include <kern/spinlock.h>
#include <kern/syscnt.h>
#include <kern/thread.h>
#include <kern/xcall.h>
#include <machine/cpu.h>

struct xcall {
    xcall_fn_t fn;
    void *arg;
};

/*
 * Per-CPU data.
 *
 * The lock is used to serialize cross-calls from different processors
 * to the same processor. It is held during the complete cross-call
 * sequence. Inside the critical section, accesses to the receive call
 * are used to enforce release-acquire ordering between the sending
 * and receiving processors.
 *
 * Locking keys :
 * (a) atomic
 * (c) cpu_data
 */
struct xcall_cpu_data {
    alignas(CPU_L1_SIZE) struct spinlock lock;
    struct xcall *recv_call;    /* (c) */
    struct syscnt sc_sent;      /* (a) */
    struct syscnt sc_received;  /* (a) */
};

static struct xcall_cpu_data xcall_cpu_data __percpu;

static struct xcall_cpu_data *
xcall_get_local_cpu_data(void)
{
    return cpu_local_ptr(xcall_cpu_data);
}

static struct xcall_cpu_data *
xcall_get_cpu_data(unsigned int cpu)
{
    return percpu_ptr(xcall_cpu_data, cpu);
}

static void
xcall_init(struct xcall *call, xcall_fn_t fn, void *arg)
{
    call->fn = fn;
    call->arg = arg;
}

static void
xcall_process(struct xcall *call)
{
    call->fn(call->arg);
}

static void
xcall_cpu_data_init(struct xcall_cpu_data *cpu_data, unsigned int cpu)
{
    char name[SYSCNT_NAME_SIZE];

    snprintf(name, sizeof(name), "xcall_sent/%u", cpu);
    syscnt_register(&cpu_data->sc_sent, name);
    snprintf(name, sizeof(name), "xcall_received/%u", cpu);
    syscnt_register(&cpu_data->sc_received, name);
    cpu_data->recv_call = NULL;
    spinlock_init(&cpu_data->lock);
}

static struct xcall *
xcall_cpu_data_get_recv_call(const struct xcall_cpu_data *cpu_data)
{
    return atomic_load(&cpu_data->recv_call, ATOMIC_ACQUIRE);
}

static void
xcall_cpu_data_set_recv_call(struct xcall_cpu_data *cpu_data,
                             struct xcall *call)
{
    atomic_store(&cpu_data->recv_call, call, ATOMIC_RELEASE);
}

static void
xcall_cpu_data_clear_recv_call(struct xcall_cpu_data *cpu_data)
{
    xcall_cpu_data_set_recv_call(cpu_data, NULL);
}

static int __init
xcall_setup(void)
{
    unsigned int i;

    for (i = 0; i < cpu_count(); i++) {
        xcall_cpu_data_init(xcall_get_cpu_data(i), i);
    }

    return 0;
}

INIT_OP_DEFINE(xcall_setup,
               INIT_OP_DEP(cpu_mp_probe, true),
               INIT_OP_DEP(thread_bootstrap, true),
               INIT_OP_DEP(spinlock_setup, true),
               INIT_OP_DEP(syscnt_setup, true));

void
xcall_call(xcall_fn_t fn, void *arg, unsigned int cpu)
{
    struct xcall_cpu_data *cpu_data;
    struct xcall call;

    assert(cpu_intr_enabled());
    assert(fn);

    xcall_init(&call, fn, arg);
    cpu_data = xcall_get_cpu_data(cpu);

    spinlock_lock(&cpu_data->lock);

    /* Enforce release ordering on the receive call */
    xcall_cpu_data_set_recv_call(cpu_data, &call);

    cpu_send_xcall(cpu);

    /* Enforce acquire ordering on the receive call */
    while (xcall_cpu_data_get_recv_call(cpu_data) != NULL) {
        cpu_pause();
    }

    spinlock_unlock(&cpu_data->lock);

    syscnt_inc(&cpu_data->sc_sent);
}

void
xcall_intr(void)
{
    struct xcall_cpu_data *cpu_data;
    struct xcall *call;

    assert(thread_check_intr_context());

    cpu_data = xcall_get_local_cpu_data();

    /* Enforce acquire ordering on the receive call */
    call = xcall_cpu_data_get_recv_call(cpu_data);

    if (call) {
        xcall_process(call);
    } else {
        log_err("xcall: spurious interrupt on cpu%u", cpu_id());
    }

    syscnt_inc(&cpu_data->sc_received);

    /* Enforce release ordering on the receive call */
    xcall_cpu_data_clear_recv_call(cpu_data);
}


code:
/*
 * Copyright (c) 2014-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/>.
 *
 *
 * Cross-processor function calls.
 *
 * This module provides the ability to run functions, called cross-calls,
 * on specific processors.
 *
 * TODO Asynchronous cross-calls.
 */

#ifndef KERN_XCALL_H
#define KERN_XCALL_H

#include <kern/init.h>

/*
 * Type for cross-call functions.
 */
typedef void (*xcall_fn_t)(void *arg);

/*
 * Run the given cross-call function on a specific processor.
 *
 * The operation is completely synchronous, returning only when the function
 * has finished running on the target processor. Release-acquire ordering is
 * enforced both before and after the function runs on the target processor,
 * so that side effects produced by the caller are visible to the function,
 * and vice-versa on return.
 *
 * The callback function runs in interrupt context. Interrupts must be enabled
 * when calling this function.
 */
void xcall_call(xcall_fn_t fn, void *arg, unsigned int cpu);

/*
 * Handle a cross-call interrupt from a remote processor.
 *
 * Called from interrupt context.
 */
void xcall_intr(void);

/*
 * This init operation provides :
 *  - cross-calls are usable
 *  - module fully initialized
 */
INIT_OP_DECLARE(xcall_setup);

#endif /* KERN_XCALL_H */


code:
menuconfig TEST_MODULE
	bool "Test module"
	default n
	---help---
	  Run a test module instead of booting the system.

if TEST_MODULE

choice
	prompt "Select test module"

config TEST_MODULE_ATOMIC
	bool "atomic"

if TEST_MODULE_ATOMIC

config TEST_MODULE_ATOMIC_64
	bool "Force 64-bit atomic operations"
	default n
	---help---
	  Force atomic operations to apply to 64-bit integers, even on 32-bit
	  targets. Some targets may not support 64-bit atomic operations
	  at all.

endif

config TEST_MODULE_BULLETIN
	bool "bulletin"

config TEST_MODULE_MUTEX
	bool "mutex"
	select MUTEX_DEBUG

config TEST_MODULE_MUTEX_PI
	bool "mutex_pi"

config TEST_MODULE_PERFMON_CPU
	bool "perfmon_cpu"
	depends on PERFMON

config TEST_MODULE_PERFMON_THREAD
	bool "perfmon_thread"
	depends on PERFMON

config TEST_MODULE_PERFMON_TORTURE
	bool "perfmon_torture"
	depends on PERFMON

config TEST_MODULE_PMAP_UPDATE_MP
	bool "pmap_update_mp"

config TEST_MODULE_RCU_DEFER
	bool "rcu_defer"

config TEST_MODULE_SREF_DIRTY_ZEROES
	bool "sref_dirty_zeroes"

config TEST_MODULE_SREF_NOREF
	bool "sref_noref"

config TEST_MODULE_SREF_WEAKREF
	bool "sref_weakref"

config TEST_MODULE_THREAD_SUSPEND
	bool "thread_suspend"

config TEST_MODULE_VM_PAGE_FILL
	bool "vm_page_fill"

config TEST_MODULE_XCALL
	bool "xcall"

endchoice

endif

path: test/Kconfig


code:
x15_SOURCES-$(CONFIG_TEST_MODULE_ATOMIC)                += test/test_atomic.c
x15_SOURCES-$(CONFIG_TEST_MODULE_BULLETIN)              += test/test_bulletin.c
x15_SOURCES-$(CONFIG_TEST_MODULE_MUTEX)                 += test/test_mutex.c
x15_SOURCES-$(CONFIG_TEST_MODULE_MUTEX_PI)              += test/test_mutex_pi.c
x15_SOURCES-$(CONFIG_TEST_MODULE_PERFMON_CPU)           += test/test_perfmon_cpu.c
x15_SOURCES-$(CONFIG_TEST_MODULE_PERFMON_THREAD)        += test/test_perfmon_thread.c
x15_SOURCES-$(CONFIG_TEST_MODULE_PERFMON_TORTURE)       += test/test_perfmon_torture.c
x15_SOURCES-$(CONFIG_TEST_MODULE_PMAP_UPDATE_MP)        += test/test_pmap_update_mp.c
x15_SOURCES-$(CONFIG_TEST_MODULE_RCU_DEFER)             += test/test_rcu_defer.c
x15_SOURCES-$(CONFIG_TEST_MODULE_SREF_DIRTY_ZEROES)     += test/test_sref_dirty_zeroes.c
x15_SOURCES-$(CONFIG_TEST_MODULE_SREF_NOREF)            += test/test_sref_noref.c
x15_SOURCES-$(CONFIG_TEST_MODULE_SREF_WEAKREF)          += test/test_sref_weakref.c
x15_SOURCES-$(CONFIG_TEST_MODULE_THREAD_SUSPEND)        += test/test_thread_suspend.c
x15_SOURCES-$(CONFIG_TEST_MODULE_VM_PAGE_FILL)          += test/test_vm_page_fill.c
x15_SOURCES-$(CONFIG_TEST_MODULE_XCALL)                 += test/test_xcall.c

path: test/Makefile


code:
/*
 * Copyright (c) 2014 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 TEST_TEST_H
#define TEST_TEST_H

void test_setup(void);

#endif /* TEST_TEST_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/>.
 *
 *
 * This test makes sure that atomic operations behave at least as their
 * non-atomic counterpart. It doesn't actually test the atomicity of the
 * operations, but rather helps check that the generated code matches
 * expectations for the target configuration.
 */

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

#include <kern/atomic.h>
#include <kern/error.h>
#include <kern/init.h>
#include <kern/log.h>
#include <kern/macros.h>
#include <test/test.h>

#ifdef CONFIG_TEST_MODULE_ATOMIC_64
typedef uint64_t test_int_t;
#else
typedef unsigned long test_int_t;
#endif

/*
 * Don't make this variable private, so that the compiler may not assume
 * that all accesses are known, which forces it to generate access
 * instructions for it. This only works when building without LTO.
 */
test_int_t test_n;

/*
 * Prevent these functions from being inlined in order to make the generated
 * code easy to find and review.
 */

static __noinline test_int_t
test_atomic_load(test_int_t *n)
{
    return atomic_load(n, ATOMIC_RELAXED);
}

static __noinline void
test_atomic_store(test_int_t *n, test_int_t val)
{
    atomic_store(n, val, ATOMIC_RELAXED);
}

static __noinline test_int_t
test_atomic_cas(test_int_t *n, test_int_t oval, test_int_t nval)
{
    return atomic_cas(n, oval, nval, ATOMIC_RELAXED);
}

static __noinline test_int_t
test_atomic_swap(test_int_t *n, test_int_t val)
{
    return atomic_swap(n, val, ATOMIC_RELAXED);
}

static __noinline test_int_t
test_atomic_fetch_add(test_int_t *n, test_int_t val)
{
    return atomic_fetch_add(n, val, ATOMIC_RELAXED);
}

static __noinline test_int_t
test_atomic_fetch_sub(test_int_t *n, test_int_t val)
{
    return atomic_fetch_sub(n, val, ATOMIC_RELAXED);
}

static __noinline test_int_t
test_atomic_fetch_and(test_int_t *n, test_int_t val)
{
    return atomic_fetch_and(n, val, ATOMIC_RELAXED);
}

static __noinline test_int_t
test_atomic_fetch_or(test_int_t *n, test_int_t val)
{
    return atomic_fetch_or(n, val, ATOMIC_RELAXED);
}

static __noinline test_int_t
test_atomic_fetch_xor(test_int_t *n, test_int_t val)
{
    return atomic_fetch_xor(n, val, ATOMIC_RELAXED);
}

static __noinline void
test_atomic_add(test_int_t *n, test_int_t val)
{
    atomic_add(n, val, ATOMIC_RELAXED);
}

static __noinline void
test_atomic_sub(test_int_t *n, test_int_t val)
{
    atomic_sub(n, val, ATOMIC_RELAXED);
}

static __noinline void
test_atomic_and(test_int_t *n, test_int_t val)
{
    atomic_and(n, val, ATOMIC_RELAXED);
}

static __noinline void
test_atomic_or(test_int_t *n, test_int_t val)
{
    atomic_or(n, val, ATOMIC_RELAXED);
}

static __noinline void
test_atomic_xor(test_int_t *n, test_int_t val)
{
    atomic_xor(n, val, ATOMIC_RELAXED);
}

static void
test_check_n(test_int_t val, const char *fn)
{
    volatile test_int_t *ptr;
    int error;

    ptr = &test_n;
    error = (val == *ptr) ? 0 : EINVAL;
    error_check(error, fn);
}

static void
test_load(void)
{
    test_int_t n;

    n = test_atomic_load(&test_n);
    test_check_n(n, __func__);
}

static void
test_store(void)
{
    test_int_t val;

    val = 0x123;
    test_atomic_store(&test_n, val);
    test_check_n(val, __func__);
}

static void
test_cas_match(void)
{
    test_int_t prev, oval, nval;
    int error;

    oval = 0;
    nval = 0x123;
    test_n = oval;
    prev = test_atomic_cas(&test_n, oval, nval);
    error = (prev == oval) ? 0 : EINVAL;
    error_check(error, __func__);
    test_check_n(nval, __func__);
}

static void
test_cas_nomatch(void)
{
    test_int_t prev, oval, nval;
    int error;

    oval = 0;
    nval = 0x123;
    test_n = oval;
    prev = test_atomic_cas(&test_n, oval + 1, nval);
    error = (prev == oval) ? 0 : EINVAL;
    error_check(error, __func__);
    test_check_n(oval, __func__);
}

static void
test_swap(void)
{
    test_int_t prev, oval, nval;
    int error;

    oval = 0;
    nval = 0x123;
    test_n = oval;
    prev = test_atomic_swap(&test_n, nval);
    error = (prev == oval) ? 0 : EINVAL;
    error_check(error, __func__);
    test_check_n(nval, __func__);
}

static void
test_fetch_add(void)
{
    test_int_t prev, oval, delta;
    int error;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    prev = test_atomic_fetch_add(&test_n, delta);
    error = (prev == oval) ? 0 : EINVAL;
    error_check(error, __func__);
    test_check_n(oval + delta, __func__);
}

static void
test_fetch_sub(void)
{
    test_int_t prev, oval, delta;
    int error;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    prev = test_atomic_fetch_sub(&test_n, delta);
    error = (prev == oval) ? 0 : EINVAL;
    error_check(error, __func__);
    test_check_n(oval - delta, __func__);
}

static void
test_fetch_and(void)
{
    test_int_t prev, oval, delta;
    int error;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    prev = test_atomic_fetch_and(&test_n, delta);
    error = (prev == oval) ? 0 : EINVAL;
    error_check(error, __func__);
    test_check_n(oval & delta, __func__);
}

static void
test_fetch_or(void)
{
    test_int_t prev, oval, delta;
    int error;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    prev = test_atomic_fetch_or(&test_n, delta);
    error = (prev == oval) ? 0 : EINVAL;
    error_check(error, __func__);
    test_check_n(oval | delta, __func__);
}

static void
test_fetch_xor(void)
{
    test_int_t prev, oval, delta;
    int error;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    prev = test_atomic_fetch_xor(&test_n, delta);
    error = (prev == oval) ? 0 : EINVAL;
    error_check(error, __func__);
    test_check_n(oval ^ delta, __func__);
}

static void
test_add(void)
{
    test_int_t oval, delta;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    test_atomic_add(&test_n, delta);
    test_check_n(oval + delta, __func__);
}

static void
test_sub(void)
{
    test_int_t oval, delta;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    test_atomic_sub(&test_n, delta);
    test_check_n(oval - delta, __func__);
}

static void
test_and(void)
{
    test_int_t oval, delta;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    test_atomic_and(&test_n, delta);
    test_check_n(oval & delta, __func__);
}

static void
test_or(void)
{
    test_int_t oval, delta;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    test_atomic_or(&test_n, delta);
    test_check_n(oval | delta, __func__);
}

static void
test_xor(void)
{
    test_int_t oval, delta;

    oval = 0x123;
    delta = 0x456;
    test_n = oval;
    test_atomic_xor(&test_n, delta);
    test_check_n(oval ^ delta, __func__);
}

void __init
test_setup(void)
{
    test_load();
    test_store();
    test_cas_match();
    test_cas_nomatch();
    test_swap();
    test_fetch_add();
    test_fetch_sub();
    test_fetch_and();
    test_fetch_or();
    test_fetch_xor();
    test_add();
    test_sub();
    test_and();
    test_or();
    test_xor();
    log_info("test: done");
}


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 test makes a bulletin subscriber subscribe to a bulletin, and uses
 * a timer to periodically publish, unsubscribe, and resubscribe to the
 * bulletin. A counter is used to test passing values to the notification
 * callback function.
 */

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

#include <kern/bulletin.h>
#include <kern/clock.h>
#include <kern/error.h>
#include <kern/log.h>
#include <kern/timer.h>
#include <test/test.h>

#define TEST_INTERVAL   1
#define TEST_NR_LOOPS   4

static struct bulletin test_bulletin;
static struct bulletin_sub test_bulletin_sub;
static struct timer test_timer;
static unsigned int test_counter;

static void
test_notify(uintptr_t value, void *arg)
{
    log_info("test: notify: value:%lu arg:%p", value, arg);
}

static void
test_tick(struct timer *timer)
{
    uint64_t ticks;

    test_counter++;
    bulletin_publish(&test_bulletin, test_counter);
    bulletin_unsubscribe(&test_bulletin, &test_bulletin_sub);

    if (test_counter == TEST_NR_LOOPS) {
        log_info("test: done");
        return;
    }

    bulletin_subscribe(&test_bulletin, &test_bulletin_sub,
                       test_notify, (void *)0x123);

    ticks = timer_get_time(timer) + clock_ticks_from_ms(TEST_INTERVAL * 1000);
    timer_schedule(&test_timer, ticks);
}

void __init
test_setup(void)
{
    uint64_t ticks;

    bulletin_init(&test_bulletin);
    bulletin_subscribe(&test_bulletin, &test_bulletin_sub,
                       test_notify, (void *)0x123);

    timer_init(&test_timer, test_tick, TIMER_DETACHED);
    ticks = clock_get_time() + clock_ticks_from_ms(TEST_INTERVAL * 1000);
    timer_schedule(&test_timer, ticks);
}


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/>.
 *
 *
 * This test module is a stress test, expected to never terminate, of the
 * timed lock functionality provided by the mutex implementations. The
 * two conditions for success are :
 *  - no assertion triggered
 *  - all debugging system counters of the selected mutex implementation
 *    must be non-zero after some time.
 *
 * The system counters are meant to perform simple code coverage, asserting
 * all the tricky code paths are taken at least once.
 */

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

#include <kern/atomic.h>
#include <kern/clock.h>
#include <kern/error.h>
#include <kern/init.h>
#include <kern/kmem.h>
#include <kern/log.h>
#include <kern/mutex.h>
#include <kern/panic.h>
#include <kern/syscnt.h>
#include <kern/thread.h>
#include <kern/timer.h>
#include <test/test.h>

#define TEST_MIN_CPUS 3

#define TEST_REPORT_INTERVAL 10000

struct test {
    struct mutex mutex;
    unsigned int counter;
};

static struct timer test_timer;

static void
test_run(void *arg)
{
    unsigned int prev, counter;
    struct test *test;
    int error;

    test = arg;

    for (counter = 1; /* no condition */; counter++) {
        if ((counter % 1024) == 0) {
            printf("%s ", thread_self()->name);
        }

        error = mutex_timedlock(&test->mutex, clock_get_time() + 1);

        if (error) {
            thread_delay(1, false);
            continue;
        }

        prev = atomic_fetch_add(&test->counter, 1, ATOMIC_SEQ_CST);

        if (prev != 0) {
            break;
        }

        if ((counter % 2) == 0) {
            cpu_delay(clock_ticks_to_ms(1) * 1000);
        } else {
            thread_delay(1, false);
        }

        prev = atomic_fetch_sub(&test->counter, 1, ATOMIC_SEQ_CST);

        if (prev != 1) {
            break;
        }

        mutex_unlock(&test->mutex);

        if ((counter % 2) == 0) {
            thread_delay(1, false);
        }
    }

    panic("test: invalid counter value (%u)", test->counter);
}

static struct test *
test_create(unsigned int nr_threads)
{
    char name[THREAD_NAME_SIZE];
    struct thread_attr attr;
    struct thread *thread;
    struct cpumap *cpumap;
    struct test *test;
    int error;

    assert(nr_threads);

    test = kmem_alloc(sizeof(*test));

    if (!test) {
        panic("test: unable to allocate memory");
    }

    mutex_init(&test->mutex);
    test->counter = 0;

    error = cpumap_create(&cpumap);
    error_check(error, "cpumap_create");

    for (size_t i = 0; i < nr_threads; i++) {
        cpumap_zero(cpumap);
        cpumap_set(cpumap, i % 3);
        snprintf(name, sizeof(name), THREAD_KERNEL_PREFIX "test_run:%u/%zu",
                 nr_threads, i);
        thread_attr_init(&attr, name);
        thread_attr_set_detached(&attr);
        thread_attr_set_cpumap(&attr, cpumap);

        if (i < 2) {
            thread_attr_set_policy(&attr, THREAD_SCHED_POLICY_RR);
            thread_attr_set_priority(&attr, THREAD_SCHED_RT_PRIO_MIN + i);
        }

        error = thread_create(&thread, &attr, test_run, test);
        error_check(error, "thread_create");
    }

    return test;
}

static void
test_report_syscnt(struct timer *timer)
{
    uint64_t time;

#ifdef CONFIG_MUTEX_PI
    syscnt_info("rtmutex");
#else /* CONFIG_MUTEX_PI */
    syscnt_info("mutex");
#endif /* CONFIG_MUTEX_PI */

    time = timer_get_time(timer) + clock_ticks_from_ms(TEST_REPORT_INTERVAL);
    timer_schedule(timer, time);
}

void __init
test_setup(void)
{
    uint64_t time;

    if (cpu_count() < TEST_MIN_CPUS) {
        panic("test: at least %u processors are required", TEST_MIN_CPUS);
    }

    test_create(1);
    test_create(2);
    test_create(3);
    test_create(10);

    timer_init(&test_timer, test_report_syscnt, TIMER_DETACHED);
    time = clock_get_time() + clock_ticks_from_ms(TEST_REPORT_INTERVAL);
    timer_schedule(&test_timer, time);
}