308 lines
6.7 KiB
C
308 lines
6.7 KiB
C
// 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(sizeof(*rule), GFP_KERNEL);
|
|
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);
|
|
}
|