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