/**
 * @file wrtd-fd.c
 *
 * Copyright (c) 2018-2019 CERN (home.cern)
 *
 * SPDX-License-Identifier: LGPL-3.0-or-later
 */

#include <errno.h>
#include <string.h>

#include "mockturtle-rt.h"
#include <mockturtle-framework.h>
#include "wrtd-common.h"
#include "hw/fd_channel_regs.h"
#include "hw/fd_main_regs.h"

#include "wrtd-fd.h"

struct wrtd_fd_calibration calib = {
        .zero_offset_ps = { -38186, -38155, -38147, -38362},
};

#define OUT_TIMEOUT 10

static inline int fd_wr_present(struct wrtd_fd_dev *fd)
{
        return !!(fd_readl(fd, FD_REG_TCR) & FD_TCR_WR_PRESENT);
}

static inline int fd_wr_link_up(struct wrtd_fd_dev *fd)
{
        return fd_readl(fd, FD_REG_TCR) & FD_TCR_WR_LINK;
}

static inline int fd_wr_time_locked(struct wrtd_fd_dev *fd)
{
        return fd_readl(fd, FD_REG_TCR) & FD_TCR_WR_LOCKED;
}

static inline int fd_wr_time_ready(struct wrtd_fd_dev *fd)
{
        return 1;
}

static inline int fd_wr_sync_timeout(void)
{
        return 0;
}

static void fd_wr_enable_lock(struct wrtd_fd_dev *fd, int enable)
{
        fd_writel(fd, enable ? FD_TCR_WR_ENABLE : 0, FD_REG_TCR);
}


/**
 * Writes to FD output registers for output (out)
 */
static inline void fd_ch_writel(struct wrtd_fd_channel *out, uint32_t value,
                                uint32_t reg)
{
        dp_writel(value, reg + out->channel_addr);
}


/**
 * Reads from FD output registers for output (out)
 */
static inline uint32_t fd_ch_readl (struct wrtd_fd_channel *out, uint32_t reg)
{
        return dp_readl(reg + out->channel_addr);
}


/**
 * Initializes an empty pulse queue
 */
static void pulse_queue_init(struct lrt_pulse_queue *p)
{
        p->head = 0;
        p->tail = 0;
        p->count = 0;
}


/**
 * Requests a new entry in a pulse queue. Returns pointer to the ne
 * entry or NULL if the queue is full.
 */
static struct wrtd_event *pulse_queue_push(struct lrt_pulse_queue *p)
{
        struct wrtd_event *ev;

        if (p->count == FD_MAX_QUEUE_PULSES)
                return NULL;

        ev = &p->events[p->head];
        p->count++;
        p->head++;

        if (p->head == FD_MAX_QUEUE_PULSES)
                p->head = 0;

        return ev;
}


/**
 * Returns non-0 if pulse queue p contains any pulses.
 */
static inline int pulse_queue_empty(struct lrt_pulse_queue *p)
{
        return (p->count == 0);
}


/**
 * Returns the oldest entry in the pulse queue (or NULL if empty).
 */
static struct wrtd_event *pulse_queue_front(struct lrt_pulse_queue *p)
{
        if (!p->count)
                return NULL;
        return &p->events[p->tail];
}


/**
 * Releases the oldest entry from the pulse queue.
 */
static void pulse_queue_pop(struct lrt_pulse_queue *p)
{
        p->tail++;

        if(p->tail == FD_MAX_QUEUE_PULSES)
                p->tail = 0;
        p->count--;
}

/**
 * Checks if the timestamp of the last programmed pulse is lost because
 * of timeout
 */
static int check_output_timeout (struct wrtd_fd_channel *out)
{
        uint32_t now_sec;
        uint32_t now_ns;
        int delta;

        /*
         * Read the current WR time, order is important: first seconds,
         * then cycles (cycles get latched on reading secs register.
         */
        now_sec = lr_readl(MT_CPU_LR_REG_TAI_SEC);
        now_ns = lr_readl(MT_CPU_LR_REG_TAI_CYCLES) * 8;

        if(out->last_programmed_sec > now_sec + OUT_TIMEOUT) {
                pr_error("Enqueued event very far in the future. Dropping.");
                return 0;
        }

        /* Current time exceeds FD setpoint? */

        delta = now_sec - out->last_programmed_sec;
        if (delta != 0)
                return delta > 0;

        delta = now_ns - out->last_programmed_ns;
        return (delta > 0);
}

/**
 * Drop the given enqueued trigger
 */
static void drop_trigger(struct wrtd_fd_channel *out,
                         struct wrtd_event *ev,
                         struct lrt_pulse_queue *q, unsigned reason)
{
        out->idle = 1;

        if (pulse_queue_empty(q))
                return;

        /* Drop the pulse */
        pulse_queue_pop(q);

        /* Disarm the FD output */
        fd_ch_writel(out, FD_DCR_MODE, FD_REG_DCR);

        wrtd_log(WRTD_LOG_MSG_EV_DISCARDED, reason, NULL, ev, NULL);
}


