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         flow_action_for_each(i, act, &rule->action) {
 321                 switch (act->id) {
 322                 case FLOW_ACTION_POLICE:
 323                         if (act->police.rate_pkt_ps) {
 324                                 NL_SET_ERR_MSG_MOD(extack,
 325                                                    "QoS offload not support packets per second");
 326                                 rc = -EOPNOTSUPP;
 327                                 goto out;
 328                         }
 329
 330                         rc = sja1105_flower_policer(priv, port, extack, cookie,
 331                                                     &key,
 332                                                     act->police.rate_bytes_ps,
 333                                                     act->police.burst);
 334                         if (rc)
 335                                 goto out;
 336                         break;
 337                 case FLOW_ACTION_TRAP: {
 338                         int cpu = dsa_upstream_port(ds, port);
 339
 340                         routing_rule = true;
 341                         vl_rule = true;
 342
 343                         rc = sja1105_vl_redirect(priv, port, extack, cookie,
 344                                                  &key, BIT(cpu), true);
 345                         if (rc)
 346                                 goto out;
 347                         break;
 348                 }
 349                 case FLOW_ACTION_REDIRECT: {
 350                         struct dsa_port *to_dp;
 351
 352                         to_dp = dsa_port_from_netdev(act->dev);
 353                         if (IS_ERR(to_dp)) {
 354                                 NL_SET_ERR_MSG_MOD(extack,
 355                                                    "Destination not a switch port");
 356                                 return -EOPNOTSUPP;
 357                         }
 358
 359                         routing_rule = true;
 360                         vl_rule = true;
 361
 362                         rc = sja1105_vl_redirect(priv, port, extack, cookie,
 363                                                  &key, BIT(to_dp->index), true);
 364                         if (rc)
 365                                 goto out;
 366                         break;
 367                 }
 368                 case FLOW_ACTION_DROP:
 369                         vl_rule = true;
 370
 371                         rc = sja1105_vl_redirect(priv, port, extack, cookie,
 372                                                  &key, 0, false);
 373                         if (rc)
 374                                 goto out;
 375                         break;
 376                 case FLOW_ACTION_GATE:
 377                         gate_rule = true;
 378                         vl_rule = true;
 379
 380                         rc = sja1105_vl_gate(priv, port, extack, cookie,
 381                                              &key, act->gate.index,
 382                                              act->gate.prio,
 383                                              act->gate.basetime,
 384                                              act->gate.cycletime,
 385                                              act->gate.cycletimeext,
 386                                              act->gate.num_entries,
 387                                              act->gate.entries);
 388                         if (rc)
 389                                 goto out;
 390                         break;
 391                 default:
 392                         NL_SET_ERR_MSG_MOD(extack,
 393                                            "Action not supported");
 394                         rc = -EOPNOTSUPP;
 395                         goto out;
 396                 }
 397         }
 398
 399         if (vl_rule && !rc) {
 400                 /* Delay scheduling configuration until DESTPORTS has been
 401                  * populated by all other actions.
 402                  */
 403                 if (gate_rule) {
 404                         if (!routing_rule) {
 405                                 NL_SET_ERR_MSG_MOD(extack,
 406                                                    "Can only offload gate action together with redirect or trap");
 407                                 return -EOPNOTSUPP;
 408                         }
 409                         rc = sja1105_init_scheduling(priv);
 410                         if (rc)
 411                                 goto out;
 412                 }
 413
 414                 rc = sja1105_static_config_reload(priv, SJA1105_VIRTUAL_LINKS);
 415         }
 416
 417 out:
 418         return rc;
 419 }
 420
 421 int sja1105_cls_flower_del(struct dsa_switch *ds, int port,
 422                            struct flow_cls_offload *cls, bool ingress)
 423 {
 424         struct sja1105_private *priv = ds->priv;
 425         struct sja1105_rule *rule = sja1105_rule_find(priv, cls->cookie);
 426         struct sja1105_l2_policing_entry *policing;
 427         int old_sharindx;
 428
 429         if (!rule)
 430                 return 0;
 431
 432         if (rule->type == SJA1105_RULE_VL)
 433                 return sja1105_vl_delete(priv, port, rule, cls->common.extack);
 434
 435         policing = priv->static_config.tables[BLK_IDX_L2_POLICING].entries;
 436
 437         if (rule->type == SJA1105_RULE_BCAST_POLICER) {
 438                 int bcast = (SJA1105_NUM_PORTS * SJA1105_NUM_TC) + port;
 439
 440                 old_sharindx = policing[bcast].sharindx;
 441                 policing[bcast].sharindx = port;
 442         } else if (rule->type == SJA1105_RULE_TC_POLICER) {
 443                 int index = (port * SJA1105_NUM_TC) + rule->key.tc.pcp;
 444
 445                 old_sharindx = policing[index].sharindx;
 446                 policing[index].sharindx = port;
 447         } else {
 448                 return -EINVAL;
 449         }
 450
 451         rule->port_mask &= ~BIT(port);
 452         if (!rule->port_mask) {
 453                 priv->flow_block.l2_policer_used[old_sharindx] = false;
 454                 list_del(&rule->list);
 455                 kfree(rule);
 456         }
 457
 458         return sja1105_static_config_reload(priv, SJA1105_BEST_EFFORT_POLICING);
 459 }
 460
 461 int sja1105_cls_flower_stats(struct dsa_switch *ds, int port,
 462                              struct flow_cls_offload *cls, bool ingress)
 463 {
 464         struct sja1105_private *priv = ds->priv;
 465         struct sja1105_rule *rule = sja1105_rule_find(priv, cls->cookie);
 466         int rc;
 467
 468         if (!rule)
 469                 return 0;
 470
 471         if (rule->type != SJA1105_RULE_VL)
 472                 return 0;
 473
 474         rc = sja1105_vl_stats(priv, port, rule, &cls->stats,
 475                               cls->common.extack);
 476         if (rc)
 477                 return rc;
 478
 479         return 0;
 480 }
 481
 482 void sja1105_flower_setup(struct dsa_switch *ds)
 483 {
 484         struct sja1105_private *priv = ds->priv;
 485         int port;
 486
 487         INIT_LIST_HEAD(&priv->flow_block.rules);
 488
 489         for (port = 0; port < SJA1105_NUM_PORTS; port++)
 490                 priv->flow_block.l2_policer_used[port] = true;
 491 }
 492
 493 void sja1105_flower_teardown(struct dsa_switch *ds)
 494 {
 495         struct sja1105_private *priv = ds->priv;
 496         struct sja1105_rule *rule;
 497         struct list_head *pos, *n;
 498
 499         list_for_each_safe(pos, n, &priv->flow_block.rules) {
 500                 rule = list_entry(pos, struct sja1105_rule, list);
 501                 list_del(&rule->list);
 502                 kfree(rule);
 503         }
 504 }