code:
/*
 * Copyright (c) 2014-2018 Remy Noel.
 * 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/>.
 *
 *
 * Locking order :
 *
 *             thread_runq -+
 *                          |
 *   event -+-> interrupts -+-> td
 *          |
 *          +-> pmu
 *
 * TODO Kernel/user mode seggregation.
 */

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

#include <kern/clock.h>
#include <kern/init.h>
#include <kern/list.h>
#include <kern/log.h>
#include <kern/macros.h>
#include <kern/percpu.h>
#include <kern/perfmon.h>
#include <kern/perfmon_types.h>
#include <kern/spinlock.h>
#include <kern/syscnt.h>
#include <kern/thread.h>
#include <kern/timer.h>
#include <kern/xcall.h>
#include <machine/boot.h>
#include <machine/cpu.h>

/*
 * Minimum hardware counter poll interval, in milliseconds.
 *
 * The main purpose of polling hardware counters is to detect overflows
 * when the driver is unable to reliably use overflow interrupts.
 */
#define PERFMON_MIN_POLL_INTERVAL 50

/*
 * Internal event flags.
 */
#define PERFMON_EF_TYPE_CPU         0x100
#define PERFMON_EF_ATTACHED         0x200
#define PERFMON_EF_PUBLIC_MASK      (PERFMON_EF_KERN \
                                     | PERFMON_EF_USER \
                                     | PERFMON_EF_RAW)

/*
 * Per-CPU performance monitoring counter.
 *
 * When an event is attached to a processor, the matching per-CPU PMC get
 * referenced. When a per-CPU PMC is referenced, its underlying hardware
 * counter is active.
 *
 * Interrupts and preemption must be disabled on access.
 */
struct perfmon_cpu_pmc {
    unsigned int nr_refs;
    unsigned int pmc_id;
    unsigned int raw_event_id;
    uint64_t raw_value;
    uint64_t value;
};

/*
 * Per-CPU performance monitoring unit.
 *
 * Per-CPU PMCs are indexed the same way as global PMCs.
 *
 * Interrupts and preemption must be disabled on access.
 */
struct perfmon_cpu_pmu {
    struct perfmon_dev *dev;
    unsigned int cpu;
    struct perfmon_cpu_pmc pmcs[PERFMON_MAX_PMCS];
    struct timer poll_timer;
    struct syscnt sc_nr_overflows;
};

/*
 * Performance monitoring counter.
 *
 * When a PMC is used, it maps a raw event to a hardware counter.
 * A PMC is used if and only if its reference counter isn't zero.
 */
struct perfmon_pmc {
    unsigned int nr_refs;
    unsigned int pmc_id;
    unsigned int raw_event_id;
};

/*
 * Performance monitoring unit.
 *
 * There is a single system-wide logical PMU, used to globally allocate
 * PMCs. Reserving a counter across the entire system ensures thread
 * migration isn't hindered by performance monitoring.
 *
 * Locking the global PMU is only required when allocating or releasing
 * a PMC. Once allocated, the PMC may safely be accessed without hodling
 * the lock.
 */
struct perfmon_pmu {
    struct perfmon_dev *dev;
    struct spinlock lock;
    struct perfmon_pmc pmcs[PERFMON_MAX_PMCS];
};

static struct perfmon_pmu perfmon_pmu;
static struct perfmon_cpu_pmu perfmon_cpu_pmu __percpu;

static struct perfmon_pmu *
perfmon_get_pmu(void)
{
    return &perfmon_pmu;
}

static struct perfmon_cpu_pmu *
perfmon_get_local_cpu_pmu(void)
{
    assert(!thread_preempt_enabled());
    return cpu_local_ptr(perfmon_cpu_pmu);
}

static struct perfmon_cpu_pmu *
perfmon_get_cpu_pmu(unsigned int cpu)
{
    return percpu_ptr(perfmon_cpu_pmu, cpu);
}

static void __init
perfmon_pmc_init(struct perfmon_pmc *pmc)
{
    pmc->nr_refs = 0;
}

static bool
perfmon_pmc_used(const struct perfmon_pmc *pmc)
{
    return pmc->nr_refs != 0;
}

static unsigned int
perfmon_pmc_id(const struct perfmon_pmc *pmc)
{
    return pmc->pmc_id;
}

static unsigned int
perfmon_pmc_raw_event_id(const struct perfmon_pmc *pmc)
{
    return pmc->raw_event_id;
}

static void
perfmon_pmc_use(struct perfmon_pmc *pmc, unsigned int pmc_id,
                unsigned int raw_event_id)
{
    assert(!perfmon_pmc_used(pmc));

    pmc->nr_refs = 1;
    pmc->pmc_id = pmc_id;
    pmc->raw_event_id = raw_event_id;
}

static void
perfmon_pmc_ref(struct perfmon_pmc *pmc)
{
    assert(perfmon_pmc_used(pmc));
    pmc->nr_refs++;
}

static void
perfmon_pmc_unref(struct perfmon_pmc *pmc)
{
    assert(perfmon_pmc_used(pmc));
    pmc->nr_refs--;
}

static unsigned int
perfmon_pmu_get_pmc_index(const struct perfmon_pmu *pmu,
                          const struct perfmon_pmc *pmc)
{
    size_t pmc_index;

    pmc_index = pmc - pmu->pmcs;
    assert(pmc_index < ARRAY_SIZE(pmu->pmcs));
    return pmc_index;
}

static struct perfmon_pmc *
perfmon_pmu_get_pmc(struct perfmon_pmu *pmu, unsigned int index)
{
    assert(index < ARRAY_SIZE(pmu->pmcs));
    return &pmu->pmcs[index];
}

static void __init
perfmon_pmu_init(struct perfmon_pmu *pmu)
{
    pmu->dev = NULL;
    spinlock_init(&pmu->lock);

    for (unsigned int i = 0; i < ARRAY_SIZE(pmu->pmcs); i++) {
        perfmon_pmc_init(perfmon_pmu_get_pmc(pmu, i));
    }
}

static void __init
perfmon_pmu_set_dev(struct perfmon_pmu *pmu, struct perfmon_dev *dev)
{
    assert(dev);
    assert(!pmu->dev);
    pmu->dev = dev;
}

static struct perfmon_dev *
perfmon_pmu_get_dev(const struct perfmon_pmu *pmu)
{
    return pmu->dev;
}

static void
perfmon_pmu_handle_overflow_intr(const struct perfmon_pmu *pmu)
{
    pmu->dev->ops->handle_overflow_intr();
}

static int
perfmon_pmu_translate(const struct perfmon_pmu *pmu,
                      unsigned int *raw_event_id,
                      unsigned int event_id)
{
    if (!pmu->dev) {
        return ENODEV;
    }

    return pmu->dev->ops->translate(raw_event_id, event_id);
}

static int
perfmon_pmu_alloc_pmc_id(const struct perfmon_pmu *pmu,
                         unsigned int *pmc_idp,
                         unsigned int pmc_index,
                         unsigned int raw_event_id)
{
    unsigned int pmc_id;
    int error;

    if (!pmu->dev) {
        return ENODEV;
    }

    error = pmu->dev->ops->alloc(&pmc_id, pmc_index, raw_event_id);

    if (error) {
        return error;
    }

    *pmc_idp = pmc_id;
    return 0;
}

static void
perfmon_pmu_free_pmc_id(const struct perfmon_pmu *pmu, unsigned int pmc_id)
{
    assert(pmu->dev);
    pmu->dev->ops->free(pmc_id);
}

static struct perfmon_pmc *
perfmon_pmu_find_unused_pmc(struct perfmon_pmu *pmu)
{
    struct perfmon_pmc *pmc;

    for (unsigned int i = 0; i < ARRAY_SIZE(pmu->pmcs); i++) {
        pmc = perfmon_pmu_get_pmc(pmu, i);

        if (!perfmon_pmc_used(pmc)) {
            return pmc;
        }
    }

    return NULL;
}

static int
perfmon_pmu_alloc_pmc(struct perfmon_pmu *pmu, struct perfmon_pmc **pmcp,
                      unsigned int raw_event_id)
{
    unsigned int pmc_id = 0, pmc_index;
    struct perfmon_pmc *pmc;
    int error;

    pmc = perfmon_pmu_find_unused_pmc(pmu);

    if (!pmc) {
        return EAGAIN;
    }

    pmc_index = perfmon_pmu_get_pmc_index(pmu, pmc);
    error = perfmon_pmu_alloc_pmc_id(pmu, &pmc_id, pmc_index, raw_event_id);

    if (error) {
        return error;
    }

    perfmon_pmc_use(pmc, pmc_id, raw_event_id);
    *pmcp = pmc;
    return 0;
}

static void
perfmon_pmu_free_pmc(struct perfmon_pmu *pmu, struct perfmon_pmc *pmc)
{
    unsigned int pmc_id;

    assert(!perfmon_pmc_used(pmc));
    pmc_id = perfmon_pmc_id(pmc);
    perfmon_pmu_free_pmc_id(pmu, pmc_id);
}

static struct perfmon_pmc *
perfmon_pmu_get_pmc_by_raw_event_id(struct perfmon_pmu *pmu,
                                    unsigned int raw_event_id)
{
    struct perfmon_pmc *pmc;

    for (unsigned int i = 0; i < ARRAY_SIZE(pmu->pmcs); i++) {
        pmc = perfmon_pmu_get_pmc(pmu, i);

        if (!perfmon_pmc_used(pmc)) {
            continue;
        }

        if (perfmon_pmc_raw_event_id(pmc) == raw_event_id) {
            return pmc;
        }
    }

    return NULL;
}

static int
perfmon_pmu_take_pmc(struct perfmon_pmu *pmu, struct perfmon_pmc **pmcp,
                     unsigned int raw_event_id)
{
    struct perfmon_pmc *pmc;
    int error;

    spinlock_lock(&pmu->lock);

    pmc = perfmon_pmu_get_pmc_by_raw_event_id(pmu, raw_event_id);

    if (pmc) {
        perfmon_pmc_ref(pmc);
        error = 0;
    } else {
        error = perfmon_pmu_alloc_pmc(pmu, &pmc, raw_event_id);

        if (error) {
            pmc = NULL;
        }
    }

    spinlock_unlock(&pmu->lock);

    if (error) {
        return error;
    }

    *pmcp = pmc;
    return 0;
}

static void
perfmon_pmu_put_pmc(struct perfmon_pmu *pmu, struct perfmon_pmc *pmc)
{
    spinlock_lock(&pmu->lock);

    perfmon_pmc_unref(pmc);

    if (!perfmon_pmc_used(pmc)) {
        perfmon_pmu_free_pmc(pmu, pmc);
    }

    spinlock_unlock(&pmu->lock);
}

static int
perfmon_check_event_args(unsigned int id, unsigned int flags)
{
    if (!((flags & PERFMON_EF_PUBLIC_MASK) == flags)
        || !((flags & PERFMON_EF_RAW) || (id < PERFMON_NR_GENERIC_EVENTS))
        || !((flags & (PERFMON_EF_KERN | PERFMON_EF_USER)))) {
        return EINVAL;
    }

    return 0;
}

