/**
 * @file wrtd-rt-common.h
 *
 * This is the common part that should be included
 * by every WRTD firmware application.
 *
 * In order for it to work, the firmware application
 * must declare the following definitions and includes:
 *
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 * #define NBR_CPUS xxx
 * #define CPU_IDX xxx
 * #define NBR_RULES xxx
 * #define NBR_DEVICES xxx
 * #define NBR_ALARMS xxx
 * #define DEVICES_NBR_CHS { xxx, 0, 0, 0}
 * #define DEVICES_CHS_DIR { WRTD_CH_DIR_IN, 0, 0, 0}
 * #define APP_ID WRTD_APP_xxx
 * #define APP_VER RT_VERSION(xxx, 0)
 * #define APP_NAME xxx
 * #define WRTD_NET_RX x // 1 = enabled, 0 = disabled
 * #define WRTD_NET_TX x
 * #define WRTD_LOCAL_RX x
 * #define WRTD_LOCAL_TX x
 * #include "wrtd-rt-common.h"
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 *
 * Copyright (c) 2018-2019 CERN (home.cern)
 *
 * SPDX-License-Identifier: LGPL-3.0-or-later
 */

/* Remote message queue for network.  */
#define WRTD_RMQ 0

/* By default use lxi-evntsvc.  */
#define WRTD_UDP_PORT 5044

/* Functions defined by the user.  */
static int  wr_link_up(void);
static int  wr_time_locked(void);
static int  wr_time_ready(void);
static void wr_enable_lock(int enable);
static int  wrtd_local_output(struct wrtd_event *ev, unsigned ch);
static int  wr_sync_timeout(void);
static int  wrtd_user_init(void);
static void wrtd_io(void);

/* Forward function declarations.  */
#if WRTD_NET_TX > 0
static int wrtd_send_network(struct wrtd_event *ev);
#endif

static struct wrtd_rule rules[NBR_RULES];
static struct wrtd_alarm alarms[NBR_ALARMS];

static uint32_t tai_start = 0;

static struct wrtd_root root =
{
        .ver_major = WRTD_VERSION_MAJOR,
        .ver_minor = WRTD_VERSION_MINOR,

        .fw_name = APP_NAME,
        .fw_id = APP_ID,

        .nbr_devices = NBR_DEVICES,
        .nbr_alarms = NBR_ALARMS,
        .nbr_rules = NBR_RULES,

        .capabilities = (((WRTD_NET_RX   > 0) << 0) |
                         ((WRTD_NET_TX   > 0) << 1) |
                         ((WRTD_LOCAL_RX > 0) << 2) |
                         ((WRTD_LOCAL_TX > 0) << 3)),

        .wr_state = WR_LINK_OFFLINE,

        .log_flags = 0,
        .freeze_flag = 0,

        .devices_nbr_chs = DEVICES_NBR_CHS,
        .devices_chs_dir = DEVICES_CHS_DIR,
        .rules_addr = (uint32_t)rules,
        .alarms_addr = (uint32_t)alarms
};

static inline int wr_is_timing_ok(void)
{
        return (root.wr_state == WR_LINK_SYNCED);
}

/**
 * It updates the White-Rabbit link status
 */
static void wr_update_link(void)
{
        switch (root.wr_state) {
        case WR_LINK_OFFLINE:
                if (wr_link_up())
                        root.wr_state = WR_LINK_ONLINE;
                break;
        case WR_LINK_ONLINE:
                if (wr_time_ready()) {
                        root.wr_state = WR_LINK_SYNCING;
                        wr_enable_lock(1);
                }
                break;
        case WR_LINK_SYNCING:
                if (wr_time_locked()) {
                        pr_debug("sync detected, waiting for plumbing to catch up...\n\r");
                        root.wr_state = WR_LINK_WAIT;
                        tai_start = lr_readl(MT_CPU_LR_REG_TAI_SEC);
                }
                break;
        case WR_LINK_WAIT:
                if (wr_sync_timeout() == 0
                    || (lr_readl(MT_CPU_LR_REG_TAI_SEC)
                        >= (tai_start + wr_sync_timeout()))) {
                        pr_debug("WR synced\n\r");
                        root.wr_state = WR_LINK_SYNCED;
                }
                break;
        case WR_LINK_SYNCED:
                break;
        }

        if (root.wr_state != WR_LINK_OFFLINE && !wr_link_up()) {
                pr_error("WR sync lost\n\r");
                root.wr_state = WR_LINK_OFFLINE;
                wr_enable_lock(0);
        }
}

