drivers/net/dsa/sja1105/sja1105_flower.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright 2020, NXP Semiconductors
   3  */
   4 #include "sja1105.h"
   5 #include "sja1105_vl.h"
   6
   7 struct sja1105_rule *sja1105_rule_find(struct sja1105_private *priv,
   8                                        unsigned long cookie)
   9 {
  10         struct sja1105_rule *rule;
  11
  12         list_for_each_entry(rule, &priv->flow_block.rules, list)
  13                 if (rule->cookie == cookie)
  14                         return rule;
  15
  16         return NULL;
  17 }
  18
  19 static int sja1105_find_free_l2_policer(struct sja1105_private *priv)
  20 {
  21         int i;
  22
  23         for (i = 0; i < SJA1105_NUM_L2_POLICERS; i++)
  24                 if (!priv->flow_block.l2_policer_used[i])
  25                         return i;
  26
  27         return -1;
  28 }
  29
  30 static int sja1105_setup_bcast_policer(struct sja1105_private *priv,
  31                                        struct netlink_ext_ack *extack,
  32                                        unsigned long cookie, int port,
  33                                        u64 rate_bytes_per_sec,
  34                                        u32 burst)
  35 {
  36         struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
  37         struct sja1105_l2_policing_entry *policing;
  38         bool new_rule = false;
  39         unsigned long p;
  40         int rc;
  41
  42         if (!rule) {
  43                 rule = kzalloc(sizeof(*rule), GFP_KERNEL);
  44                 if (!rule)
  45                         return -ENOMEM;
  46
  47                 rule->cookie = cookie;
  48                 rule->type = SJA1105_RULE_BCAST_POLICER;
  49                 rule->bcast_pol.sharindx = sja1105_find_free_l2_policer(priv);
  50                 rule->key.type = SJA1105_KEY_BCAST;
  51                 new_rule = true;
  52         }
  53
  54         if (rule->bcast_pol.sharindx == -1) {
  55                 NL_SET_ERR_MSG_MOD(extack, "No more L2 policers free");
  56                 rc = -ENOSPC;
  57                 goto out;
  58         }
  59
  60         policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
  61
  62         if (policing[(SJA1105_NUM_PORTS * SJA1105_NUM_TC) + port].sharindx != port) {
  63                 NL_SET_ERR_MSG_MOD(extack,
  64                                    "Port already has a broadcast policer");
  65                 rc = -EEXIST;
  66                 goto out;
  67         }
  68
  69         rule->port_mask |= BIT(port);
  70
  71         /* Make the broadcast policers of all ports attached to this block
  72          * point to the newly allocated policer
  73          */
  74         for_each_set_bit(p, &rule->port_mask, SJA1105_NUM_PORTS) {
  75                 int bcast = (SJA1105_NUM_PORTS * SJA1105_NUM_TC) + p;
  76
  77                 policing[bcast].sharindx = rule->bcast_pol.sharindx;
  78         }
  79
  80         policing[rule->bcast_pol.sharindx].rate = div_u64(rate_bytes_per_sec *
  81                                                           512, 1000000);
  82         policing[rule->bcast_pol.sharindx].smax = burst;
  83
  84         /* TODO: support per-flow MTU */
  85         policing[rule->bcast_pol.sharindx].maxlen = VLAN_ETH_FRAME_LEN +
  86                                                     ETH_FCS_LEN;
  87
  88         rc = sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
  89
  90 out:
  91         if (rc == 0 && new_rule) {
  92                 priv->flow_block.l2_policer_used[rule->bcast_pol.sharindx] = true;
  93                 list_add(&rule->list, &priv->flow_block.rules);
  94         } else if (new_rule) {
  95                 kfree(rule);
  96         }
  97
  98         return rc;
  99 }
 100
 101 static int sja1105_setup_tc_policer(struct sja1105_private *priv,
 102                                     struct netlink_ext_ack *extack,
 103                                     unsigned long cookie, int port, int tc,
 104                                     u64 rate_bytes_per_sec,
 105                                     u32 burst)
 106 {
 107         struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
 108         struct sja1105_l2_policing_entry *policing;
 109         bool new_rule = false;
 110         unsigned long p;
 111         int rc;
 112
 113         if (!rule) {
 114                 rule = kzalloc(sizeof(*rule), GFP_KERNEL);
 115                 if (!rule)
 116                         return -ENOMEM;
 117
 118                 rule->cookie = cookie;
 119                 rule->type = SJA1105_RULE_TC_POLICER;
 120                 rule->tc_pol.sharindx = sja1105_find_free_l2_policer(priv);
 121                 rule->key.type = SJA1105_KEY_TC;
 122                 rule->key.tc.pcp = tc;
 123                 new_rule = true;
 124         }
 125
 126         if (rule->tc_pol.sharindx == -1) {
 127                 NL_SET_ERR_MSG_MOD(extack, "No more L2 policers free");
 128                 rc = -ENOSPC;
 129                 goto out;
 130         }
 131
 132         policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
 133
 134         if (policing[(port * SJA1105_NUM_TC) + tc].sharindx != port) {
 135                 NL_SET_ERR_MSG_MOD(extack,
 136                                    "Port-TC pair already has an L2 policer");
 137                 rc = -EEXIST;
 138                 goto out;
 139         }
 140
 141         rule->port_mask |= BIT(port);
 142
 143         /* Make the policers for traffic class @tc of all ports attached to
 144          * this block point to the newly allocated policer
 145          */
 146         for_each_set_bit(p, &rule->port_mask, SJA1105_NUM_PORTS) {
 147                 int index = (p * SJA1105_NUM_TC) + tc;
 148
 149                 policing[index].sharindx = rule->tc_pol.sharindx;
 150         }
 151
 152         policing[rule->tc_pol.sharindx].rate = div_u64(rate_bytes_per_sec *
 153                                                        512, 1000000);
 154         policing[rule->tc_pol.sharindx].smax = burst;
 155
 156         /* TODO: support per-flow MTU */
 157         policing[rule->tc_pol.sharindx].maxlen = VLAN_ETH_FRAME_LEN +
 158                                                  ETH_FCS_LEN;
 159
 160         rc = sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
 161
 162 out:
 163         if (rc == 0 && new_rule) {
 164                 priv->flow_block.l2_policer_used[rule->tc_pol.sharindx] = true;
 165                 list_add(&rule->list, &priv->flow_block.rules);
 166         } else if (new_rule) {
 167                 kfree(rule);
 168         }
 169
 170         return rc;
 171 }
 172
 173 static int sja1105_flower_policer(struct sja1105_private *priv, int port,
 174                                   struct netlink_ext_ack *extack,
 175                                   unsigned long cookie,
 176                                   struct sja1105_key *key,
 177                                   u64 rate_bytes_per_sec,
 178                                   u32 burst)
 179 {
 180         switch (key->type) {
 181         case SJA1105_KEY_BCAST:
 182                 return sja1105_setup_bcast_policer(priv, extack, cookie, port,
 183                                                    rate_bytes_per_sec, burst);
 184         case SJA1105_KEY_TC:
 185                 return sja1105_setup_tc_policer(priv, extack, cookie, port,
 186                                                 key->tc.pcp, rate_bytes_per_sec,
 187                                                 burst);
 188         default:
 189                 NL_SET_ERR_MSG_MOD(extack, "Unknown keys for policing");
 190                 return -EOPNOTSUPP;
 191         }
 192 }
 193
 194 static int sja1105_flower_parse_key(struct sja1105_private *priv,
 195                                     struct netlink_ext_ack *extack,
 196                                     struct flow_cls_offload *cls,
 197                                     struct sja1105_key *key)
 198 {
 199         struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 200         struct flow_dissector *dissector = rule->match.dissector;
 201         bool is_bcast_dmac = false;
 202         u64 dmac = U64_MAX;
 203         u16 vid = U16_MAX;
 204         u16 pcp = U16_MAX;
 205
 206         if (dissector->used_keys &
 207             ~(BIT(FLOW_DISSECTOR_KEY_BASIC) |
 208               BIT(FLOW_DISSECTOR_KEY_CONTROL) |
 209               BIT(FLOW_DISSECTOR_KEY_VLAN) |
 210               BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS))) {
 211                 NL_SET_ERR_MSG_MOD(extack,
 212                                    "Unsupported keys used");
 213                 return -EOPNOTSUPP;
 214         }
 215
 216         if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_BASIC)) {
 217                 struct flow_match_basic match;
 218
 219                 flow_rule_match_basic(rule, &match);
 220                 if (match.key->n_proto) {
 221                         NL_SET_ERR_MSG_MOD(extack,
 222                                            "Matching on protocol not supported");
 223                         return -EOPNOTSUPP;
 224                 }
 225         }
 226
 227         if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
 228                 u8 bcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 229                 u8 null[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 230                 struct flow_match_eth_addrs match;
 231
 232                 flow_rule_match_eth_addrs(rule, &match);
 233
 234                 if (!ether_addr_equal_masked(match.key->src, null,
 235                                              match.mask->src)) {
 236                         NL_SET_ERR_MSG_MOD(extack,
 237                                            "Matching on source MAC not supported");
 238                         return -EOPNOTSUPP;
 239                 }
 240
 241                 if (!ether_addr_equal(match.mask->dst, bcast)) {
 242                         NL_SET_ERR_MSG_MOD(extack,
 243                                            "Masked matching on MAC not supported");
 244                         return -EOPNOTSUPP;
 245                 }
 246
 247                 dmac = ether_addr_to_u64(match.key->dst);
 248                 is_bcast_dmac = ether_addr_equal(match.key->dst, bcast);
 249         }
 250
 251         if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
 252                 struct flow_match_vlan match;
 253
 254                 flow_rule_match_vlan(rule, &match);
 255
 256                 if (match.mask->vlan_id &&
 257                     match.mask->vlan_id != VLAN_VID_MASK) {
 258                         NL_SET_ERR_MSG_MOD(extack,
 259                                            "Masked matching on VID is not supported");
 260                         return -EOPNOTSUPP;
 261                 }
 262
 263                 if (match.mask->vlan_priority &&
 264                     match.mask->vlan_priority != 0x7) {
 265                         NL_SET_ERR_MSG_MOD(extack,
 266                                            "Masked matching on PCP is not supported");
 267                         return -EOPNOTSUPP;
 268                 }
 269
 270                 if (match.mask->vlan_id)
 271                         vid = match.key->vlan_id;
 272                 if (match.mask->vlan_priority)
 273                         pcp = match.key->vlan_priority;
 274         }
 275
 276         if (is_bcast_dmac && vid == U16_MAX && pcp == U16_MAX) {
 277                 key->type = SJA1105_KEY_BCAST;
 278                 return 0;
 279         }
 280         if (dmac == U64_MAX && vid == U16_MAX && pcp != U16_MAX) {
 281                 key->type = SJA1105_KEY_TC;
 282                 key->tc.pcp = pcp;
 283                 return 0;
 284         }
 285         if (dmac != U64_MAX && vid != U16_MAX && pcp != U16_MAX) {
 286                 key->type = SJA1105_KEY_VLAN_AWARE_VL;
 287                 key->vl.dmac = dmac;
 288                 key->vl.vid = vid;
 289                 key->vl.pcp = pcp;
 290                 return 0;
 291         }
 292         if (dmac != U64_MAX) {
 293                 key->type = SJA1105_KEY_VLAN_UNAWARE_VL;
 294                 key->vl.dmac = dmac;
 295                 return 0;
 296         }
 297
 298         NL_SET_ERR_MSG_MOD(extack, "Not matching on any known key");
 299         return -EOPNOTSUPP;
 300 }
 301
 302 int sja1105_cls_flower_add(struct dsa_switch *ds, int port,
 303                            struct flow_cls_offload *cls, bool ingress)
 304 {
 305         struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 306         struct netlink_ext_ack *extack = cls->common.extack;
 307         struct sja1105_private *priv = ds->priv;
 308         const struct flow_action_entry *act;
 309         unsigned long cookie = cls->cookie;
 310         bool routing_rule = false;
 311         struct sja1105_key key;
 312         bool gate_rule = false;
 313         bool vl_rule = false;
 314         int rc, i;
 315
 316         rc = sja1105_flower_parse_key(priv, extack, cls, &key);
 317         if (rc)
 318                 return rc;
 319
 320         rc = -EOPNOTSUPP;
 321
 322         flow_action_for_each(i, act, &rule->action) {
 323                 switch (act->id) {
 324                 case FLOW_ACTION_POLICE:
 325                         rc = sja1105_flower_policer(priv, port, extack, cookie,
 326                                                     &key,
 327                                                     act->police.rate_bytes_ps,
 328                                                     act->police.burst);
 329                         if (rc)
 330                                 goto out;
 331                         break;
 332                 case FLOW_ACTION_TRAP: {
 333                         int cpu = dsa_upstream_port(ds, port);
 334
 335                         routing_rule = true;
 336                         vl_rule = true;
 337
 338                         rc = sja1105_vl_redirect(priv, port, extack, cookie,
 339                                                  &key, BIT(cpu), true);
 340                         if (rc)
 341                                 goto out;
 342                         break;
 343                 }
 344                 case FLOW_ACTION_REDIRECT: {
 345                         struct dsa_port *to_dp;
 346
 347                         to_dp = dsa_port_from_netdev(act->dev);
 348                         if (IS_ERR(to_dp)) {
 349                                 NL_SET_ERR_MSG_MOD(extack,
 350                                                    "Destination not a switch port");
 351                                 return -EOPNOTSUPP;
 352                         }
 353
 354                         routing_rule = true;
 355                         vl_rule = true;
 356
 357                         rc = sja1105_vl_redirect(priv, port, extack, cookie,
 358                                                  &key, BIT(to_dp->index), true);
 359                         if (rc)
 360                                 goto out;
 361                         break;
 362                 }
 363                 case FLOW_ACTION_DROP:
 364                         vl_rule = true;
 365
 366                         rc = sja1105_vl_redirect(priv, port, extack, cookie,
 367                                                  &key, 0, false);
 368                         if (rc)
 369                                 goto out;
 370                         break;
 371                 case FLOW_ACTION_GATE:
 372                         gate_rule = true;
 373                         vl_rule = true;
 374
 375                         rc = sja1105_vl_gate(priv, port, extack, cookie,
 376                                              &key, act->gate.index,
 377                                              act->gate.prio,
 378                                              act->gate.basetime,
 379                                              act->gate.cycletime,
 380                                              act->gate.cycletimeext,
 381                                              act->gate.num_entries,
 382                                              act->gate.entries);
 383                         if (rc)
 384                                 goto out;
 385                         break;
 386                 default:
 387                         NL_SET_ERR_MSG_MOD(extack,
 388                                            "Action not supported");
 389                         rc = -EOPNOTSUPP;
 390                         goto out;
 391                 }
 392         }
 393
 394         if (vl_rule && !rc) {
 395                 /* Delay scheduling configuration until DESTPORTS has been
 396                  * populated by all other actions.
 397                  */
 398                 if (gate_rule) {
 399                         if (!routing_rule) {
 400                                 NL_SET_ERR_MSG_MOD(extack,
 401                                                    "Can only offload gate action together with redirect or trap");
 402                                 return -EOPNOTSUPP;
 403                         }
 404                         rc = sja1105_init_scheduling(priv);
 405                         if (rc)
 406                                 goto out;
 407                 }
 408
 409                 rc = sja1105_static_config_reload(priv, SJA1105_VIRTUAL_LINKS);
 410         }
 411
 412 out:
 413         return rc;
 414 }
 415
 416 int sja1105_cls_flower_del(struct dsa_switch *ds, int port,
 417                            struct flow_cls_offload *cls, bool ingress)
 418 {
 419         struct sja1105_private *priv = ds->priv;
 420         struct sja1105_rule *rule = sja1105_rule_find(priv, cls->cookie);
 421         struct sja1105_l2_policing_entry *policing;
 422         int old_sharindx;
 423
 424         if (!rule)
 425                 return 0;
 426
 427         if (rule->type == SJA1105_RULE_VL)
 428                 return sja1105_vl_delete(priv, port, rule, cls->common.extack);
 429
 430         policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
 431
 432         if (rule->type == SJA1105_RULE_BCAST_POLICER) {
 433                 int bcast = (SJA1105_NUM_PORTS * SJA1105_NUM_TC) + port;
 434
 435                 old_sharindx = policing[bcast].sharindx;
 436                 policing[bcast].sharindx = port;
 437         } else if (rule->type == SJA1105_RULE_TC_POLICER) {
 438                 int index = (port * SJA1105_NUM_TC) + rule->key.tc.pcp;
 439
 440                 old_sharindx = policing[index].sharindx;
 441                 policing[index].sharindx = port;
 442         } else {
 443                 return -EINVAL;
 444         }
 445
 446         rule->port_mask &= ~BIT(port);
 447         if (!rule->port_mask) {
 448                 priv->flow_block.l2_policer_used[old_sharindx] = false;
 449                 list_del(&rule->list);
 450                 kfree(rule);
 451         }
 452
 453         return sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
 454 }
 455
 456 int sja1105_cls_flower_stats(struct dsa_switch *ds, int port,
 457                              struct flow_cls_offload *cls, bool ingress)
 458 {
 459         struct sja1105_private *priv = ds->priv;
 460         struct sja1105_rule *rule = sja1105_rule_find(priv, cls->cookie);
 461         int rc;
 462
 463         if (!rule)
 464                 return 0;
 465
 466         if (rule->type != SJA1105_RULE_VL)
 467                 return 0;
 468
 469         rc = sja1105_vl_stats(priv, port, rule, &cls->stats,
 470                               cls->common.extack);
 471         if (rc)
 472                 return rc;
 473
 474         return 0;
 475 }
 476
 477 void sja1105_flower_setup(struct dsa_switch *ds)
 478 {
 479         struct sja1105_private *priv = ds->priv;
 480         int port;
 481
 482         INIT_LIST_HEAD(&priv->flow_block.rules);
 483
 484         for (port = 0; port < SJA1105_NUM_PORTS; port++)
 485                 priv->flow_block.l2_policer_used[port] = true;
 486 }
 487
 488 void sja1105_flower_teardown(struct dsa_switch *ds)
 489 {
 490         struct sja1105_private *priv = ds->priv;
 491         struct sja1105_rule *rule;
 492         struct list_head *pos, *n;
 493
 494         list_for_each_safe(pos, n, &priv->flow_block.rules) {
 495                 rule = list_entry(pos, struct sja1105_rule, list);
 496                 list_del(&rule->list);
 497                 kfree(rule);
 498         }
 499 }