int
perfmon_event_init(struct perfmon_event *event, unsigned int id,
                   unsigned int flags)
{
    int error;

    error = perfmon_check_event_args(id, flags);

    if (error) {
        return error;
    }

    spinlock_init(&event->lock);
    event->flags = flags;
    event->id = id;
    event->value = 0;
    return 0;
}

static bool
perfmon_event_type_cpu(const struct perfmon_event *event)
{
    return event->flags & PERFMON_EF_TYPE_CPU;
}

static void
perfmon_event_set_type_cpu(struct perfmon_event *event)
{
    event->flags |= PERFMON_EF_TYPE_CPU;
}

static void
perfmon_event_clear_type_cpu(struct perfmon_event *event)
{
    event->flags &= ~PERFMON_EF_TYPE_CPU;
}

static bool
perfmon_event_attached(const struct perfmon_event *event)
{
    return event->flags & PERFMON_EF_ATTACHED;
}

static unsigned int
perfmon_event_pmc_index(const struct perfmon_event *event)
{
    assert(perfmon_event_attached(event));
    return event->pmc_index;
}

static void __init
perfmon_cpu_pmc_init(struct perfmon_cpu_pmc *cpu_pmc)
{
    cpu_pmc->nr_refs = 0;
}

static bool
perfmon_cpu_pmc_used(const struct perfmon_cpu_pmc *cpu_pmc)
{
    return cpu_pmc->nr_refs != 0;
}

static void
perfmon_cpu_pmc_use(struct perfmon_cpu_pmc *cpu_pmc, unsigned int pmc_id,
                    unsigned int raw_event_id, uint64_t raw_value)
{
    assert(!perfmon_cpu_pmc_used(cpu_pmc));

    cpu_pmc->nr_refs = 1;
    cpu_pmc->pmc_id = pmc_id;
    cpu_pmc->raw_event_id = raw_event_id;
    cpu_pmc->raw_value = raw_value;
    cpu_pmc->value = 0;
}

static void
perfmon_cpu_pmc_ref(struct perfmon_cpu_pmc *cpu_pmc)
{
    assert(perfmon_cpu_pmc_used(cpu_pmc));
    cpu_pmc->nr_refs++;
}

static void
perfmon_cpu_pmc_unref(struct perfmon_cpu_pmc *cpu_pmc)
{
    assert(perfmon_cpu_pmc_used(cpu_pmc));
    cpu_pmc->nr_refs--;
}

static unsigned int
perfmon_cpu_pmc_id(const struct perfmon_cpu_pmc *cpu_pmc)
{
    return cpu_pmc->pmc_id;
}

static bool
perfmon_cpu_pmc_update(struct perfmon_cpu_pmc *cpu_pmc, uint64_t raw_value,
                       unsigned int pmc_width)
{
    bool overflowed;
    uint64_t delta;

    delta = raw_value - cpu_pmc->raw_value;

    if (pmc_width == 64) {
        overflowed = false;
    } else {
        if (raw_value >= cpu_pmc->raw_value) {
            overflowed = false;
        } else {
            overflowed = true;
            delta += (uint64_t)1 << pmc_width;
        }
    }

    cpu_pmc->value += delta;
    cpu_pmc->raw_value = raw_value;
    return overflowed;
}

static uint64_t
perfmon_cpu_pmc_get_value(const struct perfmon_cpu_pmc *cpu_pmc)
{
    return cpu_pmc->value;
}

static struct perfmon_cpu_pmc *
perfmon_cpu_pmu_get_pmc(struct perfmon_cpu_pmu *cpu_pmu, unsigned int index)
{
    assert(index < ARRAY_SIZE(cpu_pmu->pmcs));
    return &cpu_pmu->pmcs[index];
}

static void
perfmon_cpu_pmu_start(struct perfmon_cpu_pmu *cpu_pmu, unsigned int pmc_id,
                      unsigned int raw_event_id)
{
    cpu_pmu->dev->ops->start(pmc_id, raw_event_id);
}

static void
perfmon_cpu_pmu_stop(struct perfmon_cpu_pmu *cpu_pmu, unsigned int pmc_id)
{
    cpu_pmu->dev->ops->stop(pmc_id);
}

static uint64_t
perfmon_cpu_pmu_read(const struct perfmon_cpu_pmu *cpu_pmu, unsigned int pmc_id)
{
    return cpu_pmu->dev->ops->read(pmc_id);
}

static void
perfmon_cpu_pmu_use_pmc(struct perfmon_cpu_pmu *cpu_pmu,
                        struct perfmon_cpu_pmc *cpu_pmc,
                        unsigned int pmc_id,
                        unsigned int raw_event_id)
{
    uint64_t raw_value;

    perfmon_cpu_pmu_start(cpu_pmu, pmc_id, raw_event_id);
    raw_value = perfmon_cpu_pmu_read(cpu_pmu, pmc_id);
    perfmon_cpu_pmc_use(cpu_pmc, pmc_id, raw_event_id, raw_value);
}

static void
perfmon_cpu_pmu_update_pmc(struct perfmon_cpu_pmu *cpu_pmu,
                           struct perfmon_cpu_pmc *cpu_pmc)
{
    uint64_t raw_value;
    bool overflowed;

    raw_value = perfmon_cpu_pmu_read(cpu_pmu, perfmon_cpu_pmc_id(cpu_pmc));
    overflowed = perfmon_cpu_pmc_update(cpu_pmc, raw_value,
                                        cpu_pmu->dev->pmc_width);

    if (overflowed) {
        syscnt_inc(&cpu_pmu->sc_nr_overflows);
    }
}

static void
perfmon_cpu_pmu_check_overflow(void *arg)
{
    struct perfmon_cpu_pmu *cpu_pmu;
    struct perfmon_cpu_pmc *cpu_pmc;

    assert(!cpu_intr_enabled());

    cpu_pmu = arg;
    assert(cpu_pmu->cpu == cpu_id());

    for (unsigned int i = 0; i < ARRAY_SIZE(cpu_pmu->pmcs); i++) {
        cpu_pmc = perfmon_cpu_pmu_get_pmc(cpu_pmu, i);

        if (!perfmon_cpu_pmc_used(cpu_pmc)) {
            continue;
        }

        perfmon_cpu_pmu_update_pmc(cpu_pmu, cpu_pmc);
    }
}

static void
perfmon_cpu_pmu_poll(struct timer *timer)
{
    struct perfmon_cpu_pmu *cpu_pmu;

    cpu_pmu = structof(timer, struct perfmon_cpu_pmu, poll_timer);
    xcall_call(perfmon_cpu_pmu_check_overflow, cpu_pmu, cpu_pmu->cpu);
    timer_schedule(timer, timer_get_time(timer) + cpu_pmu->dev->poll_interval);
}

static void __init
perfmon_cpu_pmu_init(struct perfmon_cpu_pmu *cpu_pmu, unsigned int cpu,
                     struct perfmon_dev *dev)
{
    char name[SYSCNT_NAME_SIZE];

    cpu_pmu->dev = dev;
    cpu_pmu->cpu = cpu;

    for (unsigned int i = 0; i < ARRAY_SIZE(cpu_pmu->pmcs); i++) {
        perfmon_cpu_pmc_init(perfmon_cpu_pmu_get_pmc(cpu_pmu, i));
    }

    if (dev->ops->handle_overflow_intr == NULL) {
        assert(dev->poll_interval != 0);

        /*
         * XXX Ideally, this would be an interrupt timer instead of a high
         * priority one, but it can't be because the handler performs
         * cross-calls to remote processors, which requires that interrupts
         * be enabled. This is one potential user of CPU-bound timers.
         */
        timer_init(&cpu_pmu->poll_timer, perfmon_cpu_pmu_poll, TIMER_HIGH_PRIO);
        timer_schedule(&cpu_pmu->poll_timer, dev->poll_interval);
    }

    snprintf(name, sizeof(name), "perfmon_nr_overflows/%u", cpu);
    syscnt_register(&cpu_pmu->sc_nr_overflows, name);
}

static uint64_t
perfmon_cpu_pmu_load(struct perfmon_cpu_pmu *cpu_pmu, unsigned int pmc_index,
                     unsigned int pmc_id, unsigned int raw_event_id)
{
    struct perfmon_cpu_pmc *cpu_pmc;

    assert(!cpu_intr_enabled());

    cpu_pmc = perfmon_cpu_pmu_get_pmc(cpu_pmu, pmc_index);

    if (perfmon_cpu_pmc_used(cpu_pmc)) {
        perfmon_cpu_pmc_ref(cpu_pmc);
        perfmon_cpu_pmu_update_pmc(cpu_pmu, cpu_pmc);
    } else {
        perfmon_cpu_pmu_use_pmc(cpu_pmu, cpu_pmc, pmc_id, raw_event_id);
    }

    return perfmon_cpu_pmc_get_value(cpu_pmc);
}

static uint64_t
perfmon_cpu_pmu_unload(struct perfmon_cpu_pmu *cpu_pmu, unsigned int pmc_index)
{
    struct perfmon_cpu_pmc *cpu_pmc;
    unsigned int pmc_id;
    uint64_t value;

    assert(!cpu_intr_enabled());

    cpu_pmc = perfmon_cpu_pmu_get_pmc(cpu_pmu, pmc_index);
    pmc_id = perfmon_cpu_pmc_id(cpu_pmc);

    perfmon_cpu_pmu_update_pmc(cpu_pmu, cpu_pmc);
    value = perfmon_cpu_pmc_get_value(cpu_pmc);

    perfmon_cpu_pmc_unref(cpu_pmc);

    if (!perfmon_cpu_pmc_used(cpu_pmc)) {
        perfmon_cpu_pmu_stop(cpu_pmu, pmc_id);
    }

    return value;
}

static uint64_t
perfmon_cpu_pmu_sync(struct perfmon_cpu_pmu *cpu_pmu, unsigned int pmc_index)
{
    struct perfmon_cpu_pmc *cpu_pmc;

    assert(!cpu_intr_enabled());

    cpu_pmc = perfmon_cpu_pmu_get_pmc(cpu_pmu, pmc_index);
    perfmon_cpu_pmu_update_pmc(cpu_pmu, cpu_pmc);
    return perfmon_cpu_pmc_get_value(cpu_pmc);
}

static void
perfmon_td_pmc_init(struct perfmon_td_pmc *td_pmc)
{
    td_pmc->nr_refs = 0;
    td_pmc->loaded = false;
    td_pmc->value = 0;
}

static bool
perfmon_td_pmc_used(const struct perfmon_td_pmc *td_pmc)
{
    return td_pmc->nr_refs != 0;
}

static void
perfmon_td_pmc_use(struct perfmon_td_pmc *td_pmc, unsigned int pmc_id,
                   unsigned int raw_event_id)
{
    assert(!perfmon_td_pmc_used(td_pmc));

    td_pmc->nr_refs = 1;
    td_pmc->loaded = false;
    td_pmc->pmc_id = pmc_id;
    td_pmc->raw_event_id = raw_event_id;
    td_pmc->value = 0;
}

