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

#include "tsnep.h"

#define ETHER_TYPE_FULL_MASK ((__force __be16)~0)

static void tsnep_enable_rule(struct tsnep_adapter *adapter,
                              struct tsnep_rxnfc_rule *rule)
{
        u8 rx_assign;
        void __iomem *addr;

        rx_assign = TSNEP_RX_ASSIGN_ACTIVE;
        rx_assign |= (rule->queue_index << TSNEP_RX_ASSIGN_QUEUE_SHIFT) &
                     TSNEP_RX_ASSIGN_QUEUE_MASK;

        addr = adapter->addr + TSNEP_RX_ASSIGN_ETHER_TYPE +
               TSNEP_RX_ASSIGN_ETHER_TYPE_OFFSET * rule->location;
        iowrite16(rule->filter.ether_type, addr);

        /* enable rule after all settings are done */
        addr = adapter->addr + TSNEP_RX_ASSIGN +
               TSNEP_RX_ASSIGN_OFFSET * rule->location;
        iowrite8(rx_assign, addr);
}

static void tsnep_disable_rule(struct tsnep_adapter *adapter,
                               struct tsnep_rxnfc_rule *rule)
{
        void __iomem *addr;

        addr = adapter->addr + TSNEP_RX_ASSIGN +
               TSNEP_RX_ASSIGN_OFFSET * rule->location;
        iowrite8(0, addr);
}

static struct tsnep_rxnfc_rule *tsnep_get_rule(struct tsnep_adapter *adapter,
                                               int location)
{
        struct tsnep_rxnfc_rule *rule;

        list_for_each_entry(rule, &adapter->rxnfc_rules, list) {
                if (rule->location == location)
                        return rule;
                if (rule->location > location)
                        break;
        }

        return NULL;
}

static void tsnep_add_rule(struct tsnep_adapter *adapter,
                           struct tsnep_rxnfc_rule *rule)
{
        struct tsnep_rxnfc_rule *pred, *cur;

        tsnep_enable_rule(adapter, rule);

        pred = NULL;
        list_for_each_entry(cur, &adapter->rxnfc_rules, list) {
                if (cur->location >= rule->location)
                        break;
                pred = cur;
        }

        list_add(&rule->list, pred ? &pred->list : &adapter->rxnfc_rules);
        adapter->rxnfc_count++;
}

static void tsnep_delete_rule(struct tsnep_adapter *adapter,
                              struct tsnep_rxnfc_rule *rule)
{
        tsnep_disable_rule(adapter, rule);

        list_del(&rule->list);
        adapter->rxnfc_count--;

        kfree(rule);
}

static void tsnep_flush_rules(struct tsnep_adapter *adapter)
{
        struct tsnep_rxnfc_rule *rule, *tmp;

        mutex_lock(&adapter->rxnfc_lock);

        list_for_each_entry_safe(rule, tmp, &adapter->rxnfc_rules, list)
                tsnep_delete_rule(adapter, rule);

        mutex_unlock(&adapter->rxnfc_lock);
}

int tsnep_rxnfc_get_rule(struct tsnep_adapter *adapter,
                         struct ethtool_rxnfc *cmd)
{
        struct ethtool_rx_flow_spec *fsp = &cmd->fs;
        struct tsnep_rxnfc_rule *rule = NULL;

        cmd->data = adapter->rxnfc_max;

        mutex_lock(&adapter->rxnfc_lock);

        rule = tsnep_get_rule(adapter, fsp->location);
        if (!rule) {
                mutex_unlock(&adapter->rxnfc_lock);

                return -ENOENT;
        }

        fsp->flow_type = ETHER_FLOW;
        fsp->ring_cookie = rule->queue_index;

        if (rule->filter.type == TSNEP_RXNFC_ETHER_TYPE) {
                fsp->h_u.ether_spec.h_proto = htons(rule->filter.ether_type);
                fsp->m_u.ether_spec.h_proto = ETHER_TYPE_FULL_MASK;
        }

        mutex_unlock(&adapter->rxnfc_lock);

        return 0;
}

int tsnep_rxnfc_get_all(struct tsnep_adapter *adapter,
                        struct ethtool_rxnfc *cmd,
                        u32 *rule_locs)
{
        struct tsnep_rxnfc_rule *rule;
        int count = 0;

        cmd->data = adapter->rxnfc_max;

        mutex_lock(&adapter->rxnfc_lock);

        list_for_each_entry(rule, &adapter->rxnfc_rules, list) {
                if (count == cmd->rule_cnt) {
                        mutex_unlock(&adapter->rxnfc_lock);

                        return -EMSGSIZE;
                }

                rule_locs[count] = rule->location;
                count++;
        }

        mutex_unlock(&adapter->rxnfc_lock);

        cmd->rule_cnt = count;

        return 0;
}

