net/dsa/dsa_loop.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * Distributed Switch Architecture loopback driver
0004  *
0005  * Copyright (C) 2016, Florian Fainelli <f.fainelli@gmail.com>
0006  */
0007
0008 #include <linux/platform_device.h>
0009 #include <linux/netdevice.h>
0010 #include <linux/phy.h>
0011 #include <linux/phy_fixed.h>
0012 #include <linux/export.h>
0013 #include <linux/ethtool.h>
0014 #include <linux/workqueue.h>
0015 #include <linux/module.h>
0016 #include <linux/if_bridge.h>
0017 #include <linux/dsa/loop.h>
0018 #include <net/dsa.h>
0019
0020 #include "dsa_loop.h"
0021
0022 static struct dsa_loop_mib_entry dsa_loop_mibs[] = {
0023     [DSA_LOOP_PHY_READ_OK]  = { "phy_read_ok", },
0024     [DSA_LOOP_PHY_READ_ERR] = { "phy_read_err", },
0025     [DSA_LOOP_PHY_WRITE_OK] = { "phy_write_ok", },
0026     [DSA_LOOP_PHY_WRITE_ERR] = { "phy_write_err", },
0027 };
0028
0029 static struct phy_device *phydevs[PHY_MAX_ADDR];
0030
0031 enum dsa_loop_devlink_resource_id {
0032     DSA_LOOP_DEVLINK_PARAM_ID_VTU,
0033 };
0034
0035 static u64 dsa_loop_devlink_vtu_get(void *priv)
0036 {
0037     struct dsa_loop_priv *ps = priv;
0038     unsigned int i, count = 0;
0039     struct dsa_loop_vlan *vl;
0040
0041     for (i = 0; i < ARRAY_SIZE(ps->vlans); i++) {
0042         vl = &ps->vlans[i];
0043         if (vl->members)
0044             count++;
0045     }
0046
0047     return count;
0048 }
0049
0050 static int dsa_loop_setup_devlink_resources(struct dsa_switch *ds)
0051 {
0052     struct devlink_resource_size_params size_params;
0053     struct dsa_loop_priv *ps = ds->priv;
0054     int err;
0055
0056     devlink_resource_size_params_init(&size_params, ARRAY_SIZE(ps->vlans),
0057                       ARRAY_SIZE(ps->vlans),
0058                       1, DEVLINK_RESOURCE_UNIT_ENTRY);
0059
0060     err = dsa_devlink_resource_register(ds, "VTU", ARRAY_SIZE(ps->vlans),
0061                         DSA_LOOP_DEVLINK_PARAM_ID_VTU,
0062                         DEVLINK_RESOURCE_ID_PARENT_TOP,
0063                         &size_params);
0064     if (err)
0065         goto out;
0066
0067     dsa_devlink_resource_occ_get_register(ds,
0068                           DSA_LOOP_DEVLINK_PARAM_ID_VTU,
0069                           dsa_loop_devlink_vtu_get, ps);
0070
0071     return 0;
0072
0073 out:
0074     dsa_devlink_resources_unregister(ds);
0075     return err;
0076 }
0077
0078 static enum dsa_tag_protocol dsa_loop_get_protocol(struct dsa_switch *ds,
0079                            int port,
0080                            enum dsa_tag_protocol mp)
0081 {
0082     dev_dbg(ds->dev, "%s: port: %d\n", __func__, port);
0083
0084     return DSA_TAG_PROTO_NONE;
0085 }
0086
0087 static int dsa_loop_setup(struct dsa_switch *ds)
0088 {
0089     struct dsa_loop_priv *ps = ds->priv;
0090     unsigned int i;
0091
0092     for (i = 0; i < ds->num_ports; i++)
0093         memcpy(ps->ports[i].mib, dsa_loop_mibs,
0094                sizeof(dsa_loop_mibs));
0095
0096     dev_dbg(ds->dev, "%s\n", __func__);
0097
0098     return dsa_loop_setup_devlink_resources(ds);
0099 }
0100
0101 static void dsa_loop_teardown(struct dsa_switch *ds)
0102 {
0103     dsa_devlink_resources_unregister(ds);
0104 }
0105
0106 static int dsa_loop_get_sset_count(struct dsa_switch *ds, int port, int sset)
0107 {
0108     if (sset != ETH_SS_STATS && sset != ETH_SS_PHY_STATS)
0109         return 0;
0110
0111     return __DSA_LOOP_CNT_MAX;
0112 }
0113
0114 static void dsa_loop_get_strings(struct dsa_switch *ds, int port,
0115                  u32 stringset, uint8_t *data)
0116 {
0117     struct dsa_loop_priv *ps = ds->priv;
0118     unsigned int i;
0119
0120     if (stringset != ETH_SS_STATS && stringset != ETH_SS_PHY_STATS)
0121         return;
0122
0123     for (i = 0; i < __DSA_LOOP_CNT_MAX; i++)
0124         memcpy(data + i * ETH_GSTRING_LEN,
0125                ps->ports[port].mib[i].name, ETH_GSTRING_LEN);
0126 }
0127
0128 static void dsa_loop_get_ethtool_stats(struct dsa_switch *ds, int port,
0129                        uint64_t *data)
0130 {
0131     struct dsa_loop_priv *ps = ds->priv;
0132     unsigned int i;
0133
0134     for (i = 0; i < __DSA_LOOP_CNT_MAX; i++)
0135         data[i] = ps->ports[port].mib[i].val;
0136 }
0137
0138 static int dsa_loop_phy_read(struct dsa_switch *ds, int port, int regnum)
0139 {
0140     struct dsa_loop_priv *ps = ds->priv;
0141     struct mii_bus *bus = ps->bus;
0142     int ret;
0143
0144     ret = mdiobus_read_nested(bus, ps->port_base + port, regnum);
0145     if (ret < 0)
0146         ps->ports[port].mib[DSA_LOOP_PHY_READ_ERR].val++;
0147     else
0148         ps->ports[port].mib[DSA_LOOP_PHY_READ_OK].val++;
0149
0150     return ret;
0151 }
0152
0153 static int dsa_loop_phy_write(struct dsa_switch *ds, int port,
0154                   int regnum, u16 value)
0155 {
0156     struct dsa_loop_priv *ps = ds->priv;
0157     struct mii_bus *bus = ps->bus;
0158     int ret;
0159
0160     ret = mdiobus_write_nested(bus, ps->port_base + port, regnum, value);
0161     if (ret < 0)
0162         ps->ports[port].mib[DSA_LOOP_PHY_WRITE_ERR].val++;
0163     else
0164         ps->ports[port].mib[DSA_LOOP_PHY_WRITE_OK].val++;
0165
0166     return ret;
0167 }
0168
0169 static int dsa_loop_port_bridge_join(struct dsa_switch *ds, int port,
0170                      struct dsa_bridge bridge,
0171                      bool *tx_fwd_offload,
0172                      struct netlink_ext_ack *extack)
0173 {
0174     dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n",
0175         __func__, port, bridge.dev->name);
0176
0177     return 0;
0178 }
0179
0180 static void dsa_loop_port_bridge_leave(struct dsa_switch *ds, int port,
0181                        struct dsa_bridge bridge)
0182 {
0183     dev_dbg(ds->dev, "%s: port: %d, bridge: %s\n",
0184         __func__, port, bridge.dev->name);
0185 }
0186
0187 static void dsa_loop_port_stp_state_set(struct dsa_switch *ds, int port,
0188                     u8 state)
0189 {
0190     dev_dbg(ds->dev, "%s: port: %d, state: %d\n",
0191         __func__, port, state);
0192 }
0193
0194 static int dsa_loop_port_vlan_filtering(struct dsa_switch *ds, int port,
0195                     bool vlan_filtering,
0196                     struct netlink_ext_ack *extack)
0197 {
0198     dev_dbg(ds->dev, "%s: port: %d, vlan_filtering: %d\n",
0199         __func__, port, vlan_filtering);
0200
0201     return 0;
0202 }
0203
0204 static int dsa_loop_port_vlan_add(struct dsa_switch *ds, int port,
0205                   const struct switchdev_obj_port_vlan *vlan,
0206                   struct netlink_ext_ack *extack)
0207 {
0208     bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
0209     bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
0210     struct dsa_loop_priv *ps = ds->priv;
0211     struct mii_bus *bus = ps->bus;
0212     struct dsa_loop_vlan *vl;
0213
0214     if (vlan->vid >= ARRAY_SIZE(ps->vlans))
0215         return -ERANGE;
0216
0217     /* Just do a sleeping operation to make lockdep checks effective */
0218     mdiobus_read(bus, ps->port_base + port, MII_BMSR);
0219
0220     vl = &ps->vlans[vlan->vid];
0221
0222     vl->members |= BIT(port);
0223     if (untagged)
0224         vl->untagged |= BIT(port);
0225     else
0226         vl->untagged &= ~BIT(port);
0227
0228     dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n",
0229         __func__, port, vlan->vid, untagged ? "un" : "", pvid);
0230
0231     if (pvid)
0232         ps->ports[port].pvid = vlan->vid;
0233
0234     return 0;
0235 }
0236
0237 static int dsa_loop_port_vlan_del(struct dsa_switch *ds, int port,
0238                   const struct switchdev_obj_port_vlan *vlan)
0239 {
0240     bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
0241     struct dsa_loop_priv *ps = ds->priv;
0242     u16 pvid = ps->ports[port].pvid;
0243     struct mii_bus *bus = ps->bus;
0244     struct dsa_loop_vlan *vl;
0245
0246     /* Just do a sleeping operation to make lockdep checks effective */
0247     mdiobus_read(bus, ps->port_base + port, MII_BMSR);
0248
0249     vl = &ps->vlans[vlan->vid];
0250
0251     vl->members &= ~BIT(port);
0252     if (untagged)
0253         vl->untagged &= ~BIT(port);
0254
0255     if (pvid == vlan->vid)
0256         pvid = 1;
0257
0258     dev_dbg(ds->dev, "%s: port: %d vlan: %d, %stagged, pvid: %d\n",
0259         __func__, port, vlan->vid, untagged ? "un" : "", pvid);
0260     ps->ports[port].pvid = pvid;
0261
0262     return 0;
0263 }
0264
0265 static int dsa_loop_port_change_mtu(struct dsa_switch *ds, int port,
0266                     int new_mtu)
0267 {
0268     struct dsa_loop_priv *priv = ds->priv;
0269
0270     priv->ports[port].mtu = new_mtu;
0271
0272     return 0;
0273 }
0274
0275 static int dsa_loop_port_max_mtu(struct dsa_switch *ds, int port)
0276 {
0277     return ETH_MAX_MTU;
0278 }
0279
0280 static const struct dsa_switch_ops dsa_loop_driver = {
0281     .get_tag_protocol   = dsa_loop_get_protocol,
0282     .setup          = dsa_loop_setup,
0283     .teardown       = dsa_loop_teardown,
0284     .get_strings        = dsa_loop_get_strings,
0285     .get_ethtool_stats  = dsa_loop_get_ethtool_stats,
0286     .get_sset_count     = dsa_loop_get_sset_count,
0287     .get_ethtool_phy_stats  = dsa_loop_get_ethtool_stats,
0288     .phy_read       = dsa_loop_phy_read,
0289     .phy_write      = dsa_loop_phy_write,
0290     .port_bridge_join   = dsa_loop_port_bridge_join,
0291     .port_bridge_leave  = dsa_loop_port_bridge_leave,
0292     .port_stp_state_set = dsa_loop_port_stp_state_set,
0293     .port_vlan_filtering    = dsa_loop_port_vlan_filtering,
0294     .port_vlan_add      = dsa_loop_port_vlan_add,
0295     .port_vlan_del      = dsa_loop_port_vlan_del,
0296     .port_change_mtu    = dsa_loop_port_change_mtu,
0297     .port_max_mtu       = dsa_loop_port_max_mtu,
0298 };
0299
0300 static int dsa_loop_drv_probe(struct mdio_device *mdiodev)
0301 {
0302     struct dsa_loop_pdata *pdata = mdiodev->dev.platform_data;
0303     struct dsa_loop_priv *ps;
0304     struct dsa_switch *ds;
0305     int ret;
0306
0307     if (!pdata)
0308         return -ENODEV;
0309
0310     ds = devm_kzalloc(&mdiodev->dev, sizeof(*ds), GFP_KERNEL);
0311     if (!ds)
0312         return -ENOMEM;
0313
0314     ds->dev = &mdiodev->dev;
0315     ds->num_ports = DSA_LOOP_NUM_PORTS;
0316
0317     ps = devm_kzalloc(&mdiodev->dev, sizeof(*ps), GFP_KERNEL);
0318     if (!ps)
0319         return -ENOMEM;
0320
0321     ps->netdev = dev_get_by_name(&init_net, pdata->netdev);
0322     if (!ps->netdev)
0323         return -EPROBE_DEFER;
0324
0325     pdata->cd.netdev[DSA_LOOP_CPU_PORT] = &ps->netdev->dev;
0326
0327     ds->dev = &mdiodev->dev;
0328     ds->ops = &dsa_loop_driver;
0329     ds->priv = ps;
0330     ps->bus = mdiodev->bus;
0331
0332     dev_set_drvdata(&mdiodev->dev, ds);
0333
0334     ret = dsa_register_switch(ds);
0335     if (!ret)
0336         dev_info(&mdiodev->dev, "%s: 0x%0x\n",
0337              pdata->name, pdata->enabled_ports);
0338
0339     return ret;
0340 }
0341
0342 static void dsa_loop_drv_remove(struct mdio_device *mdiodev)
0343 {
0344     struct dsa_switch *ds = dev_get_drvdata(&mdiodev->dev);
0345     struct dsa_loop_priv *ps;
0346
0347     if (!ds)
0348         return;
0349
0350     ps = ds->priv;
0351
0352     dsa_unregister_switch(ds);
0353     dev_put(ps->netdev);
0354
0355     dev_set_drvdata(&mdiodev->dev, NULL);
0356 }
0357
0358 static void dsa_loop_drv_shutdown(struct mdio_device *mdiodev)
0359 {
0360     struct dsa_switch *ds = dev_get_drvdata(&mdiodev->dev);
0361
0362     if (!ds)
0363         return;
0364
0365     dsa_switch_shutdown(ds);
0366
0367     dev_set_drvdata(&mdiodev->dev, NULL);
0368 }
0369
0370 static struct mdio_driver dsa_loop_drv = {
0371     .mdiodrv.driver = {
0372         .name   = "dsa-loop",
0373     },
0374     .probe  = dsa_loop_drv_probe,
0375     .remove = dsa_loop_drv_remove,
0376     .shutdown = dsa_loop_drv_shutdown,
0377 };
0378
0379 #define NUM_FIXED_PHYS  (DSA_LOOP_NUM_PORTS - 2)
0380
0381 static int __init dsa_loop_init(void)
0382 {
0383     struct fixed_phy_status status = {
0384         .link = 1,
0385         .speed = SPEED_100,
0386         .duplex = DUPLEX_FULL,
0387     };
0388     unsigned int i;
0389
0390     for (i = 0; i < NUM_FIXED_PHYS; i++)
0391         phydevs[i] = fixed_phy_register(PHY_POLL, &status, NULL);
0392
0393     return mdio_driver_register(&dsa_loop_drv);
0394 }
0395 module_init(dsa_loop_init);
0396
0397 static void __exit dsa_loop_exit(void)
0398 {
0399     unsigned int i;
0400
0401     mdio_driver_unregister(&dsa_loop_drv);
0402     for (i = 0; i < NUM_FIXED_PHYS; i++)
0403         if (!IS_ERR(phydevs[i]))
0404             fixed_phy_unregister(phydevs[i]);
0405 }
0406 module_exit(dsa_loop_exit);
0407
0408 MODULE_SOFTDEP("pre: dsa_loop_bdinfo");
0409 MODULE_LICENSE("GPL");
0410 MODULE_AUTHOR("Florian Fainelli");
0411 MODULE_DESCRIPTION("DSA loopback driver");