341 lines
8.8 KiB
C
341 lines
8.8 KiB
C
// SPDX-License-Identifier: GPL-2.0
|
|
/* Copyright 2020, NXP Semiconductors
|
|
*/
|
|
#include "sja1105.h"
|
|
|
|
static struct sja1105_rule *sja1105_rule_find(struct sja1105_private *priv,
|
|
unsigned long cookie)
|
|
{
|
|
struct sja1105_rule *rule;
|
|
|
|
list_for_each_entry(rule, &priv->flow_block.rules, list)
|
|
if (rule->cookie == cookie)
|
|
return rule;
|
|
|
|
return NULL;
|
|
}
|
|
|
|
static int sja1105_find_free_l2_policer(struct sja1105_private *priv)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < SJA1105_NUM_L2_POLICERS; i++)
|
|
if (!priv->flow_block.l2_policer_used[i])
|
|
return i;
|
|
|
|
return -1;
|
|
}
|
|
|
|
static int sja1105_setup_bcast_policer(struct sja1105_private *priv,
|
|
struct netlink_ext_ack *extack,
|
|
unsigned long cookie, int port,
|
|
u64 rate_bytes_per_sec,
|
|
s64 burst)
|
|
{
|
|
struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
|
|
struct sja1105_l2_policing_entry *policing;
|
|
bool new_rule = false;
|
|
unsigned long p;
|
|
int rc;
|
|
|
|
if (!rule) {
|
|
rule = kzalloc(sizeof(*rule), GFP_KERNEL);
|
|
if (!rule)
|
|
return -ENOMEM;
|
|
|
|
rule->cookie = cookie;
|
|
rule->type = SJA1105_RULE_BCAST_POLICER;
|
|
rule->bcast_pol.sharindx = sja1105_find_free_l2_policer(priv);
|
|
new_rule = true;
|
|
}
|
|
|
|
if (rule->bcast_pol.sharindx == -1) {
|
|
NL_SET_ERR_MSG_MOD(extack, "No more L2 policers free");
|
|
rc = -ENOSPC;
|
|
goto out;
|
|
}
|
|
|
|
policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
|
|
|
|
if (policing[(SJA1105_NUM_PORTS * SJA1105_NUM_TC) + port].sharindx != port) {
|
|
NL_SET_ERR_MSG_MOD(extack,
|
|
"Port already has a broadcast policer");
|
|
rc = -EEXIST;
|
|
goto out;
|
|
}
|
|
|
|
rule->port_mask |= BIT(port);
|
|
|
|
/* Make the broadcast policers of all ports attached to this block
|
|
* point to the newly allocated policer
|
|
*/
|
|
for_each_set_bit(p, &rule->port_mask, SJA1105_NUM_PORTS) {
|
|
int bcast = (SJA1105_NUM_PORTS * SJA1105_NUM_TC) + p;
|
|
|
|
policing[bcast].sharindx = rule->bcast_pol.sharindx;
|
|
}
|
|
|
|
policing[rule->bcast_pol.sharindx].rate = div_u64(rate_bytes_per_sec *
|
|
512, 1000000);
|
|
policing[rule->bcast_pol.sharindx].smax = div_u64(rate_bytes_per_sec *
|
|
PSCHED_NS2TICKS(burst),
|
|
PSCHED_TICKS_PER_SEC);
|
|
/* TODO: support per-flow MTU */
|
|
policing[rule->bcast_pol.sharindx].maxlen = VLAN_ETH_FRAME_LEN +
|
|
ETH_FCS_LEN;
|
|
|
|
rc = sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
|
|
|
|
out:
|
|
if (rc == 0 && new_rule) {
|
|
priv->flow_block.l2_policer_used[rule->bcast_pol.sharindx] = true;
|
|
list_add(&rule->list, &priv->flow_block.rules);
|
|
} else if (new_rule) {
|
|
kfree(rule);
|
|
}
|
|
|
|
return rc;
|
|
}
|
|
|
|
static int sja1105_setup_tc_policer(struct sja1105_private *priv,
|
|
struct netlink_ext_ack *extack,
|
|
unsigned long cookie, int port, int tc,
|
|
u64 rate_bytes_per_sec,
|
|
s64 burst)
|
|
{
|
|
struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
|
|
struct sja1105_l2_policing_entry *policing;
|
|
bool new_rule = false;
|
|
unsigned long p;
|
|
int rc;
|
|
|
|
if (!rule) {
|
|
rule = kzalloc(sizeof(*rule), GFP_KERNEL);
|
|
if (!rule)
|
|
return -ENOMEM;
|
|
|
|
rule->cookie = cookie;
|
|
rule->type = SJA1105_RULE_TC_POLICER;
|
|
rule->tc_pol.sharindx = sja1105_find_free_l2_policer(priv);
|
|
rule->tc_pol.tc = tc;
|
|
new_rule = true;
|
|
}
|
|
|
|
if (rule->tc_pol.sharindx == -1) {
|
|
NL_SET_ERR_MSG_MOD(extack, "No more L2 policers free");
|
|
rc = -ENOSPC;
|
|
goto out;
|
|
}
|
|
|
|
policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
|
|
|
|
if (policing[(port * SJA1105_NUM_TC) + tc].sharindx != port) {
|
|
NL_SET_ERR_MSG_MOD(extack,
|
|
"Port-TC pair already has an L2 policer");
|
|
rc = -EEXIST;
|
|
goto out;
|
|
}
|
|
|
|
rule->port_mask |= BIT(port);
|
|
|
|
/* Make the policers for traffic class @tc of all ports attached to
|
|
* this block point to the newly allocated policer
|
|
*/
|
|
for_each_set_bit(p, &rule->port_mask, SJA1105_NUM_PORTS) {
|
|
int index = (p * SJA1105_NUM_TC) + tc;
|
|
|
|
policing[index].sharindx = rule->tc_pol.sharindx;
|
|
}
|
|
|
|
policing[rule->tc_pol.sharindx].rate = div_u64(rate_bytes_per_sec *
|
|
512, 1000000);
|
|
policing[rule->tc_pol.sharindx].smax = div_u64(rate_bytes_per_sec *
|
|
PSCHED_NS2TICKS(burst),
|
|
PSCHED_TICKS_PER_SEC);
|
|
/* TODO: support per-flow MTU */
|
|
policing[rule->tc_pol.sharindx].maxlen = VLAN_ETH_FRAME_LEN +
|
|
ETH_FCS_LEN;
|
|
|
|
rc = sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
|
|
|
|
out:
|
|
if (rc == 0 && new_rule) {
|
|
priv->flow_block.l2_policer_used[rule->tc_pol.sharindx] = true;
|
|
list_add(&rule->list, &priv->flow_block.rules);
|
|
} else if (new_rule) {
|
|
kfree(rule);
|
|
}
|
|
|
|
return rc;
|
|
}
|
|
|
|
static int sja1105_flower_parse_policer(struct sja1105_private *priv, int port,
|
|
struct netlink_ext_ack *extack,
|
|
struct flow_cls_offload *cls,
|
|
u64 rate_bytes_per_sec,
|
|
s64 burst)
|
|
{
|
|
struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
|
|
struct flow_dissector *dissector = rule->match.dissector;
|
|
|
|
if (dissector->used_keys &
|
|
~(BIT(FLOW_DISSECTOR_KEY_BASIC) |
|
|
BIT(FLOW_DISSECTOR_KEY_CONTROL) |
|
|
BIT(FLOW_DISSECTOR_KEY_VLAN) |
|
|
BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS))) {
|
|
NL_SET_ERR_MSG_MOD(extack,
|
|
"Unsupported keys used");
|
|
return -EOPNOTSUPP;
|
|
}
|
|
|
|
if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) {
|
|
struct flow_match_basic match;
|
|
|
|
flow_rule_match_basic(rule, &match);
|
|
if (match.key->n_proto) {
|
|
NL_SET_ERR_MSG_MOD(extack,
|
|
"Matching on protocol not supported");
|
|
return -EOPNOTSUPP;
|
|
}
|
|
}
|
|
|
|
if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
|
|
u8 bcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
|
u8 null[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
|
|
struct flow_match_eth_addrs match;
|
|
|
|
flow_rule_match_eth_addrs(rule, &match);
|
|
|
|
if (!ether_addr_equal_masked(match.key->src, null,
|
|
match.mask->src)) {
|
|
NL_SET_ERR_MSG_MOD(extack,
|
|
"Matching on source MAC not supported");
|
|
return -EOPNOTSUPP;
|
|
}
|
|
|
|
if (!ether_addr_equal_masked(match.key->dst, bcast,
|
|
match.mask->dst)) {
|
|
NL_SET_ERR_MSG_MOD(extack,
|
|
"Only matching on broadcast DMAC is supported");
|
|
return -EOPNOTSUPP;
|
|
}
|
|
|
|
return sja1105_setup_bcast_policer(priv, extack, cls->cookie,
|
|
port, rate_bytes_per_sec,
|
|
burst);
|
|
}
|
|
|
|
if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
|
|
struct flow_match_vlan match;
|
|
|
|
flow_rule_match_vlan(rule, &match);
|
|
|
|
if (match.key->vlan_id & match.mask->vlan_id) {
|
|
NL_SET_ERR_MSG_MOD(extack,
|
|
"Matching on VID is not supported");
|
|
return -EOPNOTSUPP;
|
|
}
|
|
|
|
if (match.mask->vlan_priority != 0x7) {
|
|
NL_SET_ERR_MSG_MOD(extack,
|
|
"Masked matching on PCP is not supported");
|
|
return -EOPNOTSUPP;
|
|
}
|
|
|
|
return sja1105_setup_tc_policer(priv, extack, cls->cookie, port,
|
|
match.key->vlan_priority,
|
|
rate_bytes_per_sec,
|
|
burst);
|
|
}
|
|
|
|
NL_SET_ERR_MSG_MOD(extack, "Not matching on any known key");
|
|
return -EOPNOTSUPP;
|
|
}
|
|
|
|
int sja1105_cls_flower_add(struct dsa_switch *ds, int port,
|
|
struct flow_cls_offload *cls, bool ingress)
|
|
{
|
|
struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
|
|
struct netlink_ext_ack *extack = cls->common.extack;
|
|
struct sja1105_private *priv = ds->priv;
|
|
const struct flow_action_entry *act;
|
|
int rc = -EOPNOTSUPP, i;
|
|
|
|
flow_action_for_each(i, act, &rule->action) {
|
|
switch (act->id) {
|
|
case FLOW_ACTION_POLICE:
|
|
rc = sja1105_flower_parse_policer(priv, port, extack, cls,
|
|
act->police.rate_bytes_ps,
|
|
act->police.burst);
|
|
break;
|
|
default:
|
|
NL_SET_ERR_MSG_MOD(extack,
|
|
"Action not supported");
|
|
break;
|
|
}
|
|
}
|
|
|
|
return rc;
|
|
}
|
|
|
|
int sja1105_cls_flower_del(struct dsa_switch *ds, int port,
|
|
struct flow_cls_offload *cls, bool ingress)
|
|
{
|
|
struct sja1105_private *priv = ds->priv;
|
|
struct sja1105_rule *rule = sja1105_rule_find(priv, cls->cookie);
|
|
struct sja1105_l2_policing_entry *policing;
|
|
int old_sharindx;
|
|
|
|
if (!rule)
|
|
return 0;
|
|
|
|
policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
|
|
|
|
if (rule->type == SJA1105_RULE_BCAST_POLICER) {
|
|
int bcast = (SJA1105_NUM_PORTS * SJA1105_NUM_TC) + port;
|
|
|
|
old_sharindx = policing[bcast].sharindx;
|
|
policing[bcast].sharindx = port;
|
|
} else if (rule->type == SJA1105_RULE_TC_POLICER) {
|
|
int index = (port * SJA1105_NUM_TC) + rule->tc_pol.tc;
|
|
|
|
old_sharindx = policing[index].sharindx;
|
|
policing[index].sharindx = port;
|
|
} else {
|
|
return -EINVAL;
|
|
}
|
|
|
|
rule->port_mask &= ~BIT(port);
|
|
if (!rule->port_mask) {
|
|
priv->flow_block.l2_policer_used[old_sharindx] = false;
|
|
list_del(&rule->list);
|
|
kfree(rule);
|
|
}
|
|
|
|
return sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
|
|
}
|
|
|
|
void sja1105_flower_setup(struct dsa_switch *ds)
|
|
{
|
|
struct sja1105_private *priv = ds->priv;
|
|
int port;
|
|
|
|
INIT_LIST_HEAD(&priv->flow_block.rules);
|
|
|
|
for (port = 0; port < SJA1105_NUM_PORTS; port++)
|
|
priv->flow_block.l2_policer_used[port] = true;
|
|
}
|
|
|
|
void sja1105_flower_teardown(struct dsa_switch *ds)
|
|
{
|
|
struct sja1105_private *priv = ds->priv;
|
|
struct sja1105_rule *rule;
|
|
struct list_head *pos, *n;
|
|
|
|
list_for_each_safe(pos, n, &priv->flow_block.rules) {
|
|
rule = list_entry(pos, struct sja1105_rule, list);
|
|
list_del(&rule->list);
|
|
kfree(rule);
|
|
}
|
|
}
|