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