static unsigned int
perfmon_td_pmc_id(const struct perfmon_td_pmc *td_pmc)
{
    return td_pmc->pmc_id;
}

static unsigned int
perfmon_td_pmc_raw_event_id(const struct perfmon_td_pmc *td_pmc)
{
    return td_pmc->raw_event_id;
}

static void
perfmon_td_pmc_ref(struct perfmon_td_pmc *td_pmc)
{
    assert(perfmon_td_pmc_used(td_pmc));
    td_pmc->nr_refs++;
}

static void
perfmon_td_pmc_unref(struct perfmon_td_pmc *td_pmc)
{
    assert(perfmon_td_pmc_used(td_pmc));
    td_pmc->nr_refs--;
}

static bool
perfmon_td_pmc_loaded(const struct perfmon_td_pmc *td_pmc)
{
    return td_pmc->loaded;
}

static void
perfmon_td_pmc_load(struct perfmon_td_pmc *td_pmc, uint64_t cpu_pmc_value)
{
    assert(!perfmon_td_pmc_loaded(td_pmc));

    td_pmc->cpu_pmc_value = cpu_pmc_value;
    td_pmc->loaded = true;
}

static void
perfmon_td_pmc_update(struct perfmon_td_pmc *td_pmc, uint64_t cpu_pmc_value)
{
    uint64_t delta;

    assert(perfmon_td_pmc_loaded(td_pmc));

    delta = cpu_pmc_value - td_pmc->cpu_pmc_value;
    td_pmc->cpu_pmc_value = cpu_pmc_value;
    td_pmc->value += delta;
}

static void
perfmon_td_pmc_unload(struct perfmon_td_pmc *td_pmc, uint64_t cpu_pmc_value)
{
    perfmon_td_pmc_update(td_pmc, cpu_pmc_value);
    td_pmc->loaded = false;
}

static uint64_t
perfmon_td_pmc_read(const struct perfmon_td_pmc *td_pmc)
{
    return td_pmc->value;
}

static unsigned int
perfmon_td_get_pmc_index(const struct perfmon_td *td,
                         const struct perfmon_td_pmc *td_pmc)
{
    size_t pmc_index;

    pmc_index = td_pmc - td->pmcs;
    assert(pmc_index < ARRAY_SIZE(td->pmcs));
    return pmc_index;
}

static struct perfmon_td_pmc *
perfmon_td_get_pmc(struct perfmon_td *td, unsigned int index)
{
    assert(index < ARRAY_SIZE(td->pmcs));
    return &td->pmcs[index];
}

void
perfmon_td_init(struct perfmon_td *td)
{
    spinlock_init(&td->lock);

    for (unsigned int i = 0; i < ARRAY_SIZE(td->pmcs); i++) {
        perfmon_td_pmc_init(perfmon_td_get_pmc(td, i));
    }
}

static void
perfmon_td_load_pmc(struct perfmon_td *td, struct perfmon_td_pmc *td_pmc)
{
    unsigned int pmc_index, pmc_id, raw_event_id;
    struct perfmon_cpu_pmu *cpu_pmu;
    uint64_t cpu_pmc_value;

    cpu_pmu = perfmon_get_local_cpu_pmu();
    pmc_index = perfmon_td_get_pmc_index(td, td_pmc);
    pmc_id = perfmon_td_pmc_id(td_pmc);
    raw_event_id = perfmon_td_pmc_raw_event_id(td_pmc);
    cpu_pmc_value = perfmon_cpu_pmu_load(cpu_pmu, pmc_index,
                                         pmc_id, raw_event_id);
    perfmon_td_pmc_load(td_pmc, cpu_pmc_value);
}

static void
perfmon_td_unload_pmc(struct perfmon_td *td, struct perfmon_td_pmc *td_pmc)
{
    struct perfmon_cpu_pmu *cpu_pmu;
    unsigned int pmc_index;
    uint64_t cpu_pmc_value;

    cpu_pmu = perfmon_get_local_cpu_pmu();
    pmc_index = perfmon_td_get_pmc_index(td, td_pmc);
    cpu_pmc_value = perfmon_cpu_pmu_unload(cpu_pmu, pmc_index);
    perfmon_td_pmc_unload(td_pmc, cpu_pmc_value);
}

static void
perfmon_td_update_pmc(struct perfmon_td *td, struct perfmon_td_pmc *td_pmc)
{
    struct perfmon_cpu_pmu *cpu_pmu;
    unsigned int pmc_index;
    uint64_t cpu_pmc_value;

    cpu_pmu = perfmon_get_local_cpu_pmu();
    pmc_index = perfmon_td_get_pmc_index(td, td_pmc);
    cpu_pmc_value = perfmon_cpu_pmu_sync(cpu_pmu, pmc_index);
    perfmon_td_pmc_update(td_pmc, cpu_pmc_value);
}

void
perfmon_td_load(struct perfmon_td *td)
{
    unsigned int pmc_index, pmc_id, raw_event_id;
    struct perfmon_cpu_pmu *cpu_pmu;
    struct perfmon_td_pmc *td_pmc;
    uint64_t cpu_pmc_value;

    assert(!cpu_intr_enabled());
    assert(!thread_preempt_enabled());

    cpu_pmu = perfmon_get_local_cpu_pmu();

    spinlock_lock(&td->lock);

    for (unsigned int i = 0; i < ARRAY_SIZE(td->pmcs); i++) {
        td_pmc = perfmon_td_get_pmc(td, i);

        if (!perfmon_td_pmc_used(td_pmc) || perfmon_td_pmc_loaded(td_pmc)) {
            continue;
        }

        pmc_index = perfmon_td_get_pmc_index(td, td_pmc);
        pmc_id = perfmon_td_pmc_id(td_pmc);
        raw_event_id = perfmon_td_pmc_raw_event_id(td_pmc);
        cpu_pmc_value = perfmon_cpu_pmu_load(cpu_pmu, pmc_index,
                                             pmc_id, raw_event_id);
        perfmon_td_pmc_load(td_pmc, cpu_pmc_value);
    }

    spinlock_unlock(&td->lock);
}

void
perfmon_td_unload(struct perfmon_td *td)
{
    struct perfmon_cpu_pmu *cpu_pmu;
    struct perfmon_td_pmc *td_pmc;
    unsigned int pmc_index;
    uint64_t cpu_pmc_value;

    assert(!cpu_intr_enabled());
    assert(!thread_preempt_enabled());

    cpu_pmu = perfmon_get_local_cpu_pmu();

    spinlock_lock(&td->lock);

    for (unsigned int i = 0; i < ARRAY_SIZE(td->pmcs); i++) {
        td_pmc = perfmon_td_get_pmc(td, i);

        if (!perfmon_td_pmc_loaded(td_pmc)) {
            continue;
        }

        pmc_index = perfmon_td_get_pmc_index(td, td_pmc);
        cpu_pmc_value = perfmon_cpu_pmu_unload(cpu_pmu, pmc_index);
        perfmon_td_pmc_unload(td_pmc, cpu_pmc_value);
    }

    spinlock_unlock(&td->lock);
}

static void
perfmon_event_load(struct perfmon_event *event, uint64_t pmc_value)
{
    event->pmc_value = pmc_value;
}

static void
perfmon_event_update(struct perfmon_event *event, uint64_t pmc_value)
{
    uint64_t delta;

    delta = pmc_value - event->pmc_value;
    event->value += delta;
    event->pmc_value = pmc_value;
}

static void
perfmon_event_load_cpu_remote(void *arg)
{
    struct perfmon_event *event;
    struct perfmon_cpu_pmu *cpu_pmu;
    const struct perfmon_pmc *pmc;
    struct perfmon_pmu *pmu;
    unsigned int pmc_index;
    uint64_t cpu_pmc_value;

    event = arg;
    cpu_pmu = perfmon_get_local_cpu_pmu();
    pmu = perfmon_get_pmu();
    pmc_index = perfmon_event_pmc_index(event);
    pmc = perfmon_pmu_get_pmc(pmu, pmc_index);
    cpu_pmc_value = perfmon_cpu_pmu_load(cpu_pmu, pmc_index,
                                         perfmon_pmc_id(pmc),
                                         perfmon_pmc_raw_event_id(pmc));
    perfmon_event_load(event, cpu_pmc_value);
}

static void
perfmon_event_load_cpu(struct perfmon_event *event, unsigned int cpu)
{
    perfmon_event_set_type_cpu(event);
    event->cpu = cpu;
    xcall_call(perfmon_event_load_cpu_remote, event, cpu);
}

static void
perfmon_event_load_thread_remote(void *arg)
{
    struct perfmon_event *event;
    struct perfmon_td_pmc *td_pmc;
    struct perfmon_td *td;
    unsigned int pmc_index;
    uint64_t td_pmc_value;

    event = arg;
    pmc_index = perfmon_event_pmc_index(event);
    td = thread_get_perfmon_td(event->thread);
    td_pmc = perfmon_td_get_pmc(td, pmc_index);

    spinlock_lock(&td->lock);

    if (thread_self() == event->thread) {

        if (perfmon_td_pmc_loaded(td_pmc)) {
            perfmon_td_update_pmc(td, td_pmc);
        } else {
            perfmon_td_load_pmc(td, td_pmc);
        }
    }

    td_pmc_value = perfmon_td_pmc_read(td_pmc);

    spinlock_unlock(&td->lock);

    perfmon_event_load(event, td_pmc_value);
}

static void
perfmon_event_load_thread(struct perfmon_event *event, struct thread *thread)
{
    struct perfmon_td_pmc *td_pmc;
    struct perfmon_td *td;
    struct perfmon_pmu *pmu;
    const struct perfmon_pmc *pmc;
    unsigned int pmc_index;
    unsigned long flags;

    pmu = perfmon_get_pmu();

    thread_ref(thread);
    event->thread = thread;

    pmc_index = perfmon_event_pmc_index(event);
    pmc = perfmon_pmu_get_pmc(pmu, pmc_index);
    td = thread_get_perfmon_td(thread);
    td_pmc = perfmon_td_get_pmc(td, pmc_index);

    spinlock_lock_intr_save(&td->lock, &flags);

    if (perfmon_td_pmc_used(td_pmc)) {
        perfmon_td_pmc_ref(td_pmc);
    } else {
        perfmon_td_pmc_use(td_pmc, perfmon_pmc_id(pmc),
                           perfmon_pmc_raw_event_id(pmc));
    }

    spinlock_unlock_intr_restore(&td->lock, flags);

    xcall_call(perfmon_event_load_thread_remote, event, thread_cpu(thread));
}

static void
perfmon_event_unload_cpu_remote(void *arg)
{
    struct perfmon_event *event;
    struct perfmon_cpu_pmu *cpu_pmu;
    unsigned int pmc_index;
    uint64_t cpu_pmc_value;

    event = arg;
    cpu_pmu = perfmon_get_local_cpu_pmu();
    pmc_index = perfmon_event_pmc_index(event);
    cpu_pmc_value = perfmon_cpu_pmu_unload(cpu_pmu, pmc_index);
    perfmon_event_update(event, cpu_pmc_value);
}

