/**
 * @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}            // Number of channels
 * #define DEVICES_CHS_DIR { WRTD_CH_DIR_IN, 0, 0, 0} // Direction
 * #define APP_ID WRTD_APP_xxx
 * #define APP_VER RT_VERSION(xxx, 0)
 * #define APP_NAME xxx
 *  // 1 = enabled, 0 = disabled
 * #define WRTD_NET_RX x    //  1 if receive events from the network
 * #define WRTD_NET_TX x    //  1 if transmit events to the network
 * #define WRTD_LOCAL_RX x  //  1 if consume events
 * #define WRTD_LOCAL_TX x  //  1 if generate events
 * #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

/* The following functions are application-specific and need to be defined by the user. */

/**
 * Check whether the WR link is up or not.
 *
 * If the application does not need this, write a function that returns always ``1``.
 *
 * @return ``1`` if true, ``0`` otherwise.
 */
static int  wr_link_up(void);

/**
 * Check whether the WR time is valid.
 *
 * If the application does not need this, write a function that returns always ``1``.
 *
 * @return ``1`` if true, ``0`` otherwise.
 */
static int  wr_time_ready(void);

/**
 * Check whether the application clock is locked to the WR clock or not.
 *
 * If the application does not need this, write a function that returns always ``1``.
 *
 * @return ``1`` if true, ``0`` otherwise.
 */
static int  wr_aux_locked(void);

/**
 * Enable/disable locking of the application clock to the WR clock.
 *
 * If the application does not need this, write a function that does nothing.
 *
 * @param[in] enable         set to ``1`` to enable, set to ``0`` to disable.
 */
static void wr_enable_lock(int enable);

/**
 * Generate an output Event on a Local Channel.
 *
 * If the application does not need this, write a function that returns always ``0``.
 *
 * @param[in] ev    pointer to a struct wrtd_event, representing the Event to send.
 * @param[in] ch    Local Channel number to use.
 * @return ``0`` on success, or error code otherwise.
 */
static int  wrtd_local_output(struct wrtd_event *ev, unsigned ch);

/**
 * Return the delay in seconds that the application should wait before
 * considering the WR link synced.
 *
 * If the application does not need this, write a function that returns always ``0``.
 *
 * @return Integer number of seconds to wait.
 */
static int  wr_sync_timeout(void);

/**
 * Function to perform any apllication-specific initialisation (runs once after reset).
 *
 * If the application does not need this, write a function that returns always ``0``.
 *
 * @return ``0`` on success, or error code otherwise.
 */
static int  wrtd_user_init(void);

/**
 * Function to perform the main tasks of the application. This will run in a loop.
 *
 * Typically the application will check here for incoming Events and pass them to
 * WRTD via the wrtd_route_in() function. It might also want to check if output
 * Events previously scheduled via wrtd_local_output() have been executed or not.
 *
 * If the application does not need this, write a function that returns always ``0``.
 *
 * @return ``0`` on success, or error code otherwise.
 */
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_aux_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_ps(struct wrtd_tstamp *dest,
                              const struct wrtd_tstamp *src, uint32_t ps)
{
        uint32_t ps_ns = ps / 1000;
        uint32_t ps_left = ps % 1000;

        dest->seconds = src->seconds;
        dest->ns = src->ns + ps_ns;
        dest->frac = src->frac + (WRTD_TSTAMP_FRAC_PS * ps_left);

        if (dest->ns >= 1000000000) {
                dest->ns -= 1000000000;
                dest->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 void ts_sub3_ps(struct wrtd_tstamp *dest,
                              const struct wrtd_tstamp *src, uint32_t ps)
{
        uint32_t ps_ns = ps / 1000;
        uint32_t ps_left = ps % 1000;

        dest->seconds = src->seconds;
        dest->ns = src->ns - ps_ns;
        dest->frac = src->frac - (WRTD_TSTAMP_FRAC_PS * ps_left);

        if (ps_ns > src->ns)
                dest->seconds--;

        if ((WRTD_TSTAMP_FRAC_PS * ps_left) > src->frac)
                dest->ns --;
}

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;
}

/* Clear an id.  Like memset but denser.  */
static inline void zero_id(union wrtd_id *id)
{
        id->w[0] = 0;
        id->w[1] = 0;
        id->w[2] = 0;
        id->w[3] = 0;
}

/* Copy an id.  Like memcpy but denser.  */
static inline void copy_id(union wrtd_id *dest, const union wrtd_id *src)
{
        dest->w[0] = src->w[0];
        dest->w[1] = src->w[1];
        dest->w[2] = src->w[2];
        dest->w[3] = src->w[3];
}

/* Clear an event.  Like memset but denser.  */
static inline void zero_event(struct wrtd_event *ev)
{
        ev->ts.seconds = 0;
        ev->ts.ns = 0;
        ev->ts.frac = 0;

        zero_id(&ev->id);

        ev->seq = 0;
        ev->flags = 0;
}

static inline void copy_tstamp(struct wrtd_tstamp *dest,
                           const struct wrtd_tstamp *src)
{
        dest->seconds = src->seconds;
        dest->ns = src->ns;
        dest->frac = src->frac;
}

/* Copy an id.  Like memcpy but denser.  */
static inline void copy_event(struct wrtd_event *dest,
                              const struct wrtd_event *src)
{
        copy_tstamp(&dest->ts, &src->ts);
        copy_id(&dest->id, &src->id);
        dest->seq = src->seq;
        dest->flags = src->flags;
}

/* Returns true iff L is the same id as R.  */
static inline int wrtd_id_eq(const union wrtd_id *l, const union wrtd_id *r)
{
        return !((l->w[0] ^ r->w[0])
                 | (l->w[1] ^ r->w[1])
                 | (l->w[2] ^ r->w[2])
                 | (l->w[3] ^ r->w[3]));
}

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

        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;
        copy_event(&log->event, ev);

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

        mq_send(TRTL_HMQ, WRTD_HMQ);

}

