Quelle  sja1105_flower.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0
/* Copyright 2020 NXP
 */
#include "sja1105.h"
#include "sja1105_vl.h"

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,
           u32 burst)
{
 struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
 struct sja1105_l2_policing_entry *policing;
 struct dsa_switch *ds = priv->ds;
 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);
  rule->key.type = SJA1105_KEY_BCAST;
  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[(ds->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_MAX_NUM_PORTS) {
  int bcast = (ds->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 = burst;

 /* 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,
        u32 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->key.type = SJA1105_KEY_TC;
  rule->key.tc.pcp = 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_MAX_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 = burst;

 /* 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_policer(struct sja1105_private *priv, int port,
      struct netlink_ext_ack *extack,
      unsigned long cookie,
      struct sja1105_key *key,
      u64 rate_bytes_per_sec,
      u32 burst)
{
 switch (key->type) {
 case SJA1105_KEY_BCAST:
  return sja1105_setup_bcast_policer(priv, extack, cookie, port,
         rate_bytes_per_sec, burst);
 case SJA1105_KEY_TC:
  return sja1105_setup_tc_policer(priv, extack, cookie, port,
      key->tc.pcp, rate_bytes_per_sec,
      burst);
 default:
  NL_SET_ERR_MSG_MOD(extack, "Unknown keys for policing");
  return -EOPNOTSUPP;
 }
}

static int sja1105_flower_parse_key(struct sja1105_private *priv,
        struct netlink_ext_ack *extack,
        struct flow_cls_offload *cls,
        struct sja1105_key *key)
{
 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 struct flow_dissector *dissector = rule->match.dissector;
 bool is_bcast_dmac = false;
 u64 dmac = U64_MAX;
 u16 vid = U16_MAX;
 u16 pcp = U16_MAX;

 if (dissector->used_keys &
     ~(BIT_ULL(FLOW_DISSECTOR_KEY_BASIC) |
       BIT_ULL(FLOW_DISSECTOR_KEY_CONTROL) |
       BIT_ULL(FLOW_DISSECTOR_KEY_VLAN) |
       BIT_ULL(FLOW_DISSECTOR_KEY_ETH_ADDRS))) {
  NL_SET_ERR_MSG_MOD(extack,
       "Unsupported keys used");
  return -EOPNOTSUPP;
 }

 if (flow_rule_match_has_control_flags(rule, extack))
  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(match.mask->dst, bcast)) {
   NL_SET_ERR_MSG_MOD(extack,
        "Masked matching on MAC not supported");
   return -EOPNOTSUPP;
  }

  dmac = ether_addr_to_u64(match.key->dst);
  is_bcast_dmac = ether_addr_equal(match.key->dst, bcast);
 }

 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
  struct flow_match_vlan match;

  flow_rule_match_vlan(rule, &match);

  if (match.mask->vlan_id &&
      match.mask->vlan_id != VLAN_VID_MASK) {
   NL_SET_ERR_MSG_MOD(extack,
        "Masked matching on VID is not supported");
   return -EOPNOTSUPP;
  }

  if (match.mask->vlan_priority &&
      match.mask->vlan_priority != 0x7) {
   NL_SET_ERR_MSG_MOD(extack,
        "Masked matching on PCP is not supported");
   return -EOPNOTSUPP;
  }

  if (match.mask->vlan_id)
   vid = match.key->vlan_id;
  if (match.mask->vlan_priority)
   pcp = match.key->vlan_priority;
 }

 if (is_bcast_dmac && vid == U16_MAX && pcp == U16_MAX) {
  key->type = SJA1105_KEY_BCAST;
  return 0;
 }
 if (dmac == U64_MAX && vid == U16_MAX && pcp != U16_MAX) {
  key->type = SJA1105_KEY_TC;
  key->tc.pcp = pcp;
  return 0;
 }
 if (dmac != U64_MAX && vid != U16_MAX && pcp != U16_MAX) {
  key->type = SJA1105_KEY_VLAN_AWARE_VL;
  key->vl.dmac = dmac;
  key->vl.vid = vid;
  key->vl.pcp = pcp;
  return 0;
 }
 if (dmac != U64_MAX) {
  key->type = SJA1105_KEY_VLAN_UNAWARE_VL;
  key->vl.dmac = dmac;
  return 0;
 }

 NL_SET_ERR_MSG_MOD(extack, "Not matching on any known key");
 return -EOPNOTSUPP;
}

static int sja1105_policer_validate(const struct flow_action *action,
        const struct flow_action_entry *act,
        struct netlink_ext_ack *extack)
{
 if (act->police.exceed.act_id != FLOW_ACTION_DROP) {
  NL_SET_ERR_MSG_MOD(extack,
       "Offload not supported when exceed action is not drop");
  return -EOPNOTSUPP;
 }

 if (act->police.notexceed.act_id != FLOW_ACTION_PIPE &&
     act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) {
  NL_SET_ERR_MSG_MOD(extack,
       "Offload not supported when conform action is not pipe or ok");
  return -EOPNOTSUPP;
 }

 if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT &&
     !flow_action_is_last_entry(action, act)) {
  NL_SET_ERR_MSG_MOD(extack,
       "Offload not supported when conform action is ok, but action is not last");
  return -EOPNOTSUPP;
 }

 if (act->police.peakrate_bytes_ps ||
     act->police.avrate || act->police.overhead) {
  NL_SET_ERR_MSG_MOD(extack,
       "Offload not supported when peakrate/avrate/overhead is configured");
  return -EOPNOTSUPP;
 }

 if (act->police.rate_pkt_ps) {
  NL_SET_ERR_MSG_MOD(extack,
       "QoS offload not support packets per second");
  return -EOPNOTSUPP;
 }

 return 0;
}

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;
 unsigned long cookie = cls->cookie;
 bool routing_rule = false;
 struct sja1105_key key;
 bool gate_rule = false;
 bool vl_rule = false;
 int rc, i;

 rc = sja1105_flower_parse_key(priv, extack, cls, &key);
 if (rc)
  return rc;

 flow_action_for_each(i, act, &rule->action) {
  switch (act->id) {
  case FLOW_ACTION_POLICE:
   rc = sja1105_policer_validate(&rule->action, act, extack);
   if (rc)
    goto out;

   rc = sja1105_flower_policer(priv, port, extack, cookie,
          &key,
          act->police.rate_bytes_ps,
          act->police.burst);
   if (rc)
    goto out;
   break;
  case FLOW_ACTION_TRAP: {
   int cpu = dsa_upstream_port(ds, port);

   routing_rule = true;
   vl_rule = true;

   rc = sja1105_vl_redirect(priv, port, extack, cookie,
       &key, BIT(cpu), true);
   if (rc)
    goto out;
   break;
  }
  case FLOW_ACTION_REDIRECT: {
   struct dsa_port *to_dp;

   to_dp = dsa_port_from_netdev(act->dev);
   if (IS_ERR(to_dp)) {
    NL_SET_ERR_MSG_MOD(extack,
         "Destination not a switch port");
    return -EOPNOTSUPP;
   }

   routing_rule = true;
   vl_rule = true;

   rc = sja1105_vl_redirect(priv, port, extack, cookie,
       &key, BIT(to_dp->index), true);
   if (rc)
    goto out;
   break;
  }
  case FLOW_ACTION_DROP:
   vl_rule = true;

   rc = sja1105_vl_redirect(priv, port, extack, cookie,
       &key, 0, false);
   if (rc)
    goto out;
   break;
  case FLOW_ACTION_GATE:
   gate_rule = true;
   vl_rule = true;

   rc = sja1105_vl_gate(priv, port, extack, cookie,
          &key, act->hw_index,
          act->gate.prio,
          act->gate.basetime,
          act->gate.cycletime,
          act->gate.cycletimeext,
          act->gate.num_entries,
          act->gate.entries);
   if (rc)
    goto out;
   break;
  default:
   NL_SET_ERR_MSG_MOD(extack,
        "Action not supported");
   rc = -EOPNOTSUPP;
   goto out;
  }
 }

 if (vl_rule && !rc) {
  /* Delay scheduling configuration until DESTPORTS has been
 * populated by all other actions.
 */
  if (gate_rule) {
   if (!routing_rule) {
    NL_SET_ERR_MSG_MOD(extack,
         "Can only offload gate action together with redirect or trap");
    return -EOPNOTSUPP;
   }
   rc = sja1105_init_scheduling(priv);
   if (rc)
    goto out;
  }

  rc = sja1105_static_config_reload(priv, SJA1105_VIRTUAL_LINKS);
 }

out:
 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;

 if (rule->type == SJA1105_RULE_VL)
  return sja1105_vl_delete(priv, port, rule, cls->common.extack);

 policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;

 if (rule->type == SJA1105_RULE_BCAST_POLICER) {
  int bcast = (ds->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->key.tc.pcp;

  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);
}

int sja1105_cls_flower_stats(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);
 int rc;

 if (!rule)
  return 0;

 if (rule->type != SJA1105_RULE_VL)
  return 0;

 rc = sja1105_vl_stats(priv, port, rule, &cls->stats,
         cls->common.extack);
 if (rc)
  return rc;

 return 0;
}

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 < ds->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);
 }
}