static void
perfmon_event_unload_cpu(struct perfmon_event *event)
{
    xcall_call(perfmon_event_unload_cpu_remote, event, event->cpu);
    perfmon_event_clear_type_cpu(event);
}

static void
perfmon_event_unload_thread_remote(void *arg)
{
    struct perfmon_event *event;
    struct perfmon_td_pmc *td_pmc;
    struct perfmon_td *td;
    unsigned int pmc_index;
    uint64_t td_pmc_value;

    event = arg;
    pmc_index = perfmon_event_pmc_index(event);
    td = thread_get_perfmon_td(event->thread);
    td_pmc = perfmon_td_get_pmc(td, pmc_index);

    spinlock_lock(&td->lock);

    if ((thread_self() == event->thread) && perfmon_td_pmc_loaded(td_pmc)) {
        if (perfmon_td_pmc_used(td_pmc)) {
            perfmon_td_update_pmc(td, td_pmc);
        } else {
            perfmon_td_unload_pmc(td, td_pmc);
        }
    }

    td_pmc_value = perfmon_td_pmc_read(td_pmc);

    spinlock_unlock(&td->lock);

    perfmon_event_update(event, td_pmc_value);
}

static void
perfmon_event_unload_thread(struct perfmon_event *event)
{
    struct perfmon_td_pmc *td_pmc;
    struct perfmon_td *td;
    unsigned int pmc_index;
    unsigned long flags;

    pmc_index = perfmon_event_pmc_index(event);
    td = thread_get_perfmon_td(event->thread);
    td_pmc = perfmon_td_get_pmc(td, pmc_index);

    spinlock_lock_intr_save(&td->lock, &flags);
    perfmon_td_pmc_unref(td_pmc);
    spinlock_unlock_intr_restore(&td->lock, flags);

    xcall_call(perfmon_event_unload_thread_remote, event,
               thread_cpu(event->thread));

    thread_unref(event->thread);
    event->thread = NULL;
}

static void
perfmon_event_sync_cpu_remote(void *arg)
{
    struct perfmon_event *event;
    struct perfmon_cpu_pmu *cpu_pmu;
    unsigned int pmc_index;
    uint64_t cpu_pmc_value;

    event = arg;
    cpu_pmu = perfmon_get_local_cpu_pmu();
    pmc_index = perfmon_event_pmc_index(event);
    cpu_pmc_value = perfmon_cpu_pmu_sync(cpu_pmu, pmc_index);
    perfmon_event_update(event, cpu_pmc_value);
}

static void
perfmon_event_sync_cpu(struct perfmon_event *event)
{
    xcall_call(perfmon_event_sync_cpu_remote, event, event->cpu);
}

static void
perfmon_event_sync_thread_remote(void *arg)
{
    struct perfmon_event *event;
    struct perfmon_td_pmc *td_pmc;
    struct perfmon_td *td;
    unsigned int pmc_index;
    uint64_t td_pmc_value;

    event = arg;
    pmc_index = perfmon_event_pmc_index(event);
    td = thread_get_perfmon_td(event->thread);
    td_pmc = perfmon_td_get_pmc(td, pmc_index);

    spinlock_lock(&td->lock);

    if (thread_self() == event->thread) {
        perfmon_td_update_pmc(td, td_pmc);
    }

    td_pmc_value = perfmon_td_pmc_read(td_pmc);

    spinlock_unlock(&td->lock);

    perfmon_event_update(event, td_pmc_value);
}

static void
perfmon_event_sync_thread(struct perfmon_event *event)
{
    xcall_call(perfmon_event_sync_thread_remote, event,
               thread_cpu(event->thread));
}

static int
perfmon_event_attach_pmu(struct perfmon_event *event)
{
    unsigned int raw_event_id = 0;
    struct perfmon_pmu *pmu;
    struct perfmon_pmc *pmc;
    int error;

    pmu = perfmon_get_pmu();

    if (!(event->flags & PERFMON_EF_RAW)) {
        error = perfmon_pmu_translate(pmu, &raw_event_id, event->id);

        if (error) {
            return error;
        }
    }

    error = perfmon_pmu_take_pmc(pmu, &pmc, raw_event_id);

    if (error) {
        return error;
    }

    event->pmc_index = perfmon_pmu_get_pmc_index(pmu, pmc);
    event->flags |= PERFMON_EF_ATTACHED;
    event->value = 0;
    return 0;
}

static void
perfmon_event_detach_pmu(struct perfmon_event *event)
{
    struct perfmon_pmu *pmu;
    struct perfmon_pmc *pmc;

    pmu = perfmon_get_pmu();
    pmc = perfmon_pmu_get_pmc(pmu, perfmon_event_pmc_index(event));
    perfmon_pmu_put_pmc(pmu, pmc);
    event->flags &= ~PERFMON_EF_ATTACHED;
}

int
perfmon_event_attach(struct perfmon_event *event, struct thread *thread)
{
    int error;

    spinlock_lock(&event->lock);

    if (perfmon_event_attached(event)) {
        error = EINVAL;
        goto error;
    }

    error = perfmon_event_attach_pmu(event);

    if (error) {
        goto error;
    }

    perfmon_event_load_thread(event, thread);

    spinlock_unlock(&event->lock);

    return 0;

error:
    spinlock_unlock(&event->lock);

    return error;
}

int
perfmon_event_attach_cpu(struct perfmon_event *event, unsigned int cpu)
{
    int error;

    if (cpu >= cpu_count()) {
        return EINVAL;
    }

    spinlock_lock(&event->lock);

    if (perfmon_event_attached(event)) {
        error = EINVAL;
        goto out;
    }

    error = perfmon_event_attach_pmu(event);

    if (error) {
        goto out;
    }

    perfmon_event_load_cpu(event, cpu);
    error = 0;

out:
    spinlock_unlock(&event->lock);

    return error;
}

int
perfmon_event_detach(struct perfmon_event *event)
{
    int error;

    spinlock_lock(&event->lock);

    if (!perfmon_event_attached(event)) {
        error = EINVAL;
        goto out;
    }

    if (perfmon_event_type_cpu(event)) {
        perfmon_event_unload_cpu(event);
    } else {
        perfmon_event_unload_thread(event);
    }

    perfmon_event_detach_pmu(event);
    error = 0;

out:
    spinlock_unlock(&event->lock);

    return error;
}

uint64_t
perfmon_event_read(struct perfmon_event *event)
{
    uint64_t value;

    spinlock_lock(&event->lock);

    if (perfmon_event_attached(event)) {
        if (perfmon_event_type_cpu(event)) {
            perfmon_event_sync_cpu(event);
        } else {
            perfmon_event_sync_thread(event);
        }
    }

    value = event->value;

    spinlock_unlock(&event->lock);

    return value;
}

static uint64_t __init
perfmon_compute_poll_interval(uint64_t pmc_width)
{
    uint64_t cycles, time;

    if (pmc_width == 64) {
        cycles = (uint64_t)-1;
    } else {
        cycles = (uint64_t)1 << pmc_width;
    }

    /*
     * Assume an unrealistically high upper bound on the number of
     * events per cycle to otbain a comfortable margin of safety.
     */
    cycles /= 100;
    time = cycles / (cpu_get_freq() / 1000);

    if (time < PERFMON_MIN_POLL_INTERVAL) {
        log_warning("perfmon: invalid poll interval %llu, forced to %llu",
                    (unsigned long long)time,
                    (unsigned long long)PERFMON_MIN_POLL_INTERVAL);
        time = PERFMON_MIN_POLL_INTERVAL;
    }

    return clock_ticks_from_ms(time);
}

void __init
perfmon_register(struct perfmon_dev *dev)
{
    const struct perfmon_dev_ops *ops;

    ops = dev->ops;
    assert(ops->translate && ops->alloc && ops->free
           && ops->start && ops->stop && ops->read);
    assert(dev->pmc_width <= 64);

    if ((dev->ops->handle_overflow_intr == NULL) && (dev->poll_interval == 0)) {
        dev->poll_interval = perfmon_compute_poll_interval(dev->pmc_width);
    }

    perfmon_pmu_set_dev(perfmon_get_pmu(), dev);
}

void
perfmon_overflow_intr(void)
{
    perfmon_pmu_handle_overflow_intr(perfmon_get_pmu());
}

void
perfmon_report_overflow(unsigned int pmc_index)
{
    struct perfmon_cpu_pmu *cpu_pmu;
    struct perfmon_cpu_pmc *cpu_pmc;

    assert(!cpu_intr_enabled());
    assert(!thread_preempt_enabled());

    cpu_pmu = perfmon_get_local_cpu_pmu();
    cpu_pmc = perfmon_cpu_pmu_get_pmc(cpu_pmu, pmc_index);
    perfmon_cpu_pmu_update_pmc(cpu_pmu, cpu_pmc);
}

static int __init
perfmon_bootstrap(void)
{
    perfmon_pmu_init(perfmon_get_pmu());
    return 0;
}

INIT_OP_DEFINE(perfmon_bootstrap,
               INIT_OP_DEP(log_setup, true),
               INIT_OP_DEP(spinlock_setup, true));

static int __init
perfmon_setup(void)
{
    struct perfmon_dev *dev;

    dev = perfmon_pmu_get_dev(perfmon_get_pmu());

    if (!dev) {
        return ENODEV;
    }

    for (unsigned int cpu = 0; cpu < cpu_count(); cpu++) {
        perfmon_cpu_pmu_init(perfmon_get_cpu_pmu(cpu), cpu, dev);
    }

    return 0;
}

INIT_OP_DEFINE(perfmon_setup,
               INIT_OP_DEP(boot_setup_pmu, true),
               INIT_OP_DEP(cpu_mp_probe, true),
               INIT_OP_DEP(cpu_setup, true),
               INIT_OP_DEP(percpu_setup, true),
               INIT_OP_DEP(perfmon_bootstrap, true),
               INIT_OP_DEP(spinlock_setup, true),
               INIT_OP_DEP(syscnt_setup, true));


code:
/*
 * Copyright (c) 2014-2018 Remy Noel.
 * 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/>.
 *
 *
 * Performance monitoring based on hardware performance counters.
 *
 * The hardware layer is represented by a performance monitoring unit (PMU),
 * which provides performance monitoring counters (PMCs).
 */

#ifndef KERN_PERFMON_H
#define KERN_PERFMON_H

#include <stdint.h>

#include <kern/init.h>
#include <kern/perfmon_types.h>
#include <kern/thread.h>

/*
 * IDs of generic performance monitoring events.
 */
#define PERFMON_EV_CYCLE            0
#define PERFMON_EV_REF_CYCLE        1
#define PERFMON_EV_INSTRUCTION      2
#define PERFMON_EV_CACHE_REF        3
#define PERFMON_EV_CACHE_MISS       4
#define PERFMON_EV_BRANCH           5
#define PERFMON_EV_BRANCH_MISS      6
#define PERFMON_NR_GENERIC_EVENTS   7

/*
 * Event flags.
 */
