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 } 783