// SPDX-License-Identifier: GPL-2.0+

#include "lan966x_main.h"

#define LAN966X_TAPRIO_TIMEOUT_MS               1000
#define LAN966X_TAPRIO_ENTRIES_PER_PORT         2

/* Minimum supported cycle time in nanoseconds */
#define LAN966X_TAPRIO_MIN_CYCLE_TIME_NS        NSEC_PER_USEC

/* Maximum supported cycle time in nanoseconds */
#define LAN966X_TAPRIO_MAX_CYCLE_TIME_NS        (NSEC_PER_SEC - 1)

/* Total number of TAS GCL entries */
#define LAN966X_TAPRIO_NUM_GCL                  256

/* TAPRIO link speeds for calculation of guard band */
enum lan966x_taprio_link_speed {
        LAN966X_TAPRIO_SPEED_NO_GB,
        LAN966X_TAPRIO_SPEED_10,
        LAN966X_TAPRIO_SPEED_100,
        LAN966X_TAPRIO_SPEED_1000,
        LAN966X_TAPRIO_SPEED_2500,
};

/* TAPRIO list states */
enum lan966x_taprio_state {
        LAN966X_TAPRIO_STATE_ADMIN,
        LAN966X_TAPRIO_STATE_ADVANCING,
        LAN966X_TAPRIO_STATE_PENDING,
        LAN966X_TAPRIO_STATE_OPERATING,
        LAN966X_TAPRIO_STATE_TERMINATING,
        LAN966X_TAPRIO_STATE_MAX,
};

/* TAPRIO GCL command */
enum lan966x_taprio_gcl_cmd {
        LAN966X_TAPRIO_GCL_CMD_SET_GATE_STATES = 0,
};

static u32 lan966x_taprio_list_index(struct lan966x_port *port, u8 entry)
{
        return port->chip_port * LAN966X_TAPRIO_ENTRIES_PER_PORT + entry;
}

static u32 lan966x_taprio_list_state_get(struct lan966x_port *port)
{
        struct lan966x *lan966x = port->lan966x;
        u32 val;

        val = lan_rd(lan966x, QSYS_TAS_LST);
        return QSYS_TAS_LST_LIST_STATE_GET(val);
}

static u32 lan966x_taprio_list_index_state_get(struct lan966x_port *port,
                                               u32 list)
{
        struct lan966x *lan966x = port->lan966x;

        lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_SET(list),
                QSYS_TAS_CFG_CTRL_LIST_NUM,
                lan966x, QSYS_TAS_CFG_CTRL);

        return lan966x_taprio_list_state_get(port);
}

static void lan966x_taprio_list_state_set(struct lan966x_port *port,
                                          u32 state)
{
        struct lan966x *lan966x = port->lan966x;

        lan_rmw(QSYS_TAS_LST_LIST_STATE_SET(state),
                QSYS_TAS_LST_LIST_STATE,
                lan966x, QSYS_TAS_LST);
}

static int lan966x_taprio_list_shutdown(struct lan966x_port *port,
                                        u32 list)
{
        struct lan966x *lan966x = port->lan966x;
        bool pending, operating;
        unsigned long end;
        u32 state;

        end = jiffies +  msecs_to_jiffies(LAN966X_TAPRIO_TIMEOUT_MS);
        /* It is required to try multiple times to set the state of list,
         * because the HW can overwrite this.
         */
        do {
                state = lan966x_taprio_list_state_get(port);

                pending = false;
                operating = false;

                if (state == LAN966X_TAPRIO_STATE_ADVANCING ||
                    state == LAN966X_TAPRIO_STATE_PENDING) {
                        lan966x_taprio_list_state_set(port,
                                                      LAN966X_TAPRIO_STATE_ADMIN);
                        pending = true;
                }

                if (state == LAN966X_TAPRIO_STATE_OPERATING) {
                        lan966x_taprio_list_state_set(port,
                                                      LAN966X_TAPRIO_STATE_TERMINATING);
                        operating = true;
                }

                /* If the entry was in pending and now gets in admin, then there
                 * is nothing else to do, so just bail out
                 */
                state = lan966x_taprio_list_state_get(port);
                if (pending &&
                    state == LAN966X_TAPRIO_STATE_ADMIN)
                        return 0;

                /* If the list was in operating and now is in terminating or
                 * admin, then is OK to exit but it needs to wait until the list
                 * will get in admin. It is not required to set the state
                 * again.
                 */
                if (operating &&
                    (state == LAN966X_TAPRIO_STATE_TERMINATING ||
                     state == LAN966X_TAPRIO_STATE_ADMIN))
                        break;

        } while (!time_after(jiffies, end));

        end = jiffies + msecs_to_jiffies(LAN966X_TAPRIO_TIMEOUT_MS);
        do {
                state = lan966x_taprio_list_state_get(port);
                if (state == LAN966X_TAPRIO_STATE_ADMIN)
                        break;

        } while (!time_after(jiffies, end));

        /* If the list was in operating mode, it could be stopped while some
         * queues where closed, so make sure to restore "all-queues-open"
         */
        if (operating) {
                lan_wr(QSYS_TAS_GS_CTRL_HSCH_POS_SET(port->chip_port),
                       lan966x, QSYS_TAS_GS_CTRL);

                lan_wr(QSYS_TAS_GATE_STATE_TAS_GATE_STATE_SET(0xff),
                       lan966x, QSYS_TAS_GATE_STATE);
        }

        return 0;
}