#if NBR_CPUS > 1
/* Number of entries; must be a power of 2.  */
#define LOOPBACK_QUEUE_SIZE (1 << 3)

struct loopback_queue {
        unsigned read_idx;
        unsigned write_idx;
        int count;
        int lock;

        struct wrtd_loopback_event {
                unsigned ch;
                struct wrtd_event ev;
        } buf[LOOPBACK_QUEUE_SIZE];
};

SMEM struct loopback_queue lqueues[NBR_CPUS];

static int wrtd_remote_output(struct wrtd_event *ev,
                              unsigned dest_cpu, unsigned ch)
{
        volatile struct loopback_queue *q = &lqueues[dest_cpu];

        if (q->count >= LOOPBACK_QUEUE_SIZE) {
                return -EOVERFLOW;
        }

        /* Get lock.  */
        while (1) {
                unsigned int i;
                if(smem_atomic_test_and_set(&q->lock) == 0)
                        break;
                /* Busy...  Wait a little bit.  */
                for (i = 0; i < 16; i++)
                        asm volatile ("nop");
        }
        q->buf[q->write_idx].ev = *ev;
        q->buf[q->write_idx].ch = ch;
        q->write_idx = (q->write_idx + 1) & (LOOPBACK_QUEUE_SIZE - 1);
        smem_atomic_add(&q->count, 1);
        q->lock = 0;

        return 0;
}

static void wrtd_recv_loopback(void)
{
        volatile struct loopback_queue *q = &lqueues[CPU_IDX];
        volatile struct wrtd_loopback_event *e;

        if (q->count == 0)
                return;  /* No entry */

        e = &q->buf[q->read_idx];

#if WRTD_NET_TX > 0
        if (e->ch == WRTD_DEST_CH_NET)
                wrtd_send_network((struct wrtd_event *)&e->ev);
        else
#endif // WRTD_NET_TX
                wrtd_local_output((struct wrtd_event *)&e->ev, e->ch);

        q->read_idx = (q->read_idx + 1)  & (LOOPBACK_QUEUE_SIZE - 1);
        smem_atomic_add(&q->count, -1);
}
#endif // NBR_CPUS

static inline void ts_add2_ns(struct wrtd_tstamp *ts, uint32_t ns)
{
        ts->ns += ns;
        if (ts->ns >= 1000000000) {
                ts->ns -= 1000000000;
                ts->seconds++;
        }
}

static inline void ts_add3_ns(struct wrtd_tstamp *dest,
                              const struct wrtd_tstamp *src, uint32_t ns)
{
        dest->seconds = src->seconds;
        dest->ns = src->ns + ns;
        dest->frac = src->frac;
        if (dest->ns >= 1000000000) {
                dest->ns -= 1000000000;
                dest->seconds++;
        }
}

static inline int ts_cmp(const struct wrtd_tstamp *l, const struct wrtd_tstamp *r)
{
        if (l->seconds < r->seconds)
                return -1;
        if (l->seconds > r->seconds)
                return 1;
        if (l->ns < r->ns)
                return -1;
        if (l->ns > r->ns)
                return 1;
        if (l->frac < r->frac)
                return -1;
        if (l->frac > r->frac)
                return 1;
        return 0;
}

static void ts_now(struct wrtd_tstamp *now)
{
        /*
         * Read the current WR time, order is important: first seconds,
         * then cycles (cycles get latched on reading secs register).
         */
        now->seconds = lr_readl(MT_CPU_LR_REG_TAI_SEC);
        now->ns = lr_readl(MT_CPU_LR_REG_TAI_CYCLES) * 8;
        now->frac = 0;
}

