1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Distributed Switch Architecture loopback driver 4 * 5 * Copyright (C) 2016, Florian Fainelli <f.fainelli@gmail.com> 6 */ 7 8 #include <linux/platform_device.h> 9 #include <linux/netdevice.h> 10 #include <linux/phy.h> 11 #include <linux/phy_fixed.h> 12 #include <linux/export.h> 13 #include <linux/ethtool.h> 14 #include <linux/workqueue.h> 15 #include <linux/module.h> 16 #include <linux/if_bridge.h> 17 #include <linux/if_vlan.h> 18 #include <linux/types.h> 19 #include <net/dsa.h> 20 21 #define DSA_LOOP_NUM_PORTS 6 22 #define DSA_LOOP_CPU_PORT (DSA_LOOP_NUM_PORTS - 1) 23 #define NUM_FIXED_PHYS (DSA_LOOP_NUM_PORTS - 2) 24 25 struct dsa_loop_vlan { 26 u16 members; 27 u16 untagged; 28 }; 29 30 struct dsa_loop_mib_entry { 31 char name[ETH_GSTRING_LEN]; 32 unsigned long val; 33 }; 34 35 enum dsa_loop_mib_counters { 36 DSA_LOOP_PHY_READ_OK, 37 DSA_LOOP_PHY_READ_ERR, 38 DSA_LOOP_PHY_WRITE_OK, 39 DSA_LOOP_PHY_WRITE_ERR, 40 __DSA_LOOP_CNT_MAX, 41 }; 42 43 struct dsa_loop_port { 44 struct dsa_loop_mib_entry mib[__DSA_LOOP_CNT_MAX]; 45 u16 pvid; 46 int mtu; 47 }; 48 49 struct dsa_loop_priv { 50 struct mii_bus *bus; 51 unsigned int port_base; 52 struct dsa_loop_vlan vlans[VLAN_N_VID]; 53 struct net_device *netdev; 54 struct dsa_loop_port ports[DSA_MAX_PORTS]; 55 }; 56 57 struct dsa_loop_pdata { 58 /* Must be first, such that dsa_register_switch() can access this 59 * without gory pointer manipulations 60 */ 61 struct dsa_chip_data cd; 62 const char *name; 63 unsigned int enabled_ports; 64 const char *netdev; 65 }; 66 67 static struct dsa_loop_mib_entry dsa_loop_mibs[] = { 68 [DSA_LOOP_PHY_READ_OK] = { "phy_read_ok", }, 69 [DSA_LOOP_PHY_READ_ERR] = { "phy_read_err", }, 70 [DSA_LOOP_PHY_WRITE_OK] = { "phy_write_ok", }, 71 [DSA_LOOP_PHY_WRITE_ERR] = { "phy_write_err", }, 72 }; 73 74 static struct phy_device *phydevs[PHY_MAX_ADDR]; 75 static struct mdio_device *switch_mdiodev; 76 77 enum dsa_loop_devlink_resource_id { 78 DSA_LOOP_DEVLINK_PARAM_ID_VTU, 79 }; 80 81 static u64 dsa_loop_devlink_vtu_get(void *priv) 82 { 83 struct dsa_loop_priv *ps = priv; 84 unsigned int i, count = 0; 85 struct dsa_loop_vlan *vl; 86 87 for (i = 0; i < ARRAY_SIZE(ps->vlans); i++) { 88 vl = &ps->vlans[i]; 89 if (vl->members) 90 count++; 91 } 92 93 return count; 94 } 95 96 static int dsa_loop_setup_devlink_resources(struct dsa_switch *ds) 97 { 98 struct devlink_resource_size_params size_params; 99 struct dsa_loop_priv *ps = ds->priv; 100 int err; 101 102 devlink_resource_size_params_init(&size_params, ARRAY_SIZE(ps->vlans), 103 ARRAY_SIZE(ps->vlans), 104 1, DEVLINK_RESOURCE_UNIT_ENTRY); 105 106 err = dsa_devlink_resource_register(ds, "VTU", ARRAY_SIZE(ps->vlans), 107 DSA_LOOP_DEVLINK_PARAM_ID_VTU, 108 DEVLINK_RESOURCE_ID_PARENT_TOP, 109 &size_params); 110 if (err) 111 goto out; 112 113 dsa_devlink_resource_occ_get_register(ds, 114 DSA_LOOP_DEVLINK_PARAM_ID_VTU, 115 dsa_loop_devlink_vtu_get, ps); 116 117 return 0; 118 119 out: 120 dsa_devlink_resources_unregister(ds); 121 return err; 122 } 123 124 static enum dsa_tag_protocol dsa_loop_get_protocol(struct dsa_switch *ds, 125 int port, 126 enum dsa_tag_protocol mp) 127 { 128 dev_dbg(ds->dev, "%s: port: %d\n", __func__, port); 129 130 return DSA_TAG_PROTO_NONE; 131 } 132 133 static int dsa_loop_setup(struct dsa_switch *ds) 134 { 135 struct dsa_loop_priv *ps = ds->priv; 136 unsigned int i; 137 138 for (i = 0; i < ds->num_ports; i++) 139 memcpy(ps->ports[i].mib, dsa_loop_mibs, 140 sizeof(dsa_loop_mibs)); 141 142 dev_dbg(ds->dev, "%s\n", __func__); 143 144 return dsa_loop_setup_devlink_resources(ds); 145 } 146 147 static void dsa_loop_teardown(struct dsa_switch *ds) 148 { 149 dsa_devlink_resources_unregister(ds); 150 } 151 152 static int dsa_loop_get_sset_count(struct dsa_switch *ds, int port, int sset) 153 { 154 if (sset != ETH_SS_STATS && sset != ETH_SS_PHY_STATS) 155 return 0; 156 157 return __DSA_LOOP_CNT_MAX; 158 } 159 160 static void dsa_loop_get_strings(struct dsa_switch *ds, int port, 161 u32 stringset, uint8_t *data) 162 { 163 struct dsa_loop_priv *ps = ds->priv; 164 unsigned int i; 165 166 if (stringset != ETH_SS_STATS && stringset != ETH_SS_PHY_STATS) 167 return; 168 169 for (i = 0; i < __DSA_LOOP_CNT_MAX; i++) 170 ethtool_puts(&data, ps->ports[port].mib[i].name); 171 } 172 173 static void dsa_loop_get_ethtool_stats(struct dsa_switch *ds, int port, 174 uint64_t *data) 175 { 176 struct dsa_loop_priv *ps = ds->priv; 177 unsigned int i; 178 179 for (i = 0; i < __DSA_LOOP_CNT_MAX; i++) 180 data[i] = ps->ports[port].mib[i].val; 181 } 182 183 static int dsa_loop_phy_read(struct dsa_switch *ds, int port, int regnum) 184 { 185 struct dsa_loop_priv *ps = ds->priv; 186 struct mii_bus *bus = ps->bus; 187 int ret; 188 189 ret = mdiobus_read_nested(bus, ps->port_base + port, regnum); 190 if (ret < 0) 191 ps->ports[port].mib[DSA_LOOP_PHY_READ_ERR].val++; 192 else 193 ps->ports[port].mib[DSA_LOOP_PHY_READ_OK].val++; 194 195 return ret; 196 } 197 198 static int dsa_loop_phy_write(struct dsa_switch *ds, int port, 199 int regnum, u16 value) 200 { 201 struct dsa_loop_priv *ps = ds->priv; 202 struct mii_bus *bus = ps->bus; 203 int ret; 204 205 ret = mdiobus_write_nested(bus, ps->port_base + port, regnum, value); 206 if (ret < 0) 207 ps->ports[port].mib[DSA_LOOP_PHY_WRITE_ERR].val++; 208 else 209 ps->ports[port].mib[DSA_LOOP_PHY_WRITE_OK].val++; 210 211 return ret; 212 } 213 214 static int dsa_loop_port_bridge_join(struct dsa_switch *ds, int port, 215 struct dsa_bridge bridge, 216 bool *tx_fwd_offload, 217 struct netlink_ext_ack *extack) 218 { 219 dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n", 220 __func__, port, bridge.dev->name); 221 222 return 0; 223 } 224 225 static void dsa_loop_port_bridge_leave(struct dsa_switch *ds, int port, 226 struct dsa_bridge bridge) 227 { 228 dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n", 229 __func__, port, bridge.dev->name); 230 } 231 232 static void dsa_loop_port_stp_state_set(struct dsa_switch *ds, int port, 233 u8 state) 234 { 235 dev_dbg(ds->dev, "%s: port: %d, state: %d\n", 236 __func__, port, state); 237 } 238 239 static int dsa_loop_port_vlan_filtering(struct dsa_switch *ds, int port, 240 bool vlan_filtering, 241 struct netlink_ext_ack *extack) 242 { 243 dev_dbg(ds->dev, "%s: port: %d, vlan_filtering: %d\n", 244 __func__, port, vlan_filtering); 245 246 return 0; 247 } 248 249 static int dsa_loop_port_vlan_add(struct dsa_switch *ds, int port, 250 const struct switchdev_obj_port_vlan *vlan, 251 struct netlink_ext_ack *extack) 252 { 253 bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED; 254 bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID; 255 struct dsa_loop_priv *ps = ds->priv; 256 struct mii_bus *bus = ps->bus; 257 struct dsa_loop_vlan *vl; 258 259 if (vlan->vid >= ARRAY_SIZE(ps->vlans)) 260 return -ERANGE; 261 262 /* Just do a sleeping operation to make lockdep checks effective */ 263 mdiobus_read(bus, ps->port_base + port, MII_BMSR); 264 265 vl = &ps->vlans[vlan->vid]; 266 267 vl->members |= BIT(port); 268 if (untagged) 269 vl->untagged |= BIT(port); 270 else 271 vl->untagged &= ~BIT(port); 272 273 dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n", 274 __func__, port, vlan->vid, untagged ? "un" : "", pvid); 275 276 if (pvid) 277 ps->ports[port].pvid = vlan->vid; 278 279 return 0; 280 } 281 282 static int dsa_loop_port_vlan_del(struct dsa_switch *ds, int port, 283 const struct switchdev_obj_port_vlan *vlan) 284 { 285 bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED; 286 struct dsa_loop_priv *ps = ds->priv; 287 u16 pvid = ps->ports[port].pvid; 288 struct mii_bus *bus = ps->bus; 289 struct dsa_loop_vlan *vl; 290 291 /* Just do a sleeping operation to make lockdep checks effective */ 292 mdiobus_read(bus, ps->port_base + port, MII_BMSR); 293 294 vl = &ps->vlans[vlan->vid]; 295 296 vl->members &= ~BIT(port); 297 if (untagged) 298 vl->untagged &= ~BIT(port); 299 300 if (pvid == vlan->vid) 301 pvid = 1; 302 303 dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n", 304 __func__, port, vlan->vid, untagged ? "un" : "", pvid); 305 ps->ports[port].pvid = pvid; 306 307 return 0; 308 } 309 310 static int dsa_loop_port_change_mtu(struct dsa_switch *ds, int port, 311 int new_mtu) 312 { 313 struct dsa_loop_priv *priv = ds->priv; 314 315 priv->ports[port].mtu = new_mtu; 316 317 return 0; 318 } 319 320 static int dsa_loop_port_max_mtu(struct dsa_switch *ds, int port) 321 { 322 return ETH_MAX_MTU; 323 } 324 325 static void dsa_loop_phylink_get_caps(struct dsa_switch *dsa, int port, 326 struct phylink_config *config) 327 { 328 bitmap_fill(config->supported_interfaces, PHY_INTERFACE_MODE_MAX); 329 __clear_bit(PHY_INTERFACE_MODE_NA, config->supported_interfaces); 330 config->mac_capabilities = ~0; 331 } 332 333 static const struct dsa_switch_ops dsa_loop_driver = { 334 .get_tag_protocol = dsa_loop_get_protocol, 335 .setup = dsa_loop_setup, 336 .teardown = dsa_loop_teardown, 337 .get_strings = dsa_loop_get_strings, 338 .get_ethtool_stats = dsa_loop_get_ethtool_stats, 339 .get_sset_count = dsa_loop_get_sset_count, 340 .get_ethtool_phy_stats = dsa_loop_get_ethtool_stats, 341 .phy_read = dsa_loop_phy_read, 342 .phy_write = dsa_loop_phy_write, 343 .port_bridge_join = dsa_loop_port_bridge_join, 344 .port_bridge_leave = dsa_loop_port_bridge_leave, 345 .port_stp_state_set = dsa_loop_port_stp_state_set, 346 .port_vlan_filtering = dsa_loop_port_vlan_filtering, 347 .port_vlan_add = dsa_loop_port_vlan_add, 348 .port_vlan_del = dsa_loop_port_vlan_del, 349 .port_change_mtu = dsa_loop_port_change_mtu, 350 .port_max_mtu = dsa_loop_port_max_mtu, 351 .phylink_get_caps = dsa_loop_phylink_get_caps, 352 }; 353 354 static int dsa_loop_drv_probe(struct mdio_device *mdiodev) 355 { 356 struct dsa_loop_pdata *pdata = mdiodev->dev.platform_data; 357 struct dsa_loop_priv *ps; 358 struct dsa_switch *ds; 359 int ret; 360 361 if (!pdata) 362 return -ENODEV; 363 364 ds = devm_kzalloc(&mdiodev->dev, sizeof(*ds), GFP_KERNEL); 365 if (!ds) 366 return -ENOMEM; 367 368 ds->dev = &mdiodev->dev; 369 ds->num_ports = DSA_LOOP_NUM_PORTS; 370 371 ps = devm_kzalloc(&mdiodev->dev, sizeof(*ps), GFP_KERNEL); 372 if (!ps) 373 return -ENOMEM; 374 375 ps->netdev = dev_get_by_name(&init_net, pdata->netdev); 376 if (!ps->netdev) 377 return -EPROBE_DEFER; 378 379 pdata->cd.netdev[DSA_LOOP_CPU_PORT] = &ps->netdev->dev; 380 381 ds->dev = &mdiodev->dev; 382 ds->ops = &dsa_loop_driver; 383 ds->priv = ps; 384 ps->bus = mdiodev->bus; 385 386 dev_set_drvdata(&mdiodev->dev, ds); 387 388 ret = dsa_register_switch(ds); 389 if (!ret) 390 dev_info(&mdiodev->dev, "%s: 0x%0x\n", 391 pdata->name, pdata->enabled_ports); 392 393 return ret; 394 } 395 396 static void dsa_loop_drv_remove(struct mdio_device *mdiodev) 397 { 398 struct dsa_switch *ds = dev_get_drvdata(&mdiodev->dev); 399 struct dsa_loop_priv *ps; 400 401 if (!ds) 402 return; 403 404 ps = ds->priv; 405 406 dsa_unregister_switch(ds); 407 dev_put(ps->netdev); 408 } 409 410 static void dsa_loop_drv_shutdown(struct mdio_device *mdiodev) 411 { 412 struct dsa_switch *ds = dev_get_drvdata(&mdiodev->dev); 413 414 if (!ds) 415 return; 416 417 dsa_switch_shutdown(ds); 418 419 dev_set_drvdata(&mdiodev->dev, NULL); 420 } 421 422 static struct mdio_driver dsa_loop_drv = { 423 .mdiodrv.driver = { 424 .name = "dsa-loop", 425 }, 426 .probe = dsa_loop_drv_probe, 427 .remove = dsa_loop_drv_remove, 428 .shutdown = dsa_loop_drv_shutdown, 429 }; 430 431 static int dsa_loop_bus_match(struct device *dev, 432 const struct device_driver *drv) 433 { 434 return drv == &dsa_loop_drv.mdiodrv.driver; 435 } 436 437 static void dsa_loop_phydevs_unregister(void) 438 { 439 for (int i = 0; i < NUM_FIXED_PHYS; i++) { 440 if (!IS_ERR(phydevs[i])) 441 fixed_phy_unregister(phydevs[i]); 442 } 443 } 444 445 static int __init dsa_loop_create_switch_mdiodev(void) 446 { 447 static struct dsa_loop_pdata dsa_loop_pdata = { 448 .cd = { 449 .port_names[0] = "lan1", 450 .port_names[1] = "lan2", 451 .port_names[2] = "lan3", 452 .port_names[3] = "lan4", 453 .port_names[DSA_LOOP_CPU_PORT] = "cpu", 454 }, 455 .name = "DSA mockup driver", 456 .enabled_ports = 0x1f, 457 .netdev = "eth0", 458 }; 459 struct mii_bus *bus; 460 int ret = -ENODEV; 461 462 bus = mdio_find_bus("fixed-0"); 463 if (WARN_ON(!bus)) 464 return ret; 465 466 switch_mdiodev = mdio_device_create(bus, 31); 467 if (IS_ERR(switch_mdiodev)) 468 goto out; 469 470 switch_mdiodev->bus_match = dsa_loop_bus_match; 471 switch_mdiodev->dev.platform_data = &dsa_loop_pdata; 472 473 ret = mdio_device_register(switch_mdiodev); 474 if (ret) 475 mdio_device_free(switch_mdiodev); 476 out: 477 put_device(&bus->dev); 478 return ret; 479 } 480 481 static int __init dsa_loop_init(void) 482 { 483 unsigned int i; 484 int ret; 485 486 ret = dsa_loop_create_switch_mdiodev(); 487 if (ret) 488 return ret; 489 490 for (i = 0; i < NUM_FIXED_PHYS; i++) 491 phydevs[i] = fixed_phy_register_100fd(); 492 493 ret = mdio_driver_register(&dsa_loop_drv); 494 if (ret) { 495 dsa_loop_phydevs_unregister(); 496 mdio_device_remove(switch_mdiodev); 497 mdio_device_free(switch_mdiodev); 498 } 499 500 return ret; 501 } 502 module_init(dsa_loop_init); 503 504 static void __exit dsa_loop_exit(void) 505 { 506 mdio_driver_unregister(&dsa_loop_drv); 507 dsa_loop_phydevs_unregister(); 508 mdio_device_remove(switch_mdiodev); 509 mdio_device_free(switch_mdiodev); 510 } 511 module_exit(dsa_loop_exit); 512 513 MODULE_LICENSE("GPL"); 514 MODULE_AUTHOR("Florian Fainelli"); 515 MODULE_DESCRIPTION("DSA loopback driver"); 516