static int tsnep_rxnfc_find_location(struct tsnep_adapter *adapter)
{
        struct tsnep_rxnfc_rule *tmp;
        int location = 0;

        list_for_each_entry(tmp, &adapter->rxnfc_rules, list) {
                if (tmp->location == location)
                        location++;
                else
                        return location;
        }

        if (location >= adapter->rxnfc_max)
                return -ENOSPC;

        return location;
}

static void tsnep_rxnfc_init_rule(struct tsnep_rxnfc_rule *rule,
                                  const struct ethtool_rx_flow_spec *fsp)
{
        INIT_LIST_HEAD(&rule->list);

        rule->queue_index = fsp->ring_cookie;
        rule->location = fsp->location;

        rule->filter.type = TSNEP_RXNFC_ETHER_TYPE;
        rule->filter.ether_type = ntohs(fsp->h_u.ether_spec.h_proto);
}

static int tsnep_rxnfc_check_rule(struct tsnep_adapter *adapter,
                                  struct tsnep_rxnfc_rule *rule)
{
        struct net_device *dev = adapter->netdev;
        struct tsnep_rxnfc_rule *tmp;

        list_for_each_entry(tmp, &adapter->rxnfc_rules, list) {
                if (!memcmp(&rule->filter, &tmp->filter, sizeof(rule->filter)) &&
                    tmp->location != rule->location) {
                        netdev_dbg(dev, "rule already exists\n");

                        return -EEXIST;
                }
        }

        return 0;
}

int tsnep_rxnfc_add_rule(struct tsnep_adapter *adapter,
                         struct ethtool_rxnfc *cmd)
{
        struct net_device *netdev = adapter->netdev;
        struct ethtool_rx_flow_spec *fsp =
                (struct ethtool_rx_flow_spec *)&cmd->fs;
        struct tsnep_rxnfc_rule *rule, *old_rule;
        int retval;

        /* only EtherType is supported */
        if (fsp->flow_type != ETHER_FLOW ||
            !is_zero_ether_addr(fsp->m_u.ether_spec.h_dest) ||
            !is_zero_ether_addr(fsp->m_u.ether_spec.h_source) ||
            fsp->m_u.ether_spec.h_proto != ETHER_TYPE_FULL_MASK) {
                netdev_dbg(netdev, "only ethernet protocol is supported\n");

                return -EOPNOTSUPP;
        }

        if (fsp->ring_cookie >
            (TSNEP_RX_ASSIGN_QUEUE_MASK >> TSNEP_RX_ASSIGN_QUEUE_SHIFT)) {
                netdev_dbg(netdev, "invalid action\n");

                return -EINVAL;
        }

        if (fsp->location != RX_CLS_LOC_ANY &&
            fsp->location >= adapter->rxnfc_max) {
                netdev_dbg(netdev, "invalid location\n");

                return -EINVAL;
        }

        rule = kzalloc_obj(*rule);
        if (!rule)
                return -ENOMEM;

        mutex_lock(&adapter->rxnfc_lock);

        if (fsp->location == RX_CLS_LOC_ANY) {
                retval = tsnep_rxnfc_find_location(adapter);
                if (retval < 0)
                        goto failed;
                fsp->location = retval;
        }

        tsnep_rxnfc_init_rule(rule, fsp);

        retval = tsnep_rxnfc_check_rule(adapter, rule);
        if (retval)
                goto failed;

        old_rule = tsnep_get_rule(adapter, fsp->location);
        if (old_rule)
                tsnep_delete_rule(adapter, old_rule);

        tsnep_add_rule(adapter, rule);

        mutex_unlock(&adapter->rxnfc_lock);

        return 0;

failed:
        mutex_unlock(&adapter->rxnfc_lock);
        kfree(rule);
        return retval;
}

int tsnep_rxnfc_del_rule(struct tsnep_adapter *adapter,
                         struct ethtool_rxnfc *cmd)
{
        struct ethtool_rx_flow_spec *fsp =
                (struct ethtool_rx_flow_spec *)&cmd->fs;
        struct tsnep_rxnfc_rule *rule;

        mutex_lock(&adapter->rxnfc_lock);

        rule = tsnep_get_rule(adapter, fsp->location);
        if (!rule) {
                mutex_unlock(&adapter->rxnfc_lock);

                return -ENOENT;
        }

        tsnep_delete_rule(adapter, rule);

        mutex_unlock(&adapter->rxnfc_lock);

        return 0;
}

int tsnep_rxnfc_init(struct tsnep_adapter *adapter)
{
        int i;

        /* disable all rules */
        for (i = 0; i < adapter->rxnfc_max;
             i += sizeof(u32) / TSNEP_RX_ASSIGN_OFFSET)
                iowrite32(0, adapter->addr + TSNEP_RX_ASSIGN + i);

        return 0;
}

void tsnep_rxnfc_cleanup(struct tsnep_adapter *adapter)
{
        tsnep_flush_rules(adapter);
}