static void wrtd_log(uint32_t type, uint32_t reason,
                     struct wrtd_rule *rule,
                     const struct wrtd_event *ev,
                     const struct wrtd_tstamp *ts)
{
        struct wrtd_log_entry *log;
        struct trtl_fw_msg msg;
        int ret;

        if (root.log_flags == 0)
                return;

        /* First, handle statistics */
        if ((type == WRTD_LOG_MSG_EV_DISCARDED) && (rule != NULL)) {
                rule->stat.miss_last = ev->ts;
                switch (reason) {
                case WRTD_LOG_DISCARD_NO_SYNC:
                        rule->stat.miss_nosync++;
                        break;
                case WRTD_LOG_DISCARD_HOLDOFF:
                        rule->stat.miss_holdoff++;
                        break;
                case WRTD_LOG_DISCARD_TIMEOUT:
                        rule->stat.miss_late++;
                        break;
                case WRTD_LOG_DISCARD_OVERFLOW:
                        rule->stat.miss_overflow++;
                        break;
                default:
                        break;
                }
        }

        ret = mq_claim(TRTL_HMQ, WRTD_HMQ);
        if (ret == -EBUSY) {
                return;
        }

        mq_map_out_message(TRTL_HMQ, WRTD_HMQ, &msg);

        msg.header->flags = 0;
        msg.header->msg_id = WRTD_ACTION_LOG;
        msg.header->len = sizeof(struct wrtd_log_entry) / 4;

        log = (struct wrtd_log_entry *)msg.payload;
        log->type = type;
        log->reason = reason;
        if (ev != NULL)
                log->event = *ev;
        else
                memset(&log->event, 0, sizeof(struct wrtd_event));

        if (ts != NULL)
                log->ts = *ts;
        else
                ts_now(&log->ts);

        mq_send(TRTL_HMQ, WRTD_HMQ);

}

#if WRTD_NET_TX > 0
static void wrtd_init_tx(void)
{
        static const struct trtl_ep_eth_address addr = {
                .type = TRTL_EP_FRAME_UDP,
                .dst_mac = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF},
                .dst_ip = 0xE000179F, /* multicast on 224.0.23.159 */
                .src_ip = 0xC0A85A11,
                .dst_port = WRTD_UDP_PORT,
                .src_port = WRTD_UDP_PORT,
                .ethertype = 0x800, /* IPv4 */
                .filter = 0 /* only the TX path of the endpoint is used */
        };
        rmq_bind_out(WRTD_RMQ, TRTL_EP_ETH, &addr);
}

static int wrtd_send_network(struct wrtd_event *ev)
{
        int ret;
        struct trtl_fw_msg fw_msg;

        ret = mq_claim(TRTL_RMQ, WRTD_RMQ);
        if (ret < 0) {
                return -EOVERFLOW;
        }

        mq_map_out_message(TRTL_RMQ, WRTD_RMQ, &fw_msg);

        fw_msg.header->msg_id = 0;
        fw_msg.header->len = sizeof(struct wrtd_message) / 4;

        /* Maybe volatile is too pessimistic, but still needed.  */
        volatile struct wrtd_message *msg = fw_msg.payload;

        msg->hw_detect[0] = 'L';
        msg->hw_detect[1] = 'X';
        msg->hw_detect[2] = 'I';
        msg->domain = 0;
        memcpy((unsigned char *)msg->event_id, ev->id, WRTD_ID_LEN);
        msg->seq = ev->seq;
        msg->ts_sec = ev->ts.seconds;
        msg->ts_ns = ev->ts.ns;
        msg->ts_frac = ev->ts.frac >> 16;
        msg->ts_hi_sec = 0;
        msg->flags = 0;
        msg->zero[0] = 0;
        msg->zero[1] = 0;
        msg->pad[0] = 0;

        mq_send(TRTL_RMQ, WRTD_RMQ);

        wrtd_log(WRTD_LOG_MSG_EV_NETWORK, WRTD_LOG_NETWORK_TX, NULL, ev, NULL);

        return 0;
}
#endif // WRTD_NET_TX