#define PERFMON_EF_KERN     0x1 /* Monitor events in kernel mode */
#define PERFMON_EF_USER     0x2 /* Monitor events in user mode */
#define PERFMON_EF_RAW      0x4 /* Raw event ID, generic if unset */

/*
 * Performance monitoring operations.
 *
 * This is a public structure.
 *
 * All operations are either global but serialized by the caller, or
 * processor-local and called with interrupts and preemption disabled.
 *
 * If the hardware doesn't efficiently support overflow interrupts, the
 * handler must be set to NULL, making the perfmon module perdiocally
 * check the raw value of the hardware counters.
 */
struct perfmon_dev_ops {
    /*
     * Convert a generic event ID into a raw event ID.
     *
     * Global operation.
     */
    int (*translate)(unsigned int *raw_event_idp, unsigned int event_id);

    /*
     * Allocate a performance monitoring counter globally for the given
     * raw event ID, and return the counter ID through the given pointer.
     * The driver may return any PMC ID, as long as it uniquely identifies
     * the underlying counter. The PMC index is passed when reporting
     * overflows, if using a custom overflow interrupt handler.
     *
     * Global operation.
     */
    int (*alloc)(unsigned int *pmc_idp, unsigned int pmc_index,
                 unsigned int raw_event_id);

    /*
     * Free an allocated performance monitoring counter.
     *
     * Global operation.
     */
    void (*free)(unsigned int pmc_id);

    /*
     * Start a performance monitoring counter for the given raw event ID.
     *
     * Processor-local operation.
     */
    void (*start)(unsigned int pmc_id, unsigned int raw_event_id);

    /*
     * Stop a performance monitoring counter.
     *
     * Processor-local operation.
     */
    void (*stop)(unsigned int pmc_id);

    /*
     * Read the value of a performance monitoring counter.
     *
     * Processor-local operation.
     */
    uint64_t (*read)(unsigned int pmc_id);

    /*
     * Custom overflow interrupt handler.
     *
     * Processor-local operation.
     */
    void (*handle_overflow_intr)(void);
};

/*
 * Performance monitoring device.
 *
 * This is a public structure.
 *
 * The PMC width is expressed in bits.
 *
 * If the driver doesn't provide an overflow interrupt handler, it may set
 * the poll interval, in ticks, to a duration that safely allows the detection
 * of a single overflow. A value of 0 lets the perfmon module compute a poll
 * interval itself.
 */
struct perfmon_dev {
    const struct perfmon_dev_ops *ops;
    unsigned int pmc_width;
    uint64_t poll_interval;
};

/*
 * Performance monitoring thread data.
 */
struct perfmon_td;

/*
 * Performance monitoring event.
 *
 * An event describes a single, well-defined hardware condition and tracks
 * its occurrences over a period of time.
 */
struct perfmon_event;

/*
 * Initialize thread-specific data.
 */
void perfmon_td_init(struct perfmon_td *td);

/*
 * Load/unload events attached to a thread on the current processor.
 *
 * These functions should only be used by the scheduler on a context switch.
 * Interrupts and preemption must be disabled when calling these functions.
 */
void perfmon_td_load(struct perfmon_td *td);
void perfmon_td_unload(struct perfmon_td *td);

/*
 * Initialize an event.
 */
int perfmon_event_init(struct perfmon_event *event, unsigned int id,
                       unsigned int flags);

/*
 * Attach/detach an event to/from a thread or a processor.
 *
 * Attaching an event allocates hardware resources and enables monitoring.
 * The number of occurrences for the given event is reset.
 *
 * An event can only be attached to one thread or processor at a time.
 */
int perfmon_event_attach(struct perfmon_event *event, struct thread *thread);
int perfmon_event_attach_cpu(struct perfmon_event *event, unsigned int cpu);
int perfmon_event_detach(struct perfmon_event *event);

/*
 * Obtain the number of occurrences of an event.
 */
uint64_t perfmon_event_read(struct perfmon_event *event);

/*
 * Register a PMU device.
 *
 * Currently, there can only be a single system-wide PMU device, which
 * assumes the driver is the same for all processors.
 */
void perfmon_register(struct perfmon_dev *dev);

/*
 * Handle an overflow interrupt.
 *
 * This function must be called in interrupt context.
 */
void perfmon_overflow_intr(void);

/*
 * Report a PMC overflow.
 *
 * This function is intended to be used by PMU drivers using a custom
 * overflow interrupt handler.
 *
 * This function must be called in interrupt context.
 */
void perfmon_report_overflow(unsigned int pmc_index);

/*
 * This init operation provides :
 *  - PMU device registration
 */
INIT_OP_DECLARE(perfmon_bootstrap);

#endif /* KERN_PERFMON_H */

license: mit


code:
/*
 * Copyright (c) 2014-2018 Remy Noel.
 * 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/>.
 *
 *
 * Isolated type definitions used to avoid inclusion circular dependencies.
 */

#ifndef KERN_PERFMON_TYPES_H
#define KERN_PERFMON_TYPES_H

#ifdef CONFIG_PERFMON

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

#include <kern/spinlock_types.h>

/*
 * Maximum number of supported hardware counters.
 */
#define PERFMON_MAX_PMCS CONFIG_PERFMON_MAX_PMCS

/*
 * Performance monitoring event.
 *
 * An event may be unattached, attached to a thread, or attached to a CPU.
 * When it is loaded, the current value of the underlying PMC is saved.
 * When it is updated, the delta between the current and saved PMC values
 * is added to the event value.
 */
struct perfmon_event {
    struct spinlock lock;
    unsigned int flags;
    unsigned int id;
    uint64_t pmc_value;
    uint64_t value;

    union {
        struct thread *thread;
        unsigned int cpu;
    };

    unsigned int pmc_index;
};

/*
 * Per-thread performance monitoring counter.
 *
 * Per-thread PMCs are indexed the same way as global PMCs.
 *
 * A per-thread PMC is referenced when an event is attached to a thread.
 * The PMC may only be loaded if the thread is running on a processor,
 * as a result of an event being attached to the thread, or the thread
 * being dispatched by the scheduler. Note that this allows a transient
 * state to be seen where a per-thread PMC is both unused and loaded.
 * This happens after detaching an event from a thread, resulting in
 * the underlying per-thread PMC to become unused, but if the thread
 * is running concurrently, the counter is still loaded. The implementation
 * resolves the situation by unloading the counter, which is either
 * done by an explicit unload cross-call, or when the scheduler preempts
 * the thread and unloads its thread data.
 *
 * When a per-thread PMC is loaded, the current value of the underlying
 * PMC is saved, and when it's updated, the delta between the current
 * and saved PMC values is added to the per-thread PMC value.
 */
struct perfmon_td_pmc {
    unsigned int nr_refs;
    bool loaded;
    unsigned int pmc_id;
    unsigned int raw_event_id;
    uint64_t cpu_pmc_value;
    uint64_t value;
};

/*
 * Per-thread performance monitoring data.
 *
 * Interrupts must be disabled when locking thread data.
 */
struct perfmon_td {
    struct spinlock lock;
    struct perfmon_td_pmc pmcs[PERFMON_MAX_PMCS];
};

#endif /* CONFIG_PERFMON */

#endif /* KERN_PERFMON_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/>.
 *
 * Upstream site with license notes :
 * http://git.sceen.net/rbraun/librbraun.git/
 */

#include <kern/list.h>
#include <kern/plist.h>

void
plist_add(struct plist *plist, struct plist_node *pnode)
{
    struct plist_node *next;

    if (plist_empty(plist)) {
        list_insert_head(&plist->list, &pnode->node);
        list_insert_head(&plist->prio_list, &pnode->prio_node);
        return;
    }

    list_for_each_entry(&plist->prio_list, next, prio_node) {
        if (pnode->priority < next->priority) {
            break;
        }
    }

    if (list_end(&plist->prio_list, &next->prio_node)
        || (pnode->priority != next->priority)) {
        list_insert_before(&pnode->prio_node, &next->prio_node);
    } else {
        list_init(&pnode->prio_node);
    }

    list_insert_before(&pnode->node, &next->node);
}

void
plist_remove(struct plist *plist, struct plist_node *pnode)
{
    struct plist_node *next;

    if (!list_node_unlinked(&pnode->prio_node)) {
        next = list_next_entry(pnode, node);

        if (!list_end(&plist->list, &next->node)
            && list_node_unlinked(&next->prio_node)) {
            list_insert_after(&pnode->prio_node, &next->prio_node);
        }

        list_remove(&pnode->prio_node);
    }

    list_remove(&pnode->node);
}


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/>.
 *
 * Upstream site with license notes :
 * http://git.sceen.net/rbraun/librbraun.git/
 *
 *
 * Priority list.
 *
 * This container acts as a doubly-linked list sorted by priority in
 * ascending order. All operations behave as with a regular linked list
 * except insertion, which is O(k), k being the number of priorities
 * among the entries.
 */

#ifndef KERN_PLIST_H
#define KERN_PLIST_H

#include <stdbool.h>

#include <kern/list.h>
#include <kern/macros.h>
#include <kern/plist_types.h>

/*
 * Priority list.
 */
struct plist;

/*
 * Priority list node.
 */
struct plist_node;

/*
 * Static priority list initializer.
 */
#define PLIST_INITIALIZER(plist) \
    { LIST_INITIALIZER((plist).list), LIST_INITIALIZER((plist).prio_list) }

/*
 * Initialize a priority list.
 */
static inline void
plist_init(struct plist *plist)
{
    list_init(&plist->list);
    list_init(&plist->prio_list);
}

/*
 * Initialize a priority list node.
 */
static inline void
plist_node_init(struct plist_node *pnode, unsigned int priority)
{
    pnode->priority = priority;
    list_node_init(&pnode->node);
    list_node_init(&pnode->prio_node);
}

/*
 * Return the priority associated with a node.
 */
static inline unsigned int
plist_node_priority(const struct plist_node *pnode)
{
    return pnode->priority;
}

/*
 * Update the priority of an already initialized node.
 */
static inline void
plist_node_set_priority(struct plist_node *pnode, unsigned int priority)
{
    pnode->priority = priority;
}

/*
 * Return true if pnode is in no priority lists.
 */
static inline bool
plist_node_unlinked(const struct plist_node *pnode)
{
    return list_node_unlinked(&pnode->node);
}

/*
 * Macro that evaluates to the address of the structure containing the
 * given node based on the given type and member.
 */
#define plist_entry(pnode, type, member) structof(pnode, type, member)

/*
 * Return the first node of a priority list.
 */
static inline struct plist_node *
plist_first(const struct plist *plist)
{
    return list_first_entry(&plist->list, struct plist_node, node);
}

/*
 * Return the last node of a priority list.
 */
static inline struct plist_node *
plist_last(const struct plist *plist)
{
    return list_last_entry(&plist->list, struct plist_node, node);
}

/*
 * Return the node next to the given node.
 */
static inline struct plist_node *
plist_next(const struct plist_node *pnode)
{
    return (struct plist_node *)list_next_entry(pnode, node);
}

/*
 * Return the node previous to the given node.
 */
static inline struct plist_node *
plist_prev(const struct plist_node *pnode)
{
    return (struct plist_node *)list_prev_entry(pnode, node);
}