static int lan966x_taprio_shutdown(struct lan966x_port *port)
{
        u32 i, list, state;
        int err;

        for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
                list = lan966x_taprio_list_index(port, i);
                state = lan966x_taprio_list_index_state_get(port, list);
                if (state == LAN966X_TAPRIO_STATE_ADMIN)
                        continue;

                err = lan966x_taprio_list_shutdown(port, list);
                if (err)
                        return err;
        }

        return 0;
}

/* Find a suitable list for a new schedule. First priority is a list in state
 * pending. Second priority is a list in state admin.
 */
static int lan966x_taprio_find_list(struct lan966x_port *port,
                                    struct tc_taprio_qopt_offload *qopt,
                                    int *new_list, int *obs_list)
{
        int state[LAN966X_TAPRIO_ENTRIES_PER_PORT];
        int list[LAN966X_TAPRIO_ENTRIES_PER_PORT];
        int err, oper = -1;
        u32 i;

        *new_list = -1;
        *obs_list = -1;

        /* If there is already an entry in operating mode, return this list in
         * obs_list, such that when the new list will get activated the
         * operating list will be stopped. In this way is possible to have
         * smooth transitions between the lists
         */
        for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
                list[i] = lan966x_taprio_list_index(port, i);
                state[i] = lan966x_taprio_list_index_state_get(port, list[i]);
                if (state[i] == LAN966X_TAPRIO_STATE_OPERATING)
                        oper = list[i];
        }

        for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
                if (state[i] == LAN966X_TAPRIO_STATE_PENDING) {
                        err = lan966x_taprio_shutdown(port);
                        if (err)
                                return err;

                        *new_list = list[i];
                        *obs_list = (oper == -1) ? *new_list : oper;
                        return 0;
                }
        }

        for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
                if (state[i] == LAN966X_TAPRIO_STATE_ADMIN) {
                        *new_list = list[i];
                        *obs_list = (oper == -1) ? *new_list : oper;
                        return 0;
                }
        }

        return -ENOSPC;
}