static void wrtd_route(struct wrtd_rule *rule, const struct wrtd_event *ev)
{
        struct wrtd_event tev;
        struct wrtd_tstamp now;
        uint32_t lat_ns;

        /* Check hold-off.
           Note: this also excludes events that came before the last
           one.  */
        if (rule->conf.hold_off_ns) {
                if (ts_cmp(&ev->ts, &rule->stat.hold_off) < 0) {
                        wrtd_log(WRTD_LOG_MSG_EV_DISCARDED,
                                 WRTD_LOG_DISCARD_HOLDOFF,
                                 rule, ev, NULL);
                        return;
                }

                /* Update hold-off.  */
                ts_add3_ns(&rule->stat.hold_off,
                           &ev->ts, rule->conf.hold_off_ns);
        }

        /* Event was received.  */
        rule->stat.rx_events++;
        rule->stat.rx_last = ev->ts;

        /* Delay.  */
        if (rule->conf.delay_ns)
                ts_add3_ns(&tev.ts, &ev->ts, rule->conf.delay_ns);
        else
                tev.ts = ev->ts;

        /* Resync.  */
        if (rule->conf.resync_period_ns) {
                uint32_t r = tev.ts.ns % rule->conf.resync_period_ns;
                if (r != 0)
                        ts_add2_ns(&tev.ts, rule->conf.resync_period_ns - r);
                tev.ts.frac = 0;
                if (rule->conf.resync_factor)
                        ts_add2_ns(&tev.ts,
                                   rule->conf.resync_period_ns *
                                   rule->conf.resync_factor);
        }

        /* Repeat count.  */
        if (rule->conf.repeat_count != 0) {
                rule->conf.repeat_count--;
                if (rule->conf.repeat_count == 0)
                        rule->conf.enabled = 0;
        }

        /* Rename event.  */
        memcpy(tev.id, rule->conf.dest_id, WRTD_ID_LEN);
        tev.seq = rule->stat.seq++;

        uint32_t res = 0;

        /* Check timing.  */
        if (!wr_is_timing_ok()) {
                wrtd_log(WRTD_LOG_MSG_EV_DISCARDED,
                         WRTD_LOG_DISCARD_NO_SYNC,
                         rule, ev, NULL);
                return;
        }

        /* Check with current time.  */
        ts_now(&now);
        if (!rule->conf.send_late) {
                if (tev.ts.seconds < now.seconds
                    || (tev.ts.seconds == now.seconds
                        && tev.ts.ns < now.ns)) {
                        /* Too late... */
                        wrtd_log(WRTD_LOG_MSG_EV_DISCARDED,
                                 WRTD_LOG_DISCARD_TIMEOUT,
                                 rule, ev, &now);
                        return;
                }
        }

        /* Send.  */
        if (rule->conf.dest_ch == WRTD_DEST_CH_NET) {
#if WRTD_NET_TX > 0
                res = wrtd_send_network(&tev);
#else
                res = wrtd_remote_output
                        (&tev, rule->conf.dest_cpu, WRTD_DEST_CH_NET);
#endif
        }
        else {
                if (rule->conf.dest_cpu != WRTD_DEST_CPU_LOCAL) {
#if NBR_CPUS > 1
                        res = wrtd_remote_output
                                (&tev, rule->conf.dest_cpu, rule->conf.dest_ch);
#endif
                } else {
                        /* Local delivery.  */
                        res = wrtd_local_output(&tev, rule->conf.dest_ch);
                }
        }
        if (res == -EOVERFLOW) {
                wrtd_log(WRTD_LOG_MSG_EV_DISCARDED,
                         WRTD_LOG_DISCARD_OVERFLOW,
                         rule, ev, NULL);
        }
        else if (res == 0) {
                rule->stat.tx_events++;
                rule->stat.tx_last = ev->ts;
        }

        /* Compute latency.  */
        ts_now(&now);
        do {
                lat_ns = now.ns - ev->ts.ns;
                if(lat_ns >> 31) {
                        /* Result is negative, possible only if
                           timestamps differ by 1 sec.  */
                        if (ev->ts.seconds + 1 == now.seconds)
                                lat_ns += 1000000000;
                        else
                                break;
                }
                else if (ev->ts.seconds != now.seconds) {
                        /* In the future or way in the past.  */
                        break;
                }

                if ((rule->stat.lat_min_ns == 0) ||
                    (lat_ns < rule->stat.lat_min_ns))
                        rule->stat.lat_min_ns = lat_ns;
                if (lat_ns > rule->stat.lat_max_ns)
                        rule->stat.lat_max_ns = lat_ns;
                rule->stat.lat_lo_ns += lat_ns;
                if (rule->stat.lat_lo_ns < lat_ns)
                        rule->stat.lat_hi_ns++;
                rule->stat.lat_nbr++;
        }
        while (0);
}