/*
 * Get the first entry of a priority list.
 */
#define plist_first_entry(plist, type, member) \
    plist_entry(plist_first(plist), type, member)

/*
 * Get the last entry of a priority list.
 */
#define plist_last_entry(plist, type, member) \
    plist_entry(plist_last(plist), type, member)

/*
 * Get the entry next to the given entry.
 */
#define plist_next_entry(entry, member) \
    plist_entry(plist_next(&(entry)->member), typeof(*(entry)), member)

/*
 * Get the entry previous to the given entry.
 */
#define plist_prev_entry(entry, member) \
    plist_entry(plist_prev(&(entry)->member), typeof(*(entry)), member)

/*
 * Return true if node is after the last or before the first node of
 * a priority list.
 */
static inline bool
plist_end(const struct plist *plist, const struct plist_node *pnode)
{
    return list_end(&plist->list, &pnode->node);
}

/*
 * Return true if plist is empty.
 */
static inline bool
plist_empty(const struct plist *plist)
{
    return list_empty(&plist->list);
}

/*
 * Return true if plist contains exactly one node.
 */
static inline bool
plist_singular(const struct plist *plist)
{
    return list_singular(&plist->list);
}

/*
 * Add a node to a priority list.
 *
 * If the priority list already contains nodes with the same priority
 * as the given node, it is inserted before them.
 *
 * The node must be initialized before calling this function.
 */
void plist_add(struct plist *plist, struct plist_node *pnode);

/*
 * Remove a node from a priority list.
 *
 * After completion, the node is stale.
 */
void plist_remove(struct plist *plist, struct plist_node *pnode);

/*
 * Forge a loop to process all nodes of a priority list.
 *
 * The node must not be altered during the loop.
 */
#define plist_for_each(plist, pnode)    \
for (pnode = plist_first(plist);        \
     !plist_end(plist, pnode);          \
     pnode = plist_next(pnode))

/*
 * Forge a loop to process all nodes of a priority list.
 */
#define plist_for_each_safe(plist, pnode, tmp)              \
for (pnode = plist_first(plist), tmp = plist_next(pnode);   \
     !plist_end(plist, pnode);                              \
     pnode = tmp, tmp = plist_next(pnode))

/*
 * Version of plist_for_each() that processes nodes backward.
 */
#define plist_for_each_reverse(plist, pnode)    \
for (pnode = plist_last(plist);                 \
     !plist_end(plist, pnode);                  \
     pnode = plist_prev(pnode))

/*
 * Version of plist_for_each_safe() that processes nodes backward.
 */
#define plist_for_each_reverse_safe(plist, pnode, tmp)      \
for (pnode = plist_last(plist), tmp = plist_prev(pnode);    \
     !plist_end(plist, pnode);                              \
     pnode = tmp, tmp = plist_prev(pnode))

/*
 * Forge a loop to process all entries of a priority list.
 *
 * The entry node must not be altered during the loop.
 */
#define plist_for_each_entry(plist, entry, member) \
    list_for_each_entry(&(plist)->list, entry, member.node)

/*
 * Forge a loop to process all entries of a priority list.
 */
#define plist_for_each_entry_safe(plist, entry, tmp, member) \
    list_for_each_entry_safe(&(plist)->list, entry, tmp, member.node)

/*
 * Version of plist_for_each_entry() that processes entries backward.
 */
#define plist_for_each_entry_reverse(plist, entry, member) \
    list_for_each_entry_reverse(&(plist)->list, entry, member.node)

/*
 * Version of plist_for_each_entry_safe() that processes entries backward.
 */
#define plist_for_each_entry_reverse_safe(plist, entry, tmp, member) \
    list_for_each_entry_reverse_safe(&(plist)->list, entry, tmp, member.node)

#endif /* KERN_PLIST_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_PLIST_TYPES_H
#define KERN_PLIST_TYPES_H

#include <kern/list_types.h>

/*
 * The list member is used as the underlying regular linked list and
 * contains all entries, sorted by priority in ascending order. The
 * prio_list member contains exactly one entry for each priority
 * present, also sorted by priority in ascending order. An entry
 * on prio_list is the first entry in list for that priority.
 * Here is a representation of a possible priority list :
 *
 *      list--|1|--|3|--|3|--|3|--|4|--|6|--|6|--|8|
 *            | |  | |            | |  | |       |
 * prio_list--+ +--+ +------------+ +--+ +-------+
 *
 */
struct plist {
    struct list list;
    struct list prio_list;
};

struct plist_node {
    unsigned int priority;
    struct list node;
    struct list prio_node;
};

#endif /* KERN_PLIST_TYPES_H */


code:
/*
 * Copyright (c) 2010, 2012, 2013 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 <stdio.h>

#include <kern/console.h>
#include <kern/fmt.h>
#include <kern/init.h>
#include <kern/spinlock.h>
#include <machine/boot.h>
#include <machine/cpu.h>

/*
 * Size of the static buffer.
 */
#define PRINTF_BUFSIZE 1024

static char printf_buffer[PRINTF_BUFSIZE];
static struct spinlock printf_lock;

int
printf(const char *format, ...)
{
    va_list ap;
    int length;

    va_start(ap, format);
    length = vprintf(format, ap);
    va_end(ap);

    return length;
}

int
vprintf(const char *format, va_list ap)
{
    unsigned long flags;
    int length;
    char *ptr;

    spinlock_lock_intr_save(&printf_lock, &flags);

    length = fmt_vsnprintf(printf_buffer, sizeof(printf_buffer), format, ap);

    for (ptr = printf_buffer; *ptr != '\0'; ptr++) {
        console_putchar(*ptr);
    }

    spinlock_unlock_intr_restore(&printf_lock, flags);

    return length;
}

static int __init
printf_setup(void)
{
    spinlock_init(&printf_lock);
    return 0;
}

INIT_OP_DEFINE(printf_setup,
               INIT_OP_DEP(boot_bootstrap_console, true),
               INIT_OP_DEP(console_bootstrap, true),
               INIT_OP_DEP(spinlock_setup, true));


code:
/*
 * Copyright (c) 2010 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/>.
 *
 *
 * Formatted output functions.
 *
 * The printf() and vprintf() functions internally use a statically
 * allocated buffer. They won't produce output larger than 1 KiB. They can
 * be used safely in any context.
 *
 * See the sprintf module for information about the supported formats.
 */

#ifndef KERN_PRINTF_H
#define KERN_PRINTF_H

#ifndef STDIO_H
#error "do not use <kern/printf.h> directly; include <stdio.h> instead"
#endif /* STDIO_H */

#include <stdarg.h>

#include <kern/init.h>

int printf(const char *format, ...)
    __attribute__((format(printf, 1, 2)));

int vprintf(const char *format, va_list ap)
    __attribute__((format(printf, 1, 0)));

/*
 * This init operation provides :
 *  - printf is usable
 */
INIT_OP_DECLARE(printf_setup);

#endif /* KERN_PRINTF_H */


code:
/*
 * Copyright (c) 2010-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/>.
 *
 * Upstream site with license notes :
 * http://git.sceen.net/rbraun/librbraun.git/
 */

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

#include <kern/macros.h>
#include <kern/rbtree.h>
#include <kern/rbtree_i.h>

/*
 * Return the index of a node in the children array of its parent.
 *
 * The parent parameter must not be NULL, and must be the parent of the
 * given node.
 */
static inline int
rbtree_node_index(const struct rbtree_node *node,
                  const struct rbtree_node *parent)
{
    assert(parent != NULL);
    assert((node == NULL) || (rbtree_node_parent(node) == parent));

    if (parent->children[RBTREE_LEFT] == node) {
        return RBTREE_LEFT;
    }

    assert(parent->children[RBTREE_RIGHT] == node);

    return RBTREE_RIGHT;
}

/*
 * Return the color of a node.
 */
static inline int
rbtree_node_color(const struct rbtree_node *node)
{
    return node->parent & RBTREE_COLOR_MASK;
}

/*
 * Return true if the node is red.
 */
static inline int
rbtree_node_is_red(const struct rbtree_node *node)
{
    return rbtree_node_color(node) == RBTREE_COLOR_RED;
}

/*
 * Return true if the node is black.
 */
static inline int
rbtree_node_is_black(const struct rbtree_node *node)
{
    return rbtree_node_color(node) == RBTREE_COLOR_BLACK;
}

/*
 * Set the parent of a node, retaining its current color.
 */
static inline void
rbtree_node_set_parent(struct rbtree_node *node, struct rbtree_node *parent)
{
    assert(rbtree_node_check_alignment(node));
    assert(rbtree_node_check_alignment(parent));

    node->parent = (uintptr_t)parent | (node->parent & RBTREE_COLOR_MASK);
}

/*
 * Set the color of a node, retaining its current parent.
 */
static inline void
rbtree_node_set_color(struct rbtree_node *node, int color)
{
    assert((color & ~RBTREE_COLOR_MASK) == 0);
    node->parent = (node->parent & RBTREE_PARENT_MASK) | color;
}

/*
 * Set the color of a node to red, retaining its current parent.
 */
static inline void
rbtree_node_set_red(struct rbtree_node *node)
{
    rbtree_node_set_color(node, RBTREE_COLOR_RED);
}

/*
 * Set the color of a node to black, retaining its current parent.
 */
static inline void
rbtree_node_set_black(struct rbtree_node *node)
{
    rbtree_node_set_color(node, RBTREE_COLOR_BLACK);
}

/*
 * Return the left-most deepest child node of the given node.
 */
