drivers/net/dsa/sja1105/sja1105_vl.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright 2020, NXP Semiconductors
   3  */
   4 #include <net/tc_act/tc_gate.h>
   5 #include <linux/dsa/8021q.h>
   6 #include "sja1105_vl.h"
   7
   8 #define SJA1105_SIZE_VL_STATUS                  8
   9
  10 /* The switch flow classification core implements TTEthernet, which 'thinks' in
  11  * terms of Virtual Links (VL), a concept borrowed from ARINC 664 part 7.
  12  * However it also has one other operating mode (VLLUPFORMAT=0) where it acts
  13  * somewhat closer to a pre-standard implementation of IEEE 802.1Qci
  14  * (Per-Stream Filtering and Policing), which is what the driver is going to be
  15  * implementing.
  16  *
  17  *                                 VL Lookup
  18  *        Key = {DMAC && VLANID   +---------+  Key = { (DMAC[47:16] & VLMASK ==
  19  *               && VLAN PCP      |         |                         VLMARKER)
  20  *               && INGRESS PORT} +---------+                      (both fixed)
  21  *            (exact match,            |             && DMAC[15:0] == VLID
  22  *         all specified in rule)      |                    (specified in rule)
  23  *                                     v             && INGRESS PORT }
  24  *                               ------------
  25  *                    0 (PSFP)  /            \  1 (ARINC664)
  26  *                 +-----------/  VLLUPFORMAT \----------+
  27  *                 |           \    (fixed)   /          |
  28  *                 |            \            /           |
  29  *  0 (forwarding) v             ------------            |
  30  *           ------------                                |
  31  *          /            \  1 (QoS classification)       |
  32  *     +---/  ISCRITICAL  \-----------+                  |
  33  *     |   \  (per rule)  /           |                  |
  34  *     |    \            /   VLID taken from      VLID taken from
  35  *     v     ------------     index of rule       contents of rule
  36  *  select                     that matched         that matched
  37  * DESTPORTS                          |                  |
  38  *  |                                 +---------+--------+
  39  *  |                                           |
  40  *  |                                           v
  41  *  |                                     VL Forwarding
  42  *  |                                   (indexed by VLID)
  43  *  |                                      +---------+
  44  *  |                       +--------------|         |
  45  *  |                       |  select TYPE +---------+
  46  *  |                       v
  47  *  |   0 (rate      ------------    1 (time
  48  *  |  constrained) /            \   triggered)
  49  *  |       +------/     TYPE     \------------+
  50  *  |       |      \  (per VLID)  /            |
  51  *  |       v       \            /             v
  52  *  |  VL Policing   ------------         VL Policing
  53  *  | (indexed by VLID)                (indexed by VLID)
  54  *  |  +---------+                        +---------+
  55  *  |  | TYPE=0  |                        | TYPE=1  |
  56  *  |  +---------+                        +---------+
  57  *  |  select SHARINDX                 select SHARINDX to
  58  *  |  to rate-limit                 re-enter VL Forwarding
  59  *  |  groups of VL's               with new VLID for egress
  60  *  |  to same quota                           |
  61  *  |       |                                  |
  62  *  |  select MAXLEN -> exceed => drop    select MAXLEN -> exceed => drop
  63  *  |       |                                  |
  64  *  |       v                                  v
  65  *  |  VL Forwarding                      VL Forwarding
  66  *  | (indexed by SHARINDX)             (indexed by SHARINDX)
  67  *  |  +---------+                        +---------+
  68  *  |  | TYPE=0  |                        | TYPE=1  |
  69  *  |  +---------+                        +---------+
  70  *  |  select PRIORITY,                 select PRIORITY,
  71  *  | PARTITION, DESTPORTS            PARTITION, DESTPORTS
  72  *  |       |                                  |
  73  *  |       v                                  v
  74  *  |  VL Policing                        VL Policing
  75  *  | (indexed by SHARINDX)           (indexed by SHARINDX)
  76  *  |  +---------+                        +---------+
  77  *  |  | TYPE=0  |                        | TYPE=1  |
  78  *  |  +---------+                        +---------+
  79  *  |       |                                  |
  80  *  |       v                                  |
  81  *  |  select BAG, -> exceed => drop           |
  82  *  |    JITTER                                v
  83  *  |       |             ----------------------------------------------
  84  *  |       |            /    Reception Window is open for this VL      \
  85  *  |       |           /    (the Schedule Table executes an entry i     \
  86  *  |       |          /   M <= i < N, for which these conditions hold):  \ no
  87  *  |       |    +----/                                                    \-+
  88  *  |       |    |yes \       WINST[M] == 1 && WINSTINDEX[M] == VLID       / |
  89  *  |       |    |     \     WINEND[N] == 1 && WINSTINDEX[N] == VLID      /  |
  90  *  |       |    |      \                                                /   |
  91  *  |       |    |       \ (the VL window has opened and not yet closed)/    |
  92  *  |       |    |        ----------------------------------------------     |
  93  *  |       |    v                                                           v
  94  *  |       |  dispatch to DESTPORTS when the Schedule Table               drop
  95  *  |       |  executes an entry i with TXEN == 1 && VLINDEX == i
  96  *  v       v
  97  * dispatch immediately to DESTPORTS
  98  *
  99  * The per-port classification key is always composed of {DMAC, VID, PCP} and
 100  * is non-maskable. This 'looks like' the NULL stream identification function
 101  * from IEEE 802.1CB clause 6, except for the extra VLAN PCP. When the switch
 102  * ports operate as VLAN-unaware, we do allow the user to not specify the VLAN
 103  * ID and PCP, and then the port-based defaults will be used.
 104  *
 105  * In TTEthernet, routing is something that needs to be done manually for each
 106  * Virtual Link. So the flow action must always include one of:
 107  * a. 'redirect', 'trap' or 'drop': select the egress port list
 108  * Additionally, the following actions may be applied on a Virtual Link,
 109  * turning it into 'critical' traffic:
 110  * b. 'police': turn it into a rate-constrained VL, with bandwidth limitation
 111  *    given by the maximum frame length, bandwidth allocation gap (BAG) and
 112  *    maximum jitter.
 113  * c. 'gate': turn it into a time-triggered VL, which can be only be received
 114  *    and forwarded according to a given schedule.
 115  */
 116
 117 static bool sja1105_vl_key_lower(struct sja1105_vl_lookup_entry *a,
 118                                  struct sja1105_vl_lookup_entry *b)
 119 {
 120         if (a->macaddr < b->macaddr)
 121                 return true;
 122         if (a->macaddr > b->macaddr)
 123                 return false;
 124         if (a->vlanid < b->vlanid)
 125                 return true;
 126         if (a->vlanid > b->vlanid)
 127                 return false;
 128         if (a->port < b->port)
 129                 return true;
 130         if (a->port > b->port)
 131                 return false;
 132         if (a->vlanprior < b->vlanprior)
 133                 return true;
 134         if (a->vlanprior > b->vlanprior)
 135                 return false;
 136         /* Keys are equal */
 137         return false;
 138 }
 139
 140 static int sja1105_init_virtual_links(struct sja1105_private *priv,
 141                                       struct netlink_ext_ack *extack)
 142 {
 143         struct sja1105_vl_policing_entry *vl_policing;
 144         struct sja1105_vl_forwarding_entry *vl_fwd;
 145         struct sja1105_vl_lookup_entry *vl_lookup;
 146         bool have_critical_virtual_links = false;
 147         struct sja1105_table *table;
 148         struct sja1105_rule *rule;
 149         int num_virtual_links = 0;
 150         int max_sharindx = 0;
 151         int i, j, k;
 152
 153         /* Figure out the dimensioning of the problem */
 154         list_for_each_entry(rule, &priv->flow_block.rules, list) {
 155                 if (rule->type != SJA1105_RULE_VL)
 156                         continue;
 157                 /* Each VL lookup entry matches on a single ingress port */
 158                 num_virtual_links += hweight_long(rule->port_mask);
 159
 160                 if (rule->vl.type != SJA1105_VL_NONCRITICAL)
 161                         have_critical_virtual_links = true;
 162                 if (max_sharindx < rule->vl.sharindx)
 163                         max_sharindx = rule->vl.sharindx;
 164         }
 165
 166         if (num_virtual_links > SJA1105_MAX_VL_LOOKUP_COUNT) {
 167                 NL_SET_ERR_MSG_MOD(extack, "Not enough VL entries available");
 168                 return -ENOSPC;
 169         }
 170
 171         if (max_sharindx + 1 > SJA1105_MAX_VL_LOOKUP_COUNT) {
 172                 NL_SET_ERR_MSG_MOD(extack, "Policer index out of range");
 173                 return -ENOSPC;
 174         }
 175
 176         max_sharindx = max_t(int, num_virtual_links, max_sharindx) + 1;
 177
 178         /* Discard previous VL Lookup Table */
 179         table = &priv->static_config.tables[BLK_IDX_VL_LOOKUP];
 180         if (table->entry_count) {
 181                 kfree(table->entries);
 182                 table->entry_count = 0;
 183         }
 184
 185         /* Discard previous VL Policing Table */
 186         table = &priv->static_config.tables[BLK_IDX_VL_POLICING];
 187         if (table->entry_count) {
 188                 kfree(table->entries);
 189                 table->entry_count = 0;
 190         }
 191
 192         /* Discard previous VL Forwarding Table */
 193         table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING];
 194         if (table->entry_count) {
 195                 kfree(table->entries);
 196                 table->entry_count = 0;
 197         }
 198
 199         /* Discard previous VL Forwarding Parameters Table */
 200         table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING_PARAMS];
 201         if (table->entry_count) {
 202                 kfree(table->entries);
 203                 table->entry_count = 0;
 204         }
 205
 206         /* Nothing to do */
 207         if (!num_virtual_links)
 208                 return 0;
 209
 210         /* Pre-allocate space in the static config tables */
 211
 212         /* VL Lookup Table */
 213         table = &priv->static_config.tables[BLK_IDX_VL_LOOKUP];
 214         table->entries = kcalloc(num_virtual_links,
 215                                  table->ops->unpacked_entry_size,
 216                                  GFP_KERNEL);
 217         if (!table->entries)
 218                 return -ENOMEM;
 219         table->entry_count = num_virtual_links;
 220         vl_lookup = table->entries;
 221
 222         k = 0;
 223
 224         list_for_each_entry(rule, &priv->flow_block.rules, list) {
 225                 unsigned long port;
 226
 227                 if (rule->type != SJA1105_RULE_VL)
 228                         continue;
 229
 230                 for_each_set_bit(port, &rule->port_mask, SJA1105_NUM_PORTS) {
 231                         vl_lookup[k].format = SJA1105_VL_FORMAT_PSFP;
 232                         vl_lookup[k].port = port;
 233                         vl_lookup[k].macaddr = rule->key.vl.dmac;
 234                         if (rule->key.type == SJA1105_KEY_VLAN_AWARE_VL) {
 235                                 vl_lookup[k].vlanid = rule->key.vl.vid;
 236                                 vl_lookup[k].vlanprior = rule->key.vl.pcp;
 237                         } else {
 238                                 u16 vid = dsa_8021q_rx_vid(priv->ds, port);
 239
 240                                 vl_lookup[k].vlanid = vid;
 241                                 vl_lookup[k].vlanprior = 0;
 242                         }
 243                         /* For critical VLs, the DESTPORTS mask is taken from
 244                          * the VL Forwarding Table, so no point in putting it
 245                          * in the VL Lookup Table
 246                          */
 247                         if (rule->vl.type == SJA1105_VL_NONCRITICAL)
 248                                 vl_lookup[k].destports = rule->vl.destports;
 249                         else
 250                                 vl_lookup[k].iscritical = true;
 251                         vl_lookup[k].flow_cookie = rule->cookie;
 252                         k++;
 253                 }
 254         }
 255
 256         /* UM10944.pdf chapter 4.2.3 VL Lookup table:
 257          * "the entries in the VL Lookup table must be sorted in ascending
 258          * order (i.e. the smallest value must be loaded first) according to
 259          * the following sort order: MACADDR, VLANID, PORT, VLANPRIOR."
 260          */
 261         for (i = 0; i < num_virtual_links; i++) {
 262                 struct sja1105_vl_lookup_entry *a = &vl_lookup[i];
 263
 264                 for (j = i + 1; j < num_virtual_links; j++) {
 265                         struct sja1105_vl_lookup_entry *b = &vl_lookup[j];
 266
 267                         if (sja1105_vl_key_lower(b, a)) {
 268                                 struct sja1105_vl_lookup_entry tmp = *a;
 269
 270                                 *a = *b;
 271                                 *b = tmp;
 272                         }
 273                 }
 274         }
 275
 276         if (!have_critical_virtual_links)
 277                 return 0;
 278
 279         /* VL Policing Table */
 280         table = &priv->static_config.tables[BLK_IDX_VL_POLICING];
 281         table->entries = kcalloc(max_sharindx, table->ops->unpacked_entry_size,
 282                                  GFP_KERNEL);
 283         if (!table->entries)
 284                 return -ENOMEM;
 285         table->entry_count = max_sharindx;
 286         vl_policing = table->entries;
 287
 288         /* VL Forwarding Table */
 289         table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING];
 290         table->entries = kcalloc(max_sharindx, table->ops->unpacked_entry_size,
 291                                  GFP_KERNEL);
 292         if (!table->entries)
 293                 return -ENOMEM;
 294         table->entry_count = max_sharindx;
 295         vl_fwd = table->entries;
 296
 297         /* VL Forwarding Parameters Table */
 298         table = &priv->static_config.tables[BLK_IDX_VL_FORWARDING_PARAMS];
 299         table->entries = kcalloc(1, table->ops->unpacked_entry_size,
 300                                  GFP_KERNEL);
 301         if (!table->entries)
 302                 return -ENOMEM;
 303         table->entry_count = 1;
 304
 305         for (i = 0; i < num_virtual_links; i++) {
 306                 unsigned long cookie = vl_lookup[i].flow_cookie;
 307                 struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
 308
 309                 if (rule->vl.type == SJA1105_VL_NONCRITICAL)
 310                         continue;
 311                 if (rule->vl.type == SJA1105_VL_TIME_TRIGGERED) {
 312                         int sharindx = rule->vl.sharindx;
 313
 314                         vl_policing[i].type = 1;
 315                         vl_policing[i].sharindx = sharindx;
 316                         vl_policing[i].maxlen = rule->vl.maxlen;
 317                         vl_policing[sharindx].type = 1;
 318
 319                         vl_fwd[i].type = 1;
 320                         vl_fwd[sharindx].type = 1;
 321                         vl_fwd[sharindx].priority = rule->vl.ipv;
 322                         vl_fwd[sharindx].partition = 0;
 323                         vl_fwd[sharindx].destports = rule->vl.destports;
 324                 }
 325         }
 326
 327         sja1105_frame_memory_partitioning(priv);
 328
 329         return 0;
 330 }
 331
 332 int sja1105_vl_redirect(struct sja1105_private *priv, int port,
 333                         struct netlink_ext_ack *extack, unsigned long cookie,
 334                         struct sja1105_key *key, unsigned long destports,
 335                         bool append)
 336 {
 337         struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
 338         int rc;
 339
 340         if (priv->vlan_state == SJA1105_VLAN_UNAWARE &&
 341             key->type != SJA1105_KEY_VLAN_UNAWARE_VL) {
 342                 NL_SET_ERR_MSG_MOD(extack,
 343                                    "Can only redirect based on DMAC");
 344                 return -EOPNOTSUPP;
 345         } else if (key->type != SJA1105_KEY_VLAN_AWARE_VL) {
 346                 NL_SET_ERR_MSG_MOD(extack,
 347                                    "Can only redirect based on {DMAC, VID, PCP}");
 348                 return -EOPNOTSUPP;
 349         }
 350
 351         if (!rule) {
 352                 rule = kzalloc(sizeof(*rule), GFP_KERNEL);
 353                 if (!rule)
 354                         return -ENOMEM;
 355
 356                 rule->cookie = cookie;
 357                 rule->type = SJA1105_RULE_VL;
 358                 rule->key = *key;
 359                 list_add(&rule->list, &priv->flow_block.rules);
 360         }
 361
 362         rule->port_mask |= BIT(port);
 363         if (append)
 364                 rule->vl.destports |= destports;
 365         else
 366                 rule->vl.destports = destports;
 367
 368         rc = sja1105_init_virtual_links(priv, extack);
 369         if (rc) {
 370                 rule->port_mask &= ~BIT(port);
 371                 if (!rule->port_mask) {
 372                         list_del(&rule->list);
 373                         kfree(rule);
 374                 }
 375         }
 376
 377         return rc;
 378 }
 379
 380 int sja1105_vl_delete(struct sja1105_private *priv, int port,
 381                       struct sja1105_rule *rule, struct netlink_ext_ack *extack)
 382 {
 383         int rc;
 384
 385         rule->port_mask &= ~BIT(port);
 386         if (!rule->port_mask) {
 387                 list_del(&rule->list);
 388                 kfree(rule);
 389         }
 390
 391         rc = sja1105_init_virtual_links(priv, extack);
 392         if (rc)
 393                 return rc;
 394
 395         return sja1105_static_config_reload(priv, SJA1105_VIRTUAL_LINKS);
 396 }
 397
 398 /* Insert into the global gate list, sorted by gate action time. */
 399 static int sja1105_insert_gate_entry(struct sja1105_gating_config *gating_cfg,
 400                                      struct sja1105_rule *rule,
 401                                      u8 gate_state, s64 entry_time,
 402                                      struct netlink_ext_ack *extack)
 403 {
 404         struct sja1105_gate_entry *e;
 405         int rc;
 406
 407         e = kzalloc(sizeof(*e), GFP_KERNEL);
 408         if (!e)
 409                 return -ENOMEM;
 410
 411         e->rule = rule;
 412         e->gate_state = gate_state;
 413         e->interval = entry_time;
 414
 415         if (list_empty(&gating_cfg->entries)) {
 416                 list_add(&e->list, &gating_cfg->entries);
 417         } else {
 418                 struct sja1105_gate_entry *p;
 419
 420                 list_for_each_entry(p, &gating_cfg->entries, list) {
 421                         if (p->interval == e->interval) {
 422                                 NL_SET_ERR_MSG_MOD(extack,
 423                                                    "Gate conflict");
 424                                 rc = -EBUSY;
 425                                 goto err;
 426                         }
 427
 428                         if (e->interval < p->interval)
 429                                 break;
 430                 }
 431                 list_add(&e->list, p->list.prev);
 432         }
 433
 434         gating_cfg->num_entries++;
 435
 436         return 0;
 437 err:
 438         kfree(e);
 439         return rc;
 440 }
 441
 442 /* The gate entries contain absolute times in their e->interval field. Convert
 443  * that to proper intervals (i.e. "0, 5, 10, 15" to "5, 5, 5, 5").
 444  */
 445 static void
 446 sja1105_gating_cfg_time_to_interval(struct sja1105_gating_config *gating_cfg,
 447                                     u64 cycle_time)
 448 {
 449         struct sja1105_gate_entry *last_e;
 450         struct sja1105_gate_entry *e;
 451         struct list_head *prev;
 452
 453         list_for_each_entry(e, &gating_cfg->entries, list) {
 454                 struct sja1105_gate_entry *p;
 455
 456                 prev = e->list.prev;
 457
 458                 if (prev == &gating_cfg->entries)
 459                         continue;
 460
 461                 p = list_entry(prev, struct sja1105_gate_entry, list);
 462                 p->interval = e->interval - p->interval;
 463         }
 464         last_e = list_last_entry(&gating_cfg->entries,
 465                                  struct sja1105_gate_entry, list);
 466         if (last_e->list.prev != &gating_cfg->entries)
 467                 last_e->interval = cycle_time - last_e->interval;
 468 }
 469
 470 static void sja1105_free_gating_config(struct sja1105_gating_config *gating_cfg)
 471 {
 472         struct sja1105_gate_entry *e, *n;
 473
 474         list_for_each_entry_safe(e, n, &gating_cfg->entries, list) {
 475                 list_del(&e->list);
 476                 kfree(e);
 477         }
 478 }
 479
 480 static int sja1105_compose_gating_subschedule(struct sja1105_private *priv,
 481                                               struct netlink_ext_ack *extack)
 482 {
 483         struct sja1105_gating_config *gating_cfg = &priv->tas_data.gating_cfg;
 484         struct sja1105_rule *rule;
 485         s64 max_cycle_time = 0;
 486         s64 its_base_time = 0;
 487         int i, rc = 0;
 488
 489         list_for_each_entry(rule, &priv->flow_block.rules, list) {
 490                 if (rule->type != SJA1105_RULE_VL)
 491                         continue;
 492                 if (rule->vl.type != SJA1105_VL_TIME_TRIGGERED)
 493                         continue;
 494
 495                 if (max_cycle_time < rule->vl.cycle_time) {
 496                         max_cycle_time = rule->vl.cycle_time;
 497                         its_base_time = rule->vl.base_time;
 498                 }
 499         }
 500
 501         if (!max_cycle_time)
 502                 return 0;
 503
 504         dev_dbg(priv->ds->dev, "max_cycle_time %lld its_base_time %lld\n",
 505                 max_cycle_time, its_base_time);
 506
 507         sja1105_free_gating_config(gating_cfg);
 508
 509         gating_cfg->base_time = its_base_time;
 510         gating_cfg->cycle_time = max_cycle_time;
 511         gating_cfg->num_entries = 0;
 512
 513         list_for_each_entry(rule, &priv->flow_block.rules, list) {
 514                 s64 time;
 515                 s64 rbt;
 516
 517                 if (rule->type != SJA1105_RULE_VL)
 518                         continue;
 519                 if (rule->vl.type != SJA1105_VL_TIME_TRIGGERED)
 520                         continue;
 521
 522                 /* Calculate the difference between this gating schedule's
 523                  * base time, and the base time of the gating schedule with the
 524                  * longest cycle time. We call it the relative base time (rbt).
 525                  */
 526                 rbt = future_base_time(rule->vl.base_time, rule->vl.cycle_time,
 527                                        its_base_time);
 528                 rbt -= its_base_time;
 529
 530                 time = rbt;
 531
 532                 for (i = 0; i < rule->vl.num_entries; i++) {
 533                         u8 gate_state = rule->vl.entries[i].gate_state;
 534                         s64 entry_time = time;
 535
 536                         while (entry_time < max_cycle_time) {
 537                                 rc = sja1105_insert_gate_entry(gating_cfg, rule,
 538                                                                gate_state,
 539                                                                entry_time,
 540                                                                extack);
 541                                 if (rc)
 542                                         goto err;
 543
 544                                 entry_time += rule->vl.cycle_time;
 545                         }
 546                         time += rule->vl.entries[i].interval;
 547                 }
 548         }
 549
 550         sja1105_gating_cfg_time_to_interval(gating_cfg, max_cycle_time);
 551
 552         return 0;
 553 err:
 554         sja1105_free_gating_config(gating_cfg);
 555         return rc;
 556 }
 557
 558 int sja1105_vl_gate(struct sja1105_private *priv, int port,
 559                     struct netlink_ext_ack *extack, unsigned long cookie,
 560                     struct sja1105_key *key, u32 index, s32 prio,
 561                     u64 base_time, u64 cycle_time, u64 cycle_time_ext,
 562                     u32 num_entries, struct action_gate_entry *entries)
 563 {
 564         struct sja1105_rule *rule = sja1105_rule_find(priv, cookie);
 565         int ipv = -1;
 566         int i, rc;
 567         s32 rem;
 568
 569         if (cycle_time_ext) {
 570                 NL_SET_ERR_MSG_MOD(extack,
 571                                    "Cycle time extension not supported");
 572                 return -EOPNOTSUPP;
 573         }
 574
 575         div_s64_rem(base_time, sja1105_delta_to_ns(1), &rem);
 576         if (rem) {
 577                 NL_SET_ERR_MSG_MOD(extack,
 578                                    "Base time must be multiple of 200 ns");
 579                 return -ERANGE;
 580         }
 581
 582         div_s64_rem(cycle_time, sja1105_delta_to_ns(1), &rem);
 583         if (rem) {
 584                 NL_SET_ERR_MSG_MOD(extack,
 585                                    "Cycle time must be multiple of 200 ns");
 586                 return -ERANGE;
 587         }
 588
 589         if (priv->vlan_state == SJA1105_VLAN_UNAWARE &&
 590             key->type != SJA1105_KEY_VLAN_UNAWARE_VL) {
 591                 dev_err(priv->ds->dev, "1: vlan state %d key type %d\n",
 592                         priv->vlan_state, key->type);
 593                 NL_SET_ERR_MSG_MOD(extack,
 594                                    "Can only gate based on DMAC");
 595                 return -EOPNOTSUPP;
 596         } else if (key->type != SJA1105_KEY_VLAN_AWARE_VL) {
 597                 dev_err(priv->ds->dev, "2: vlan state %d key type %d\n",
 598                         priv->vlan_state, key->type);
 599                 NL_SET_ERR_MSG_MOD(extack,
 600                                    "Can only gate based on {DMAC, VID, PCP}");
 601                 return -EOPNOTSUPP;
 602         }
 603
 604         if (!rule) {
 605                 rule = kzalloc(sizeof(*rule), GFP_KERNEL);
 606                 if (!rule)
 607                         return -ENOMEM;
 608
 609                 list_add(&rule->list, &priv->flow_block.rules);
 610                 rule->cookie = cookie;
 611                 rule->type = SJA1105_RULE_VL;
 612                 rule->key = *key;
 613                 rule->vl.type = SJA1105_VL_TIME_TRIGGERED;
 614                 rule->vl.sharindx = index;
 615                 rule->vl.base_time = base_time;
 616                 rule->vl.cycle_time = cycle_time;
 617                 rule->vl.num_entries = num_entries;
 618                 rule->vl.entries = kcalloc(num_entries,
 619                                            sizeof(struct action_gate_entry),
 620                                            GFP_KERNEL);
 621                 if (!rule->vl.entries) {
 622                         rc = -ENOMEM;
 623                         goto out;
 624                 }
 625
 626                 for (i = 0; i < num_entries; i++) {
 627                         div_s64_rem(entries[i].interval,
 628                                     sja1105_delta_to_ns(1), &rem);
 629                         if (rem) {
 630                                 NL_SET_ERR_MSG_MOD(extack,
 631                                                    "Interval must be multiple of 200 ns");
 632                                 rc = -ERANGE;
 633                                 goto out;
 634                         }
 635
 636                         if (!entries[i].interval) {
 637                                 NL_SET_ERR_MSG_MOD(extack,
 638                                                    "Interval cannot be zero");
 639                                 rc = -ERANGE;
 640                                 goto out;
 641                         }
 642
 643                         if (ns_to_sja1105_delta(entries[i].interval) >
 644                             SJA1105_TAS_MAX_DELTA) {
 645                                 NL_SET_ERR_MSG_MOD(extack,
 646                                                    "Maximum interval is 52 ms");
 647                                 rc = -ERANGE;
 648                                 goto out;
 649                         }
 650
 651                         if (entries[i].maxoctets != -1) {
 652                                 NL_SET_ERR_MSG_MOD(extack,
 653                                                    "Cannot offload IntervalOctetMax");
 654                                 rc = -EOPNOTSUPP;
 655                                 goto out;
 656                         }
 657
 658                         if (ipv == -1) {
 659                                 ipv = entries[i].ipv;
 660                         } else if (ipv != entries[i].ipv) {
 661                                 NL_SET_ERR_MSG_MOD(extack,
 662                                                    "Only support a single IPV per VL");
 663                                 rc = -EOPNOTSUPP;
 664                                 goto out;
 665                         }
 666
 667                         rule->vl.entries[i] = entries[i];
 668                 }
 669
 670                 if (ipv == -1) {
 671                         if (key->type == SJA1105_KEY_VLAN_AWARE_VL)
 672                                 ipv = key->vl.pcp;
 673                         else
 674                                 ipv = 0;
 675                 }
 676
 677                 /* TODO: support per-flow MTU */
 678                 rule->vl.maxlen = VLAN_ETH_FRAME_LEN + ETH_FCS_LEN;
 679                 rule->vl.ipv = ipv;
 680         }
 681
 682         rule->port_mask |= BIT(port);
 683
 684         rc = sja1105_compose_gating_subschedule(priv, extack);
 685         if (rc)
 686                 goto out;
 687
 688         rc = sja1105_init_virtual_links(priv, extack);
 689         if (rc)
 690                 goto out;
 691
 692         if (sja1105_gating_check_conflicts(priv, -1, extack)) {
 693                 NL_SET_ERR_MSG_MOD(extack, "Conflict with tc-taprio schedule");
 694                 rc = -ERANGE;
 695                 goto out;
 696         }
 697
 698 out:
 699         if (rc) {
 700                 rule->port_mask &= ~BIT(port);
 701                 if (!rule->port_mask) {
 702                         list_del(&rule->list);
 703                         kfree(rule->vl.entries);
 704                         kfree(rule);
 705                 }
 706         }
 707
 708         return rc;
 709 }
 710
 711 static int sja1105_find_vlid(struct sja1105_private *priv, int port,
 712                              struct sja1105_key *key)
 713 {
 714         struct sja1105_vl_lookup_entry *vl_lookup;
 715         struct sja1105_table *table;
 716         int i;
 717
 718         if (WARN_ON(key->type != SJA1105_KEY_VLAN_AWARE_VL &&
 719                     key->type != SJA1105_KEY_VLAN_UNAWARE_VL))
 720                 return -1;
 721
 722         table = &priv->static_config.tables[BLK_IDX_VL_LOOKUP];
 723         vl_lookup = table->entries;
 724
 725         for (i = 0; i < table->entry_count; i++) {
 726                 if (key->type == SJA1105_KEY_VLAN_AWARE_VL) {
 727                         if (vl_lookup[i].port == port &&
 728                             vl_lookup[i].macaddr == key->vl.dmac &&
 729                             vl_lookup[i].vlanid == key->vl.vid &&
 730                             vl_lookup[i].vlanprior == key->vl.pcp)
 731                                 return i;
 732                 } else {
 733                         if (vl_lookup[i].port == port &&
 734                             vl_lookup[i].macaddr == key->vl.dmac)
 735                                 return i;
 736                 }
 737         }
 738
 739         return -1;
 740 }
 741
 742 int sja1105_vl_stats(struct sja1105_private *priv, int port,
 743                      struct sja1105_rule *rule, struct flow_stats *stats,
 744                      struct netlink_ext_ack *extack)
 745 {
 746         const struct sja1105_regs *regs = priv->info->regs;
 747         u8 buf[SJA1105_SIZE_VL_STATUS] = {0};
 748         u64 unreleased;
 749         u64 timingerr;
 750         u64 lengtherr;
 751         int vlid, rc;
 752         u64 pkts;
 753
 754         if (rule->vl.type != SJA1105_VL_TIME_TRIGGERED)
 755                 return 0;
 756
 757         vlid = sja1105_find_vlid(priv, port, &rule->key);
 758         if (vlid < 0)
 759                 return 0;
 760
 761         rc = sja1105_xfer_buf(priv, SPI_READ, regs->vl_status + 2 * vlid, buf,
 762                               SJA1105_SIZE_VL_STATUS);
 763         if (rc) {
 764                 NL_SET_ERR_MSG_MOD(extack, "SPI access failed");
 765                 return rc;
 766         }
 767
 768         sja1105_unpack(buf, &timingerr,  31, 16, SJA1105_SIZE_VL_STATUS);
 769         sja1105_unpack(buf, &unreleased, 15,  0, SJA1105_SIZE_VL_STATUS);
 770         sja1105_unpack(buf, &lengtherr,  47, 32, SJA1105_SIZE_VL_STATUS);
 771
 772         pkts = timingerr + unreleased + lengtherr;
 773
 774         flow_stats_update(stats, 0, pkts - rule->vl.stats.pkts,
 775                           jiffies - rule->vl.stats.lastused,
 776                           FLOW_ACTION_HW_STATS_IMMEDIATE);
 777
 778         rule->vl.stats.pkts = pkts;
 779         rule->vl.stats.lastused = jiffies;
 780
 781         return 0;
 782 }