static int lan966x_taprio_check(struct tc_taprio_qopt_offload *qopt)
{
        u64 total_time = 0;
        u32 i;

        /* This is not supported by th HW */
        if (qopt->cycle_time_extension)
                return -EOPNOTSUPP;

        /* There is a limited number of gcl entries that can be used, they are
         * shared by all ports
         */
        if (qopt->num_entries > LAN966X_TAPRIO_NUM_GCL)
                return -EINVAL;

        /* Don't allow cycle times bigger than 1 sec or smaller than 1 usec */
        if (qopt->cycle_time < LAN966X_TAPRIO_MIN_CYCLE_TIME_NS ||
            qopt->cycle_time > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
                return -EINVAL;

        for (i = 0; i < qopt->num_entries; ++i) {
                struct tc_taprio_sched_entry *entry = &qopt->entries[i];

                /* Don't allow intervals bigger than 1 sec or smaller than 1
                 * usec
                 */
                if (entry->interval < LAN966X_TAPRIO_MIN_CYCLE_TIME_NS ||
                    entry->interval > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
                        return -EINVAL;

                if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
                        return -EINVAL;

                total_time += qopt->entries[i].interval;
        }

        /* Don't allow the total time of intervals be bigger than 1 sec */
        if (total_time > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
                return -EINVAL;

        /* The HW expects that the cycle time to be at least as big as sum of
         * each interval of gcl
         */
        if (qopt->cycle_time < total_time)
                return -EINVAL;

        return 0;
}

static int lan966x_taprio_gcl_free_get(struct lan966x_port *port,
                                       unsigned long *free_list)
{
        struct lan966x *lan966x = port->lan966x;
        u32 num_free, state, list;
        u32 base, next, max_list;

        /* By default everything is free */
        bitmap_fill(free_list, LAN966X_TAPRIO_NUM_GCL);
        num_free = LAN966X_TAPRIO_NUM_GCL;

        /* Iterate over all gcl entries and find out which are free. And mark
         * those that are not free.
         */
        max_list = lan966x->num_phys_ports * LAN966X_TAPRIO_ENTRIES_PER_PORT;
        for (list = 0; list < max_list; ++list) {
                state = lan966x_taprio_list_index_state_get(port, list);
                if (state == LAN966X_TAPRIO_STATE_ADMIN)
                        continue;

                base = lan_rd(lan966x, QSYS_TAS_LIST_CFG);
                base = QSYS_TAS_LIST_CFG_LIST_BASE_ADDR_GET(base);
                next = base;

                do {
                        clear_bit(next, free_list);
                        num_free--;

                        lan_rmw(QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM_SET(next),
                                QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM,
                                lan966x, QSYS_TAS_CFG_CTRL);

                        next = lan_rd(lan966x, QSYS_TAS_GCL_CT_CFG2);
                        next = QSYS_TAS_GCL_CT_CFG2_NEXT_GCL_GET(next);
                } while (base != next);
        }

        return num_free;
}

static void lan966x_taprio_gcl_setup_entry(struct lan966x_port *port,
                                           struct tc_taprio_sched_entry *entry,
                                           u32 next_entry)
{
        struct lan966x *lan966x = port->lan966x;

        /* Setup a single gcl entry */
        lan_wr(QSYS_TAS_GCL_CT_CFG_GATE_STATE_SET(entry->gate_mask) |
               QSYS_TAS_GCL_CT_CFG_HSCH_POS_SET(port->chip_port) |
               QSYS_TAS_GCL_CT_CFG_OP_TYPE_SET(LAN966X_TAPRIO_GCL_CMD_SET_GATE_STATES),
               lan966x, QSYS_TAS_GCL_CT_CFG);

        lan_wr(QSYS_TAS_GCL_CT_CFG2_PORT_PROFILE_SET(port->chip_port) |
               QSYS_TAS_GCL_CT_CFG2_NEXT_GCL_SET(next_entry),
               lan966x, QSYS_TAS_GCL_CT_CFG2);

        lan_wr(entry->interval, lan966x, QSYS_TAS_GCL_TM_CFG);
}

static int lan966x_taprio_gcl_setup(struct lan966x_port *port,
                                    struct tc_taprio_qopt_offload *qopt,
                                    int list)
{
        DECLARE_BITMAP(free_list, LAN966X_TAPRIO_NUM_GCL);
        struct lan966x *lan966x = port->lan966x;
        u32 i, base, next;

        if (lan966x_taprio_gcl_free_get(port, free_list) < qopt->num_entries)
                return -ENOSPC;

        /* Select list */
        lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_SET(list),
                QSYS_TAS_CFG_CTRL_LIST_NUM,
                lan966x, QSYS_TAS_CFG_CTRL);

        /* Setup the address of the first gcl entry */
        base = find_first_bit(free_list, LAN966X_TAPRIO_NUM_GCL);
        lan_rmw(QSYS_TAS_LIST_CFG_LIST_BASE_ADDR_SET(base),
                QSYS_TAS_LIST_CFG_LIST_BASE_ADDR,
                lan966x, QSYS_TAS_LIST_CFG);

        /* Iterate over entries and add them to the gcl list */
        next = base;
        for (i = 0; i < qopt->num_entries; ++i) {
                lan_rmw(QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM_SET(next),
                        QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM,
                        lan966x, QSYS_TAS_CFG_CTRL);

                /* If the entry is last, point back to the start of the list */
                if (i == qopt->num_entries - 1)
                        next = base;
                else
                        next = find_next_bit(free_list, LAN966X_TAPRIO_NUM_GCL,
                                             next + 1);

                lan966x_taprio_gcl_setup_entry(port, &qopt->entries[i], next);
        }

        return 0;
}

/* Calculate new base_time based on cycle_time. The HW recommends to have the
 * new base time at least 2 * cycle type + current time
 */
static void lan966x_taprio_new_base_time(struct lan966x *lan966x,
                                         const u32 cycle_time,
                                         const ktime_t org_base_time,
                                         ktime_t *new_base_time)
{
        ktime_t current_time, threshold_time;
        struct timespec64 ts;

        /* Get the current time and calculate the threshold_time */
        lan966x_ptp_gettime64(&lan966x->phc[LAN966X_PHC_PORT].info, &ts);
        current_time = timespec64_to_ktime(ts);
        threshold_time = current_time + (2 * cycle_time);

        /* If the org_base_time is in enough in future just use it */
        if (org_base_time >= threshold_time) {
                *new_base_time = org_base_time;
                return;
        }

        /* If the org_base_time is smaller than current_time, calculate the new
         * base time as following.
         */
        if (org_base_time <= current_time) {
                u64 tmp = current_time - org_base_time;
                u32 rem = 0;

                if (tmp > cycle_time)
                        div_u64_rem(tmp, cycle_time, &rem);
                rem = cycle_time - rem;
                *new_base_time = threshold_time + rem;
                return;
        }

        /* The only left place for org_base_time is between current_time and
         * threshold_time. In this case the new_base_time is calculated like
         * org_base_time + 2 * cycletime
         */
        *new_base_time = org_base_time + 2 * cycle_time;
}

int lan966x_taprio_speed_set(struct lan966x_port *port, int speed)
{
        struct lan966x *lan966x = port->lan966x;
        u8 taprio_speed;

        switch (speed) {
        case SPEED_10:
                taprio_speed = LAN966X_TAPRIO_SPEED_10;
                break;
        case SPEED_100:
                taprio_speed = LAN966X_TAPRIO_SPEED_100;
                break;
        case SPEED_1000:
                taprio_speed = LAN966X_TAPRIO_SPEED_1000;
                break;
        case SPEED_2500:
                taprio_speed = LAN966X_TAPRIO_SPEED_2500;
                break;
        default:
                return -EINVAL;
        }

        lan_rmw(QSYS_TAS_PROFILE_CFG_LINK_SPEED_SET(taprio_speed),
                QSYS_TAS_PROFILE_CFG_LINK_SPEED,
                lan966x, QSYS_TAS_PROFILE_CFG(port->chip_port));

        return 0;
}

int lan966x_taprio_add(struct lan966x_port *port,
                       struct tc_taprio_qopt_offload *qopt)
{
        struct lan966x *lan966x = port->lan966x;
        int err, new_list, obs_list;
        struct timespec64 ts;
        ktime_t base_time;

        err = lan966x_taprio_check(qopt);
        if (err)
                return err;

        err = lan966x_taprio_find_list(port, qopt, &new_list, &obs_list);
        if (err)
                return err;

        err = lan966x_taprio_gcl_setup(port, qopt, new_list);
        if (err)
                return err;

        lan966x_taprio_new_base_time(lan966x, qopt->cycle_time,
                                     qopt->base_time, &base_time);

        ts = ktime_to_timespec64(base_time);
        lan_wr(QSYS_TAS_BT_NSEC_NSEC_SET(ts.tv_nsec),
               lan966x, QSYS_TAS_BT_NSEC);

        lan_wr(lower_32_bits(ts.tv_sec),
               lan966x, QSYS_TAS_BT_SEC_LSB);

        lan_wr(QSYS_TAS_BT_SEC_MSB_SEC_MSB_SET(upper_32_bits(ts.tv_sec)),
               lan966x, QSYS_TAS_BT_SEC_MSB);

        lan_wr(qopt->cycle_time, lan966x, QSYS_TAS_CT_CFG);

        lan_rmw(QSYS_TAS_STARTUP_CFG_OBSOLETE_IDX_SET(obs_list),
                QSYS_TAS_STARTUP_CFG_OBSOLETE_IDX,
                lan966x, QSYS_TAS_STARTUP_CFG);

        /* Start list processing */
        lan_rmw(QSYS_TAS_LST_LIST_STATE_SET(LAN966X_TAPRIO_STATE_ADVANCING),
                QSYS_TAS_LST_LIST_STATE,
                lan966x, QSYS_TAS_LST);

        return err;
}

int lan966x_taprio_del(struct lan966x_port *port)
{
        return lan966x_taprio_shutdown(port);
}

void lan966x_taprio_init(struct lan966x *lan966x)
{
        int num_taprio_lists;
        int p;

        lan_wr(QSYS_TAS_STM_CFG_REVISIT_DLY_SET((256 * 1000) /
                                                lan966x_ptp_get_period_ps()),
               lan966x, QSYS_TAS_STM_CFG);

        num_taprio_lists = lan966x->num_phys_ports *
                           LAN966X_TAPRIO_ENTRIES_PER_PORT;

        /* For now we always use guard band on all queues */
        lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_MAX_SET(num_taprio_lists) |
                QSYS_TAS_CFG_CTRL_ALWAYS_GB_SCH_Q_SET(1),
                QSYS_TAS_CFG_CTRL_LIST_NUM_MAX |
                QSYS_TAS_CFG_CTRL_ALWAYS_GB_SCH_Q,
                lan966x, QSYS_TAS_CFG_CTRL);

        for (p = 0; p < lan966x->num_phys_ports; p++)
                lan_rmw(QSYS_TAS_PROFILE_CFG_PORT_NUM_SET(p),
                        QSYS_TAS_PROFILE_CFG_PORT_NUM,
                        lan966x, QSYS_TAS_PROFILE_CFG(p));
}

void lan966x_taprio_deinit(struct lan966x *lan966x)
{
        int p;

        for (p = 0; p < lan966x->num_phys_ports; ++p) {
                if (!lan966x->ports[p])
                        continue;

                lan966x_taprio_del(lan966x->ports[p]);
        }
}