static struct rbtree_node *
rbtree_node_find_deepest(struct rbtree_node *node)
{
    struct rbtree_node *parent;

    assert(node != NULL);

    for (;;) {
        parent = node;
        node = node->children[RBTREE_LEFT];

        if (node == NULL) {
            node = parent->children[RBTREE_RIGHT];

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

/*
 * Perform a tree rotation, rooted at the given node.
 *
 * The direction parameter defines the rotation direction and is either
 * RBTREE_LEFT or RBTREE_RIGHT.
 */
static void
rbtree_rotate(struct rbtree *tree, struct rbtree_node *node, int direction)
{
    struct rbtree_node *parent, *rnode;
    int left, right;

    left = direction;
    right = 1 - left;
    parent = rbtree_node_parent(node);
    rnode = node->children[right];

    node->children[right] = rnode->children[left];

    if (rnode->children[left] != NULL) {
        rbtree_node_set_parent(rnode->children[left], node);
    }

    rnode->children[left] = node;
    rbtree_node_set_parent(rnode, parent);

    if (unlikely(parent == NULL)) {
        tree->root = rnode;
    } else {
        parent->children[rbtree_node_index(node, parent)] = rnode;
    }

    rbtree_node_set_parent(node, rnode);
}

void
rbtree_insert_rebalance(struct rbtree *tree, struct rbtree_node *parent,
                        int index, struct rbtree_node *node)
{
    struct rbtree_node *grand_parent, *uncle;
    int left, right;

    assert(rbtree_node_check_alignment(parent));
    assert(rbtree_node_check_alignment(node));

    node->parent = (uintptr_t)parent | RBTREE_COLOR_RED;
    node->children[RBTREE_LEFT] = NULL;
    node->children[RBTREE_RIGHT] = NULL;

    if (unlikely(parent == NULL)) {
        tree->root = node;
    } else {
        parent->children[index] = node;
    }

    for (;;) {
        if (parent == NULL) {
            rbtree_node_set_black(node);
            break;
        }

        if (rbtree_node_is_black(parent)) {
            break;
        }

        grand_parent = rbtree_node_parent(parent);
        assert(grand_parent != NULL);

        left = rbtree_node_index(parent, grand_parent);
        right = 1 - left;

        uncle = grand_parent->children[right];

        /*
         * Uncle is red. Flip colors and repeat at grand parent.
         */
        if ((uncle != NULL) && rbtree_node_is_red(uncle)) {
            rbtree_node_set_black(uncle);
            rbtree_node_set_black(parent);
            rbtree_node_set_red(grand_parent);
            node = grand_parent;
            parent = rbtree_node_parent(node);
            continue;
        }

        /*
         * Node is the right child of its parent. Rotate left at parent.
         */
        if (parent->children[right] == node) {
            rbtree_rotate(tree, parent, left);
            parent = node;
        }

        /*
         * Node is the left child of its parent. Handle colors, rotate right
         * at grand parent, and leave.
         */
        rbtree_node_set_black(parent);
        rbtree_node_set_red(grand_parent);
        rbtree_rotate(tree, grand_parent, right);
        break;
    }

    assert(rbtree_node_is_black(tree->root));
}

void *
rbtree_replace_slot(struct rbtree *tree, rbtree_slot_t slot,
                    struct rbtree_node *node)
{
    struct rbtree_node *parent, *prev;
    int index;

    parent = rbtree_slot_parent(slot);

    if (!parent) {
        prev = tree->root;
        tree->root = node;
    } else {
        index = rbtree_slot_index(slot);
        assert(rbtree_check_index(index));
        prev = parent->children[index];
        parent->children[index] = node;
    }

    assert(prev);
    *node = *prev;

    for (size_t i = 0; i < ARRAY_SIZE(node->children); i++) {
        if (!node->children[i]) {
            continue;
        }

        rbtree_node_set_parent(node->children[i], node);
    }

    return prev;
}

void
rbtree_remove(struct rbtree *tree, struct rbtree_node *node)
{
    struct rbtree_node *child, *parent, *brother;
    int color, left, right;

    if (node->children[RBTREE_LEFT] == NULL) {
        child = node->children[RBTREE_RIGHT];
    } else if (node->children[RBTREE_RIGHT] == NULL) {
        child = node->children[RBTREE_LEFT];
    } else {
        struct rbtree_node *successor;

        /*
         * Two-children case: replace the node with its successor.
         */

        successor = node->children[RBTREE_RIGHT];

        while (successor->children[RBTREE_LEFT] != NULL) {
            successor = successor->children[RBTREE_LEFT];
        }

        color = rbtree_node_color(successor);
        child = successor->children[RBTREE_RIGHT];
        parent = rbtree_node_parent(node);

        if (unlikely(parent == NULL)) {
            tree->root = successor;
        } else {
            parent->children[rbtree_node_index(node, parent)] = successor;
        }

        parent = rbtree_node_parent(successor);

        /*
         * Set parent directly to keep the original color.
         */
        successor->parent = node->parent;
        successor->children[RBTREE_LEFT] = node->children[RBTREE_LEFT];
        rbtree_node_set_parent(successor->children[RBTREE_LEFT], successor);

        if (node == parent) {
            parent = successor;
        } else {
            successor->children[RBTREE_RIGHT] = node->children[RBTREE_RIGHT];
            rbtree_node_set_parent(successor->children[RBTREE_RIGHT],
                                   successor);
            parent->children[RBTREE_LEFT] = child;

            if (child != NULL) {
                rbtree_node_set_parent(child, parent);
            }
        }

        goto update_color;
    }

    /*
     * Node has at most one child.
     */

    color = rbtree_node_color(node);
    parent = rbtree_node_parent(node);

    if (child != NULL) {
        rbtree_node_set_parent(child, parent);
    }

    if (unlikely(parent == NULL)) {
        tree->root = child;
    } else {
        parent->children[rbtree_node_index(node, parent)] = child;
    }

    /*
     * The node has been removed, update the colors. The child pointer can
     * be NULL, in which case it is considered a black leaf.
     */
update_color:
    if (color == RBTREE_COLOR_RED) {
        return;
    }

    for (;;) {
        if ((child != NULL) && rbtree_node_is_red(child)) {
            rbtree_node_set_black(child);
            break;
        }

        if (parent == NULL) {
            break;
        }

        left = rbtree_node_index(child, parent);
        right = 1 - left;

        brother = parent->children[right];

        /*
         * Brother is red. Recolor and rotate left at parent so that brother
         * becomes black.
         */
        if (rbtree_node_is_red(brother)) {
            rbtree_node_set_black(brother);
            rbtree_node_set_red(parent);
            rbtree_rotate(tree, parent, left);
            brother = parent->children[right];
        }

        assert(brother != NULL);

        /*
         * Brother has no red child. Recolor and repeat at parent.
         */
        if (((brother->children[RBTREE_LEFT] == NULL)
             || rbtree_node_is_black(brother->children[RBTREE_LEFT]))
            && ((brother->children[RBTREE_RIGHT] == NULL)
                || rbtree_node_is_black(brother->children[RBTREE_RIGHT]))) {
            rbtree_node_set_red(brother);
            child = parent;
            parent = rbtree_node_parent(child);
            continue;
        }

        /*
         * Brother's right child is black. Recolor and rotate right at brother.
         */
        if ((brother->children[right] == NULL)
            || rbtree_node_is_black(brother->children[right])) {
            rbtree_node_set_black(brother->children[left]);
            rbtree_node_set_red(brother);
            rbtree_rotate(tree, brother, right);
            brother = parent->children[right];
        }

        /*
         * Brother's left child is black. Exchange parent and brother colors
         * (we already know brother is black), set brother's right child black,
         * rotate left at parent and leave.
         */
        assert(brother->children[right] != NULL);
        rbtree_node_set_color(brother, rbtree_node_color(parent));
        rbtree_node_set_black(parent);
        rbtree_node_set_black(brother->children[right]);
        rbtree_rotate(tree, parent, left);
        break;
    }

    assert((tree->root == NULL) || rbtree_node_is_black(tree->root));
}

struct rbtree_node *
rbtree_nearest(struct rbtree_node *parent, int index, int direction)
{
    assert(rbtree_check_index(direction));

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

    assert(rbtree_check_index(index));

    if (index != direction) {
        return parent;
    }

    return rbtree_walk(parent, direction);
}

struct rbtree_node *
rbtree_firstlast(const struct rbtree *tree, int direction)
{
    struct rbtree_node *prev, *cur;

    assert(rbtree_check_index(direction));

    prev = NULL;

    for (cur = tree->root; cur != NULL; cur = cur->children[direction]) {
        prev = cur;
    }

    return prev;
}

struct rbtree_node *
rbtree_walk(struct rbtree_node *node, int direction)
{
    int left, right;

    assert(rbtree_check_index(direction));

    left = direction;
    right = 1 - left;

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

    if (node->children[left] != NULL) {
        node = node->children[left];

        while (node->children[right] != NULL) {
            node = node->children[right];
        }
    } else {
        struct rbtree_node *parent;
        int index;

        for (;;) {
            parent = rbtree_node_parent(node);

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

            index = rbtree_node_index(node, parent);
            node = parent;

            if (index == right) {
                break;
            }
        }
    }

    return node;
}

struct rbtree_node *
rbtree_postwalk_deepest(const struct rbtree *tree)
{
    struct rbtree_node *node;

    node = tree->root;

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

    return rbtree_node_find_deepest(node);
}

struct rbtree_node *
rbtree_postwalk_unlink(struct rbtree_node *node)
{
    struct rbtree_node *parent;
    int index;

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

    assert(node->children[RBTREE_LEFT] == NULL);
    assert(node->children[RBTREE_RIGHT] == NULL);

    parent = rbtree_node_parent(node);

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

    index = rbtree_node_index(node, parent);
    parent->children[index] = NULL;
    node = parent->children[RBTREE_RIGHT];

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

    return rbtree_node_find_deepest(node);
}


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

#ifndef KERN_RBTREE_H
#define KERN_RBTREE_H

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

#include <kern/macros.h>

/*
 * Indexes of the left and right nodes in the children array of a node.
 */
#define RBTREE_LEFT     0
#define RBTREE_RIGHT    1

/*
 * Red-black node.
 */
struct rbtree_node;

/*
 * Red-black tree.
 */
struct rbtree;

/*
 * Insertion point identifier.
 */
typedef uintptr_t rbtree_slot_t;

/*
 * Static tree initializer.
 */
#define RBTREE_INITIALIZER { NULL }

#include <kern/rbtree_i.h>

/*
 * Initialize a tree.
 */
static inline void
rbtree_init(struct rbtree *tree)
{
    tree->root = NULL;
}

/*
 * Initialize a node.
 *
 * A node is in no tree when its parent points to itself.
 */
static inline void
rbtree_node_init(struct rbtree_node *node)
{
    assert(rbtree_node_check_alignment(node));

    node->parent = (uintptr_t)node | RBTREE_COLOR_RED;
    node->children[RBTREE_LEFT] = NULL;
    node->children[RBTREE_RIGHT] = NULL;
}

/*
 * Return true if node is in no tree.
 */
static inline int
rbtree_node_unlinked(const struct rbtree_node *node)
{
    return rbtree_node_parent(node) == node;
}

/*
 * Macro that evaluates to the address of the structure containing the
 * given node based on the given type and member.
 */
#define rbtree_entry(node, type, member) structof(node, type, member)

/*
 * Return true if tree is empty.
 */
static inline int
rbtree_empty(const struct rbtree *tree)
{
    return tree->root == NULL;
}

/*
 * Look up a node in a tree.
 *
 * Note that implementing the lookup algorithm as a macro gives two benefits:
 * First, it avoids the overhead of a callback function. Next, the type of the
 * cmp_fn parameter isn't rigid. The only guarantee offered by this
 * implementation is that the key parameter is the first parameter given to
 * cmp_fn. This way, users can pass only the value they need for comparison
 * instead of e.g. allocating a full structure on the stack.
 *
 * See rbtree_insert().
 */
#define rbtree_lookup(tree, key, cmp_fn)                \
MACRO_BEGIN                                             \
    struct rbtree_node *cur_;                           \
    int diff_;                                          \
                                                        \
    cur_ = (tree)->root;                                \
                                                        \
    while (cur_ != NULL) {                              \
        diff_ = cmp_fn(key, cur_);                      \
                                                        \
        if (diff_ == 0) {                               \
            break;                                      \
        }                                               \
                                                        \
        cur_ = cur_->children[rbtree_d2i(diff_)];       \
    }                                                   \
                                                        \
    cur_;                                               \
MACRO_END

/*
 * Look up a node or one of its nearest nodes in a tree.
 *
 * This macro essentially acts as rbtree_lookup() but if no entry matched
 * the key, an additional step is performed to obtain the next or previous
 * node, depending on the direction (left or right).
 *
 * The constraints that apply to the key parameter are the same as for
 * rbtree_lookup().
 */
#define rbtree_lookup_nearest(tree, key, cmp_fn, dir)       \
MACRO_BEGIN                                                 \
    struct rbtree_node *cur_, *prev_;                       \
    int diff_, index_;                                      \
                                                            \
    prev_ = NULL;                                           \
    index_ = -1;                                            \
    cur_ = (tree)->root;                                    \
                                                            \
    while (cur_ != NULL) {                                  \
        diff_ = cmp_fn(key, cur_);                          \
                                                            \
        if (diff_ == 0) {                                   \
            break;                                          \
        }                                                   \
                                                            \
        prev_ = cur_;                                       \
        index_ = rbtree_d2i(diff_);                         \
        cur_ = cur_->children[index_];                      \
    }                                                       \
                                                            \
    if (cur_ == NULL) {                                     \
        cur_ = rbtree_nearest(prev_, index_, dir);          \
    }                                                       \
                                                            \
    cur_;                                                   \
MACRO_END

/*
 * Insert a node in a tree.
 *
 * This macro performs a standard lookup to obtain the insertion point of
 * the given node in the tree (it is assumed that the inserted node never
 * compares equal to any other entry in the tree) and links the node. It
 * then checks red-black rules violations, and rebalances the tree if
 * necessary.
 *
 * Unlike rbtree_lookup(), the cmp_fn parameter must compare two complete
 * entries, so it is suggested to use two different comparison inline
 * functions, such as myobj_cmp_lookup() and myobj_cmp_insert(). There is no
 * guarantee about the order of the nodes given to the comparison function.
 *
 * See rbtree_lookup().
 */
#define rbtree_insert(tree, node, cmp_fn)                   \
MACRO_BEGIN                                                 \
    struct rbtree_node *cur_, *prev_;                       \
    int diff_, index_;                                      \
                                                            \
    prev_ = NULL;                                           \
    index_ = -1;                                            \
    cur_ = (tree)->root;                                    \
                                                            \
    while (cur_ != NULL) {                                  \
        diff_ = cmp_fn(node, cur_);                         \
        assert(diff_ != 0);                                 \
        prev_ = cur_;                                       \
        index_ = rbtree_d2i(diff_);                         \
        cur_ = cur_->children[index_];                      \
    }                                                       \
                                                            \
    rbtree_insert_rebalance(tree, prev_, index_, node);     \
MACRO_END

/*
 * Look up a node/slot pair in a tree.
 *
 * This macro essentially acts as rbtree_lookup() but in addition to a node,
 * it also returns a slot, which identifies an insertion point in the tree.
 * If the returned node is NULL, the slot can be used by rbtree_insert_slot()
 * to insert without the overhead of an additional lookup.
 *
 * The constraints that apply to the key parameter are the same as for
 * rbtree_lookup().
 */
#define rbtree_lookup_slot(tree, key, cmp_fn, slot) \
MACRO_BEGIN                                         \
    struct rbtree_node *cur_, *prev_;               \
    int diff_, index_;                              \
                                                    \
    prev_ = NULL;                                   \
    index_ = 0;                                     \
    cur_ = (tree)->root;                            \
                                                    \
    while (cur_ != NULL) {                          \
        diff_ = cmp_fn(key, cur_);                  \
                                                    \
        if (diff_ == 0) {                           \
            break;                                  \
        }                                           \
                                                    \
        prev_ = cur_;                               \
        index_ = rbtree_d2i(diff_);                 \
        cur_ = cur_->children[index_];              \
    }                                               \
                                                    \
    (slot) = rbtree_slot(prev_, index_);            \
    cur_;                                           \
MACRO_END

/*
 * Insert a node at an insertion point in a tree.
 *
 * This macro essentially acts as rbtree_insert() except that it doesn't
 * obtain the insertion point with a standard lookup. The insertion point
 * is obtained by calling rbtree_lookup_slot(). In addition, the new node
 * must not compare equal to an existing node in the tree (i.e. the slot
 * must denote a NULL node).
 */
static inline void
rbtree_insert_slot(struct rbtree *tree, rbtree_slot_t slot,
                   struct rbtree_node *node)
{
    struct rbtree_node *parent;
    int index;

    parent = rbtree_slot_parent(slot);
    index = rbtree_slot_index(slot);
    rbtree_insert_rebalance(tree, parent, index, node);
}

/*
 * Replace a node at an insertion point in a tree.
 *
 * The given node must compare strictly equal to the previous node,
 * which is returned on completion.
 */
void * rbtree_replace_slot(struct rbtree *tree, rbtree_slot_t slot,
                           struct rbtree_node *node);

/*
 * Remove a node from a tree.
 *
 * After completion, the node is stale.
 */
void rbtree_remove(struct rbtree *tree, struct rbtree_node *node);

/*
 * Return the first node of a tree.
 */
#define rbtree_first(tree) rbtree_firstlast(tree, RBTREE_LEFT)

/*
 * Return the last node of a tree.
 */
#define rbtree_last(tree) rbtree_firstlast(tree, RBTREE_RIGHT)

/*
 * Return the node previous to the given node.
 */
#define rbtree_prev(node) rbtree_walk(node, RBTREE_LEFT)

/*
 * Return the node next to the given node.
 */
#define rbtree_next(node) rbtree_walk(node, RBTREE_RIGHT)

/*
 * Forge a loop to process all nodes of a tree, removing them when visited.
 *
 * This macro can only be used to destroy a tree, so that the resources used
 * by the entries can be released by the user. It basically removes all nodes
 * without doing any color checking.
 *
 * After completion, all nodes and the tree root member are stale.
 */
#define rbtree_for_each_remove(tree, node, tmp)         \
for (node = rbtree_postwalk_deepest(tree),              \
     tmp = rbtree_postwalk_unlink(node);                \
     node != NULL;                                      \
     node = tmp, tmp = rbtree_postwalk_unlink(node))

#endif /* KERN_RBTREE_H */


code:
/*
 * Copyright (c) 2010-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/>.
 *
 * Upstream site with license notes :
 * http://git.sceen.net/rbraun/librbraun.git/
 */

#ifndef KERN_RBTREE_I_H
#define KERN_RBTREE_I_H

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

#include <kern/macros.h>

/*
 * Red-black node structure.
 *
 * To reduce the number of branches and the instruction cache footprint,
 * the left and right child pointers are stored in an array, and the symmetry
 * of most tree operations is exploited by using left/right variables when
 * referring to children.
 *
 * In addition, this implementation assumes that all nodes are 4-byte aligned,
 * so that the least significant bit of the parent member can be used to store
 * the color of the node. This is true for all modern 32 and 64 bits
 * architectures, as long as the nodes aren't embedded in structures with
 * special alignment constraints such as member packing.
 */
struct rbtree_node {
    uintptr_t parent;
    struct rbtree_node *children[2];
};

/*
 * Red-black tree structure.
 */
struct rbtree {
    struct rbtree_node *root;
};

/*
 * Masks applied on the parent member of a node to obtain either the
 * color or the parent address.
 */
#define RBTREE_COLOR_MASK   ((uintptr_t)0x1)
#define RBTREE_PARENT_MASK  (~(uintptr_t)0x3)

/*
 * Node colors.
 */
#define RBTREE_COLOR_RED    0
#define RBTREE_COLOR_BLACK  1

/*
 * Masks applied on slots to obtain either the child index or the parent
 * address.
 */
#define RBTREE_SLOT_INDEX_MASK  0x1UL
#define RBTREE_SLOT_PARENT_MASK (~RBTREE_SLOT_INDEX_MASK)

/*
 * Return true if the given index is a valid child index.
 */
static inline int
rbtree_check_index(int index)
{
    return index == (index & 1);
}

/*
 * Convert the result of a comparison into an index in the children array
 * (0 or 1).
 *
 * This function is mostly used when looking up a node.
 */
static inline int
rbtree_d2i(int diff)
{
    return !(diff <= 0);
}

/*
 * Return true if the given pointer is suitably aligned.
 */
static inline int
rbtree_node_check_alignment(const struct rbtree_node *node)
{
    return ((uintptr_t)node & (~RBTREE_PARENT_MASK)) == 0;
}

/*
 * Return the parent of a node.
 */
static inline struct rbtree_node *
rbtree_node_parent(const struct rbtree_node *node)
{
    return (struct rbtree_node *)(node->parent & RBTREE_PARENT_MASK);
}

/*
 * Translate an insertion point into a slot.
 */
static inline rbtree_slot_t
rbtree_slot(struct rbtree_node *parent, int index)
{
    assert(rbtree_node_check_alignment(parent));
    assert(rbtree_check_index(index));
    return (rbtree_slot_t)parent | index;
}

/*
 * Extract the parent address from a slot.
 */
static inline struct rbtree_node *
rbtree_slot_parent(rbtree_slot_t slot)
{
    return (struct rbtree_node *)(slot & RBTREE_SLOT_PARENT_MASK);
}

/*
 * Extract the index from a slot.
 */
static inline int
rbtree_slot_index(rbtree_slot_t slot)
{
    return slot & RBTREE_SLOT_INDEX_MASK;
}

/*
 * Insert a node in a tree, rebalancing it if necessary.
 *
 * The index parameter is the index in the children array of the parent where
 * the new node is to be inserted. It is ignored if the parent is NULL.
 *
 * This function is intended to be used by the rbtree_insert() macro only.
 */
void rbtree_insert_rebalance(struct rbtree *tree, struct rbtree_node *parent,
                             int index, struct rbtree_node *node);

/*
 * Return the previous or next node relative to a location in a tree.
 *
 * The parent and index parameters define the location, which can be empty.
 * The direction parameter is either RBTREE_LEFT (to obtain the previous
 * node) or RBTREE_RIGHT (to obtain the next one).
 */
struct rbtree_node * rbtree_nearest(struct rbtree_node *parent, int index,
                                    int direction);

/*
 * Return the first or last node of a tree.
 *
 * The direction parameter is either RBTREE_LEFT (to obtain the first node)
 * or RBTREE_RIGHT (to obtain the last one).
 */
struct rbtree_node * rbtree_firstlast(const struct rbtree *tree, int direction);

/*
 * Return the node next to, or previous to the given node.
 *
 * The direction parameter is either RBTREE_LEFT (to obtain the previous node)
 * or RBTREE_RIGHT (to obtain the next one).
 */
struct rbtree_node * rbtree_walk(struct rbtree_node *node, int direction);

/*
 * Return the left-most deepest node of a tree, which is the starting point of
 * the postorder traversal performed by rbtree_for_each_remove().
 */
struct rbtree_node * rbtree_postwalk_deepest(const struct rbtree *tree);

/*
 * Unlink a node from its tree and return the next (right) node in postorder.
 */
struct rbtree_node * rbtree_postwalk_unlink(struct rbtree_node *node);

#endif /* KERN_RBTREE_I_H */