// SPDX-License-Identifier: GPL-2.0
/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */

#include "tsnep.h"

#include <net/pkt_sched.h>

/* save one operation at the end for additional operation at list change */
#define TSNEP_MAX_GCL_NUM (TSNEP_GCL_COUNT - 1)

static int tsnep_validate_gcl(struct tc_taprio_qopt_offload *qopt)
{
        int i;
        u64 cycle_time;

        if (!qopt->cycle_time)
                return -ERANGE;
        if (qopt->num_entries > TSNEP_MAX_GCL_NUM)
                return -EINVAL;
        cycle_time = 0;
        for (i = 0; i < qopt->num_entries; i++) {
                if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
                        return -EINVAL;
                if (qopt->entries[i].gate_mask & ~TSNEP_GCL_MASK)
                        return -EINVAL;
                if (qopt->entries[i].interval < TSNEP_GCL_MIN_INTERVAL)
                        return -EINVAL;
                cycle_time += qopt->entries[i].interval;
        }
        if (qopt->cycle_time != cycle_time)
                return -EINVAL;
        if (qopt->cycle_time_extension >= qopt->cycle_time)
                return -EINVAL;

        return 0;
}

static void tsnep_write_gcl_operation(struct tsnep_gcl *gcl, int index,
                                      u32 properties, u32 interval, bool flush)
{
        void __iomem *addr = gcl->addr +
                             sizeof(struct tsnep_gcl_operation) * index;

        gcl->operation[index].properties = properties;
        gcl->operation[index].interval = interval;

        iowrite32(properties, addr);
        iowrite32(interval, addr + sizeof(u32));

        if (flush) {
                /* flush write with read access */
                ioread32(addr);
        }
}

static u64 tsnep_change_duration(struct tsnep_gcl *gcl, int index)
{
        u64 duration;
        int count;

        /* change needs to be triggered one or two operations before start of
         * new gate control list
         * - change is triggered at start of operation (minimum one operation)
         * - operation with adjusted interval is inserted on demand to exactly
         *   meet the start of the new gate control list (optional)
         *
         * additionally properties are read directly after start of previous
         * operation
         *
         * therefore, three operations needs to be considered for the limit
         */
        duration = 0;
        count = 3;
        while (count) {
                duration += gcl->operation[index].interval;

                index--;
                if (index < 0)
                        index = gcl->count - 1;

                count--;
        }

        return duration;
}

static void tsnep_write_gcl(struct tsnep_gcl *gcl,
                            struct tc_taprio_qopt_offload *qopt)
{
        int i;
        u32 properties;
        u64 extend;
        u64 cut;

        gcl->base_time = ktime_to_ns(qopt->base_time);
        gcl->cycle_time = qopt->cycle_time;
        gcl->cycle_time_extension = qopt->cycle_time_extension;

        for (i = 0; i < qopt->num_entries; i++) {
                properties = qopt->entries[i].gate_mask;
                if (i == (qopt->num_entries - 1))
                        properties |= TSNEP_GCL_LAST;

                tsnep_write_gcl_operation(gcl, i, properties,
                                          qopt->entries[i].interval, true);
        }
        gcl->count = qopt->num_entries;

        /* calculate change limit; i.e., the time needed between enable and
         * start of new gate control list
         */

        /* case 1: extend cycle time for change
         * - change duration of last operation
         * - cycle time extension
         */
        extend = tsnep_change_duration(gcl, gcl->count - 1);
        extend += gcl->cycle_time_extension;

        /* case 2: cut cycle time for change
         * - maximum change duration
         */
        cut = 0;
        for (i = 0; i < gcl->count; i++)
                cut = max(cut, tsnep_change_duration(gcl, i));

        /* use maximum, because the actual case (extend or cut) can be
         * determined only after limit is known (chicken-and-egg problem)
         */
        gcl->change_limit = max(extend, cut);
}

static u64 tsnep_gcl_start_after(struct tsnep_gcl *gcl, u64 limit)
{
        u64 start = gcl->base_time;
        u64 n;

        if (start <= limit) {
                n = div64_u64(limit - start, gcl->cycle_time);
                start += (n + 1) * gcl->cycle_time;
        }

        return start;
}

static u64 tsnep_gcl_start_before(struct tsnep_gcl *gcl, u64 limit)
{
        u64 start = gcl->base_time;
        u64 n;

        n = div64_u64(limit - start, gcl->cycle_time);
        start += n * gcl->cycle_time;
        if (start == limit)
                start -= gcl->cycle_time;

        return start;
}