#if WRTD_LOCAL_RX > 0 || WRTD_NET_RX > 0 || NBR_ALARMS > 0
/* Route an event coming from a local input, network, or alarm.  */
static void wrtd_route_in(struct wrtd_event *ev)
{
        struct wrtd_rule *rule;
        unsigned int hash = wrtd_id_hash(ev->id) % NBR_RULES;

        rule = &rules[hash];
        while (1) {
                /* A disabled rule cannot be followed by an enabled one. */
                if (!rule->conf.enabled)
                        break;

                if (memcmp(ev->id, rule->conf.source_id, WRTD_ID_LEN) == 0)
                        wrtd_route(rule, ev);

                if (rule->conf.hash_chain == -1)
                        break;
                rule = &rules[rule->conf.hash_chain];
        }
}
#endif

#if WRTD_NET_RX > 0
static void wrtd_init_rx(void)
{
        static const struct trtl_ep_eth_address addr = {
                .type = TRTL_EP_FRAME_UDP,
                .dst_mac = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF},
                .dst_ip = 0xE000179F, /* multicast on 224.0.23.159 */
                .dst_port = WRTD_UDP_PORT,
                .ethertype = 0x800, /* IPv4 */
                .filter = (TRTL_EP_FILTER_UDP
                           | TRTL_EP_FILTER_DST_PORT
                           | TRTL_EP_FILTER_ETHERTYPE
                           | TRTL_EP_FILTER_DST_IP
                           | TRTL_EP_FILTER_ENABLE)
        };
        rmq_bind_in(WRTD_RMQ, TRTL_EP_ETH, &addr);
}

static void wrtd_recv_network(void)
{
        volatile struct wrtd_message *msg;
        struct wrtd_event ev;

        if (!mq_poll_in(TRTL_RMQ, 1 << WRTD_RMQ)) {
                /* No packet.  */
                return;
        }

        msg = mq_map_in_buffer(TRTL_RMQ, WRTD_RMQ);

        ev.seq = msg->seq;

        ev.ts.seconds = msg->ts_sec | ((uint64_t)msg->ts_hi_sec << 32);
        ev.ts.ns = msg->ts_ns;
        ev.ts.frac = msg->ts_frac << 16;

        memcpy(ev.id, (char *)msg->event_id, WRTD_ID_LEN);

        ev.flags = msg->flags;

        mq_discard(TRTL_RMQ, WRTD_RMQ);

        wrtd_log(WRTD_LOG_MSG_EV_NETWORK, WRTD_LOG_NETWORK_RX, NULL, &ev, NULL);

        wrtd_route_in(&ev);

}
#endif //WRTD_NET_RX

#if NBR_ALARMS > 0
static void wrtd_alarms(void)
{
        struct wrtd_tstamp now;
        int i;

        if (!wr_is_timing_ok())
                return;

        ts_now(&now);
        for (i = 0; i < NBR_ALARMS; i++) {
                struct wrtd_alarm *al = &alarms[i];
                if (!al->enabled)
                        continue;

                if (ts_cmp(&al->setup_time, &now) > 0)
                        continue;

                wrtd_log(WRTD_LOG_MSG_EV_GENERATED, WRTD_LOG_GENERATED_ALARM,
                         NULL, &al->event, &now);

                /* Trigger.  */
                wrtd_route_in(&al->event);

                /* Repeat ?  */
                if (al->repeat_count > 0) {
                        al->repeat_count--;
                        if (al->repeat_count == 0) {
                                /* Disabled.  */
                                al->enabled = 0;
                                continue;
                        }
                }
                if (al->period_ns == 0) {
                        al->enabled = 0;
                        continue;
                }
                ts_add2_ns(&al->setup_time, al->period_ns);
                ts_add2_ns(&al->event.ts, al->period_ns);
        }
}
#endif // NBR_ALARMS