/**
 * Output driving function. Reads pulses from the output queue,
 * programs the output and updates the output statistics.
 */
static void fd_output (struct wrtd_fd_dev *fd, unsigned channel)
{
        struct wrtd_fd_channel *out = &fd->channels[channel];
        struct lrt_pulse_queue *q = &out->queue;
        struct wrtd_event *ev = pulse_queue_front(q);
        uint32_t dcr = fd_ch_readl(out, FD_REG_DCR);
        struct wrtd_tstamp ts;

        /* Check if the output has triggered */
        if (!out->idle) {
                if (!wr_is_timing_ok()) {
                        /* Timing has been lost.  */
                        drop_trigger(out, ev, q, WRTD_LOG_DISCARD_NO_SYNC);
                        return;
                }
                if (!(dcr & FD_DCR_PG_TRIG)) {
                        /* Armed but still waiting for trigger */
                        if (check_output_timeout (out)) {
                                /* Will never trigger.  Missed.  */
                                drop_trigger(out, ev, q,
                                             WRTD_LOG_DISCARD_TIMEOUT);
                        }
                } else {
                        /* Has been triggered.  */
                        wrtd_log(WRTD_LOG_MSG_EV_CONSUMED,
                                 WRTD_LOG_CONSUMED_DONE,
                                 NULL, ev, NULL);

                        pulse_queue_pop(q);
                        out->idle = 1;
                }
                return;
        }

        /* Output is idle: check if there's something in the queue to execute */
        if (pulse_queue_empty(q))
                return;

        if (!wr_is_timing_ok()) {
                drop_trigger(out, ev, q, WRTD_LOG_DISCARD_NO_SYNC);
                return;
        }

        /* Apply zero offset calibration value if necessary */
        if (calib.zero_offset_ps[channel] == 0)
                ts = ev->ts;
        else if (calib.zero_offset_ps[channel] > 0)
                ts_add3_ps(&ts, &ev->ts, calib.zero_offset_ps[channel]);
        else
                ts_sub3_ps(&ts, &ev->ts, -calib.zero_offset_ps[channel]);

        /* Program the output start time */
        fd_ch_writel(out, ts.seconds, FD_REG_U_STARTL);
        fd_ch_writel(out, ts.ns >> 3, FD_REG_C_START);
        fd_ch_writel(out, ((ts.ns & 7) << 9) | (ts.frac >> (32 - 9)),
                     FD_REG_F_START);

        /* Adjust pulse width and program the output end time */
        ts_add2_ns(&ts, out->width_ns);

        fd_ch_writel(out, ts.seconds, FD_REG_U_ENDL);
        fd_ch_writel(out, ts.ns >> 3, FD_REG_C_END);
        fd_ch_writel(out, ((ts.ns & 7) << 9) | (ts.frac >> (32 - 9)),
                     FD_REG_F_END);
        fd_ch_writel(out, 0, FD_REG_RCR);
        fd_ch_writel(out, FD_DCR_MODE, FD_REG_DCR);
        fd_ch_writel(out, FD_DCR_MODE | FD_DCR_UPDATE, FD_REG_DCR);
        fd_ch_writel(out, FD_DCR_MODE | FD_DCR_PG_ARM | FD_DCR_ENABLE,
                     FD_REG_DCR);

        wrtd_log(WRTD_LOG_MSG_EV_CONSUMED, WRTD_LOG_CONSUMED_START,
                 NULL, ev, NULL);

        ts_add2_ns (&ts, 8000);

        /*
         * Store the last programmed timestamp (+ some margin) and mark
         * the output as busy
         */
        out->last_programmed_sec = ts.seconds;
        out->last_programmed_ns = ts.ns;
        out->idle = 0;
}

static int fd_local_output(struct wrtd_fd_dev *fd,
                           struct wrtd_event *ev, unsigned ch)
{
        struct wrtd_fd_channel *out = &fd->channels[ch];
        struct wrtd_event *pq_ev;

        pq_ev = pulse_queue_push(&out->queue);
        if (!pq_ev) {
                return -EOVERFLOW;
        }

        *pq_ev = *ev;

        return 0;
}

static void fd_outputs(struct wrtd_fd_dev *fd)
{
        int i;

        for (i = 0;i < FD_NUM_CHANNELS; i++)
                fd_output(fd, i);
}

static void wrtd_fd_data_init(struct wrtd_fd_dev *fd)
{
        unsigned i;

        /* Channels */
        for (i = 0; i < FD_NUM_CHANNELS; i++) {
                memset(&fd->channels[i], 0, sizeof(struct wrtd_fd_channel));
                fd->channels[i].channel_addr = fd->io_addr + 0x100 + i * 0x100;
                pulse_queue_init(&fd->channels[i].queue);
                fd->channels[i].idle = 1;
                fd->channels[i].width_ns = 100; // 100ns
                fd->channels[i].last_programmed_sec = 0;
                fd->channels[i].last_programmed_ns = 0;
        }
}