static u64 tsnep_set_gcl_change(struct tsnep_gcl *gcl, int index, u64 change,
                                bool insert)
{
        /* previous operation triggers change and properties are evaluated at
         * start of operation
         */
        if (index == 0)
                index = gcl->count - 1;
        else
                index = index - 1;
        change -= gcl->operation[index].interval;

        /* optionally change to new list with additional operation in between */
        if (insert) {
                void __iomem *addr = gcl->addr +
                                     sizeof(struct tsnep_gcl_operation) * index;

                gcl->operation[index].properties |= TSNEP_GCL_INSERT;
                iowrite32(gcl->operation[index].properties, addr);
        }

        return change;
}

static void tsnep_clean_gcl(struct tsnep_gcl *gcl)
{
        int i;
        u32 mask = TSNEP_GCL_LAST | TSNEP_GCL_MASK;
        void __iomem *addr;

        /* search for insert operation and reset properties */
        for (i = 0; i < gcl->count; i++) {
                if (gcl->operation[i].properties & ~mask) {
                        addr = gcl->addr +
                               sizeof(struct tsnep_gcl_operation) * i;

                        gcl->operation[i].properties &= mask;
                        iowrite32(gcl->operation[i].properties, addr);

                        break;
                }
        }
}

static u64 tsnep_insert_gcl_operation(struct tsnep_gcl *gcl, int ref,
                                      u64 change, u32 interval)
{
        u32 properties;

        properties = gcl->operation[ref].properties & TSNEP_GCL_MASK;
        /* change to new list directly after inserted operation */
        properties |= TSNEP_GCL_CHANGE;

        /* last operation of list is reserved to insert operation */
        tsnep_write_gcl_operation(gcl, TSNEP_GCL_COUNT - 1, properties,
                                  interval, false);

        return tsnep_set_gcl_change(gcl, ref, change, true);
}

static u64 tsnep_extend_gcl(struct tsnep_gcl *gcl, u64 start, u32 extension)
{
        int ref = gcl->count - 1;
        u32 interval = gcl->operation[ref].interval + extension;

        start -= gcl->operation[ref].interval;

        return tsnep_insert_gcl_operation(gcl, ref, start, interval);
}

static u64 tsnep_cut_gcl(struct tsnep_gcl *gcl, u64 start, u64 cycle_time)
{
        u64 sum = 0;
        int i;

        /* find operation which shall be cutted */
        for (i = 0; i < gcl->count; i++) {
                u64 sum_tmp = sum + gcl->operation[i].interval;
                u64 interval;

                /* sum up operations as long as cycle time is not exceeded */
                if (sum_tmp > cycle_time)
                        break;

                /* remaining interval must be big enough for hardware */
                interval = cycle_time - sum_tmp;
                if (interval > 0 && interval < TSNEP_GCL_MIN_INTERVAL)
                        break;

                sum = sum_tmp;
        }
        if (sum == cycle_time) {
                /* no need to cut operation itself or whole cycle
                 * => change exactly at operation
                 */
                return tsnep_set_gcl_change(gcl, i, start + sum, false);
        }
        return tsnep_insert_gcl_operation(gcl, i, start + sum,
                                          cycle_time - sum);
}

static int tsnep_enable_gcl(struct tsnep_adapter *adapter,
                            struct tsnep_gcl *gcl, struct tsnep_gcl *curr)
{
        u64 system_time;
        u64 timeout;
        u64 limit;

        /* estimate timeout limit after timeout enable, actually timeout limit
         * in hardware will be earlier than estimate so we are on the safe side
         */
        tsnep_get_system_time(adapter, &system_time);
        timeout = system_time + TSNEP_GC_TIMEOUT;

        if (curr)
                limit = timeout + curr->change_limit;
        else
                limit = timeout;

        gcl->start_time = tsnep_gcl_start_after(gcl, limit);

        /* gate control time register is only 32bit => time shall be in the near
         * future (no driver support for far future implemented)
         */
        if ((gcl->start_time - system_time) >= U32_MAX)
                return -EAGAIN;

