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