/* Send a log message (only if log is enabled).
   If TS is NULL, use the current time as time stamp.  */
static inline void wrtd_log(uint32_t type, uint32_t reason,
                            const struct wrtd_event *ev,
                            const struct wrtd_tstamp *ts)
{
        if (root.log_flags == 0)
                return;
        wrtd_log_1(type, reason, ev, ts);
}

static void wrtd_log_discard(struct wrtd_rule *rule,
                             uint32_t reason,
                             const struct wrtd_event *ev,
                             const struct wrtd_tstamp *ts)
{
        copy_tstamp(&rule->stat.miss_last, &ev->ts);

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

#if WRTD_NET_TX > 0
static void wrtd_init_tx(void)
{
        static const struct trtl_ep_eth_address addr = {
                .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;
        copy_id((union wrtd_id *)&msg->event_id, &ev->id);
        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, 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) {
                        rule->stat.miss_holdoff++;
                        wrtd_log_discard(rule, WRTD_LOG_DISCARD_HOLDOFF,
                                         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++;
        copy_tstamp(&rule->stat.rx_last, &ev->ts);

        /* Delay.  */
        if (rule->conf.delay_ns)
                ts_add3_ns(&tev.ts, &ev->ts, rule->conf.delay_ns);
        else
                copy_tstamp(&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.  */
        copy_id(&tev.id, &rule->conf.dest_id);
        tev.seq = rule->stat.seq++;

        uint32_t res = 0;

        /* Check timing.  */
        if (!wr_is_timing_ok()) {
                rule->stat.miss_nosync++;
                wrtd_log_discard(rule, WRTD_LOG_DISCARD_NO_SYNC, 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... */
                        rule->stat.miss_late++;
                        wrtd_log_discard(rule, WRTD_LOG_DISCARD_TIMEOUT,
                                         ev, &now);
                        return;
                }
        }

        /* Send.  */
        if (rule->conf.dest_ch == WRTD_DEST_CH_NET) {
#if WRTD_NET_TX > 0
                res = wrtd_send_network(&tev);
#elif NBR_CPUS > 1
                res = wrtd_remote_output
                        (&tev, rule->conf.dest_cpu, WRTD_DEST_CH_NET);
#else
                /* Event cannot be consumed.  */
                res = -EOVERFLOW;
#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) {
                rule->stat.miss_overflow++;
                wrtd_log_discard(rule, WRTD_LOG_DISCARD_OVERFLOW, ev, NULL);
        }
        else if (res == 0) {
                rule->stat.tx_events++;
                copy_tstamp(&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);
}

/**
 * Route an event coming from a Local Input, Network, or Alarm.
 *
 * @param[in] ev    pointer to a struct wrtd_event, representing the received Event.
 */
#if WRTD_LOCAL_RX > 0 || WRTD_NET_RX > 0 || NBR_ALARMS > 0
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 (wrtd_id_eq(&ev->id, &rule->conf.source_id))
                        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 = {
                .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;

        copy_id(&ev.id, (union wrtd_id*)&msg->event_id);

        ev.flags = msg->flags;

        mq_discard(TRTL_RMQ, WRTD_RMQ);

        wrtd_log(WRTD_LOG_MSG_EV_NETWORK, WRTD_LOG_NETWORK_RX, &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;

                /* Wait until the setup time is over.  */
                if (ts_cmp(&al->setup_time, &now) > 0)
                        continue;

                /* TODO/FIXME: discard if the alarm time is over ?  */

                wrtd_log(WRTD_LOG_MSG_EV_GENERATED, WRTD_LOG_GENERATED_ALARM,
                         &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,
};