        if (curr) {
                /* change gate control list */
                u64 last;
                u64 change;

                last = tsnep_gcl_start_before(curr, gcl->start_time);
                if ((last + curr->cycle_time) == gcl->start_time)
                        change = tsnep_cut_gcl(curr, last,
                                               gcl->start_time - last);
                else if (((gcl->start_time - last) <=
                          curr->cycle_time_extension) ||
                         ((gcl->start_time - last) <= TSNEP_GCL_MIN_INTERVAL))
                        change = tsnep_extend_gcl(curr, last,
                                                  gcl->start_time - last);
                else
                        change = tsnep_cut_gcl(curr, last,
                                               gcl->start_time - last);

                WARN_ON(change <= timeout);
                gcl->change = true;
                iowrite32(change & 0xFFFFFFFF, adapter->addr + TSNEP_GC_CHANGE);
        } else {
                /* start gate control list */
                WARN_ON(gcl->start_time <= timeout);
                gcl->change = false;
                iowrite32(gcl->start_time & 0xFFFFFFFF,
                          adapter->addr + TSNEP_GC_TIME);
        }

        return 0;
}

static int tsnep_taprio(struct tsnep_adapter *adapter,
                        struct tc_taprio_qopt_offload *qopt)
{
        struct tsnep_gcl *gcl;
        struct tsnep_gcl *curr;
        int retval;

        if (!adapter->gate_control)
                return -EOPNOTSUPP;

        if (qopt->cmd == TAPRIO_CMD_DESTROY) {
                /* disable gate control if active */
                mutex_lock(&adapter->gate_control_lock);

                if (adapter->gate_control_active) {
                        iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
                        adapter->gate_control_active = false;
                }

                mutex_unlock(&adapter->gate_control_lock);

                return 0;
        } else if (qopt->cmd != TAPRIO_CMD_REPLACE) {
                return -EOPNOTSUPP;
        }

        retval = tsnep_validate_gcl(qopt);
        if (retval)
                return retval;

        mutex_lock(&adapter->gate_control_lock);

        gcl = &adapter->gcl[adapter->next_gcl];
        tsnep_write_gcl(gcl, qopt);

        /* select current gate control list if active */
        if (adapter->gate_control_active) {
                if (adapter->next_gcl == 0)
                        curr = &adapter->gcl[1];
                else
                        curr = &adapter->gcl[0];
        } else {
                curr = NULL;
        }

        for (;;) {
                /* start timeout which discards late enable, this helps ensuring
                 * that start/change time are in the future at enable
                 */
                iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);

                retval = tsnep_enable_gcl(adapter, gcl, curr);
                if (retval) {
                        mutex_unlock(&adapter->gate_control_lock);

                        return retval;
                }

                /* enable gate control list */
                if (adapter->next_gcl == 0)
                        iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
                else
                        iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC);

                /* done if timeout did not happen */
                if (!(ioread32(adapter->addr + TSNEP_GC) &
                      TSNEP_GC_TIMEOUT_SIGNAL))
                        break;

                /* timeout is acknowledged with any enable */
                iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);

                if (curr)
                        tsnep_clean_gcl(curr);

                /* retry because of timeout */
        }

        adapter->gate_control_active = true;

        if (adapter->next_gcl == 0)
                adapter->next_gcl = 1;
        else
                adapter->next_gcl = 0;

        mutex_unlock(&adapter->gate_control_lock);

        return 0;
}

static int tsnep_tc_query_caps(struct tsnep_adapter *adapter,
                               struct tc_query_caps_base *base)
{
        switch (base->type) {
        case TC_SETUP_QDISC_TAPRIO: {
                struct tc_taprio_caps *caps = base->caps;

                if (!adapter->gate_control)
                        return -EOPNOTSUPP;

                caps->gate_mask_per_txq = true;

                return 0;
        }
        default:
                return -EOPNOTSUPP;
        }
}

int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,
                   void *type_data)
{
        struct tsnep_adapter *adapter = netdev_priv(netdev);

        switch (type) {
        case TC_QUERY_CAPS:
                return tsnep_tc_query_caps(adapter, type_data);
        case TC_SETUP_QDISC_TAPRIO:
                return tsnep_taprio(adapter, type_data);
        default:
                return -EOPNOTSUPP;
        }
}

int tsnep_tc_init(struct tsnep_adapter *adapter)
{
        if (!adapter->gate_control)
                return 0;

        /* open all gates */
        iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
        iowrite32(TSNEP_GC_OPEN | TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC);

        adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A;
        adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B;

        return 0;
}

void tsnep_tc_cleanup(struct tsnep_adapter *adapter)
{
        if (!adapter->gate_control)
                return;

        if (adapter->gate_control_active) {
                iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
                adapter->gate_control_active = false;
        }
}