static int wrtd_action_readw(struct trtl_fw_msg *msg_i,
                             struct trtl_fw_msg *msg_o)
{
        volatile struct wrtd_io_msg *imsg;
        volatile uint32_t *odata;
        volatile uint32_t *addr;
        uint32_t len;
        unsigned int i;

        /* Verify that the size is correct */
        if (msg_i->header->len != sizeof(struct wrtd_io_msg) / 4)
                return -EINVAL;

        imsg = (volatile struct wrtd_io_msg *)msg_i->payload;
        odata = (volatile uint32_t *)msg_o->payload;

        addr = (volatile uint32_t *)imsg->addr;
        len = imsg->nwords;

        /* Copy data.  */
        for (i = 0; i < len; i++)
                odata[i] = addr[i];
        msg_o->header->msg_id = WRTD_ACTION_READW;
        msg_o->header->len = len;

        return 0;
}

static int wrtd_action_writew(struct trtl_fw_msg *msg_i,
                              struct trtl_fw_msg *msg_o)
{
        volatile uint32_t *idata;
        volatile uint32_t *addr;
        uint32_t len;
        unsigned int i;

        /* Verify that the size is correct */
        if (msg_i->header->len * 4 < sizeof(uint32_t))
                return -EINVAL;

        idata = (volatile uint32_t *)msg_i->payload;
        addr = (volatile uint32_t *)idata[0];
        len = idata[1];

        /* Copy data.  */
        for (i = 0; i < len; i++)
                addr[i] = idata[i + 2];

        msg_o->header->msg_id = WRTD_ACTION_WRITEW;
        msg_o->header->len = 0;

        return 0;
}

static int wrtd_action_get_config(struct trtl_fw_msg *msg_i,
                                  struct trtl_fw_msg *msg_o)
{
        struct wrtd_config_msg *cfg;

        /* Verify that the size is correct */
        if (msg_i->header->len * 4 != 0)
                return -EINVAL;

        cfg = (struct wrtd_config_msg *)msg_o->payload;
        cfg->root_addr = (uint32_t) &root;
        cfg->sync_flag = wr_is_timing_ok();
        ts_now(&cfg->now);

        msg_o->header->msg_id = WRTD_ACTION_GET_CONFIG;
        msg_o->header->len = sizeof(*cfg) / 4;
        return 0;
}

static trtl_fw_action_t *wrtd_actions[] = {
        [WRTD_ACTION_GET_CONFIG] = wrtd_action_get_config,
        [WRTD_ACTION_READW] = wrtd_action_readw,
        [WRTD_ACTION_WRITEW] = wrtd_action_writew
};

static int wrtd_main(void)
{
        while (1) {
                if (!root.freeze_flag) {
                        wrtd_io();
#if WRTD_NET_RX > 0
                        wrtd_recv_network();
#endif
#if NBR_CPUS > 1
                        wrtd_recv_loopback();
#endif

#if NBR_ALARMS > 0
                        wrtd_alarms();
#endif
                }
                trtl_fw_mq_action_dispatch(TRTL_HMQ, WRTD_HMQ);
                wr_update_link();
        }

        return 0;
}

static int wrtd_sys_init(void)
{
#if WRTD_NET_RX > 0
        wrtd_init_rx();
#endif
#if WRTD_NET_TX > 0
        wrtd_init_tx();
#endif
        root.wr_state = WR_LINK_OFFLINE;

        return wrtd_user_init();
}

struct trtl_fw_application app = {
        .name = APP_NAME,
        .version = {
                .rt_id = APP_ID,
                .rt_version = APP_VER,
                .git_version = GIT_VERSION
        },

        .actions = wrtd_actions,
        .n_actions = ARRAY_SIZE(wrtd_actions),

        .init = wrtd_sys_init,
        .main = wrtd_main,
        .exit = NULL,
};
