drivers/net/dsa/dsa_loop.c

   1 /*
   2  * Distributed Switch Architecture loopback driver
   3  *
   4  * Copyright (C) 2016, Florian Fainelli <f.fainelli@gmail.com>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  */
  11
  12 #include <linux/platform_device.h>
  13 #include <linux/netdevice.h>
  14 #include <linux/phy.h>
  15 #include <linux/phy_fixed.h>
  16 #include <linux/export.h>
  17 #include <linux/workqueue.h>
  18 #include <linux/module.h>
  19 #include <linux/if_bridge.h>
  20 #include <net/dsa.h>
  21
  22 #include "dsa_loop.h"
  23
  24 struct dsa_loop_vlan {
  25         u16 members;
  26         u16 untagged;
  27 };
  28
  29 #define DSA_LOOP_VLANS  5
  30
  31 struct dsa_loop_priv {
  32         struct mii_bus  *bus;
  33         unsigned int    port_base;
  34         struct dsa_loop_vlan vlans[DSA_LOOP_VLANS];
  35         struct net_device *netdev;
  36         u16 pvid;
  37 };
  38
  39 static struct phy_device *phydevs[PHY_MAX_ADDR];
  40
  41 static enum dsa_tag_protocol dsa_loop_get_protocol(struct dsa_switch *ds)
  42 {
  43         dev_dbg(ds->dev, "%s\n", __func__);
  44
  45         return DSA_TAG_PROTO_NONE;
  46 }
  47
  48 static int dsa_loop_setup(struct dsa_switch *ds)
  49 {
  50         dev_dbg(ds->dev, "%s\n", __func__);
  51
  52         return 0;
  53 }
  54
  55 static int dsa_loop_set_addr(struct dsa_switch *ds, u8 *addr)
  56 {
  57         dev_dbg(ds->dev, "%s\n", __func__);
  58
  59         return 0;
  60 }
  61
  62 static int dsa_loop_phy_read(struct dsa_switch *ds, int port, int regnum)
  63 {
  64         struct dsa_loop_priv *ps = ds->priv;
  65         struct mii_bus *bus = ps->bus;
  66
  67         dev_dbg(ds->dev, "%s\n", __func__);
  68
  69         return mdiobus_read_nested(bus, ps->port_base + port, regnum);
  70 }
  71
  72 static int dsa_loop_phy_write(struct dsa_switch *ds, int port,
  73                               int regnum, u16 value)
  74 {
  75         struct dsa_loop_priv *ps = ds->priv;
  76         struct mii_bus *bus = ps->bus;
  77
  78         dev_dbg(ds->dev, "%s\n", __func__);
  79
  80         return mdiobus_write_nested(bus, ps->port_base + port, regnum, value);
  81 }
  82
  83 static int dsa_loop_port_bridge_join(struct dsa_switch *ds, int port,
  84                                      struct net_device *bridge)
  85 {
  86         dev_dbg(ds->dev, "%s\n", __func__);
  87
  88         return 0;
  89 }
  90
  91 static void dsa_loop_port_bridge_leave(struct dsa_switch *ds, int port,
  92                                        struct net_device *bridge)
  93 {
  94         dev_dbg(ds->dev, "%s\n", __func__);
  95 }
  96
  97 static void dsa_loop_port_stp_state_set(struct dsa_switch *ds, int port,
  98                                         u8 state)
  99 {
 100         dev_dbg(ds->dev, "%s\n", __func__);
 101 }
 102
 103 static int dsa_loop_port_vlan_filtering(struct dsa_switch *ds, int port,
 104                                         bool vlan_filtering)
 105 {
 106         dev_dbg(ds->dev, "%s\n", __func__);
 107
 108         return 0;
 109 }
 110
 111 static int dsa_loop_port_vlan_prepare(struct dsa_switch *ds, int port,
 112                                       const struct switchdev_obj_port_vlan *vlan,
 113                                       struct switchdev_trans *trans)
 114 {
 115         struct dsa_loop_priv *ps = ds->priv;
 116         struct mii_bus *bus = ps->bus;
 117
 118         dev_dbg(ds->dev, "%s\n", __func__);
 119
 120         /* Just do a sleeping operation to make lockdep checks effective */
 121         mdiobus_read(bus, ps->port_base + port, MII_BMSR);
 122
 123         if (vlan->vid_end > DSA_LOOP_VLANS)
 124                 return -ERANGE;
 125
 126         return 0;
 127 }
 128
 129 static void dsa_loop_port_vlan_add(struct dsa_switch *ds, int port,
 130                                    const struct switchdev_obj_port_vlan *vlan,
 131                                    struct switchdev_trans *trans)
 132 {
 133         bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
 134         bool pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
 135         struct dsa_loop_priv *ps = ds->priv;
 136         struct mii_bus *bus = ps->bus;
 137         struct dsa_loop_vlan *vl;
 138         u16 vid;
 139
 140         dev_dbg(ds->dev, "%s\n", __func__);
 141
 142         /* Just do a sleeping operation to make lockdep checks effective */
 143         mdiobus_read(bus, ps->port_base + port, MII_BMSR);
 144
 145         for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid) {
 146                 vl = &ps->vlans[vid];
 147
 148                 vl->members |= BIT(port);
 149                 if (untagged)
 150                         vl->untagged |= BIT(port);
 151                 else
 152                         vl->untagged &= ~BIT(port);
 153         }
 154
 155         if (pvid)
 156                 ps->pvid = vid;
 157 }
 158
 159 static int dsa_loop_port_vlan_del(struct dsa_switch *ds, int port,
 160                                   const struct switchdev_obj_port_vlan *vlan)
 161 {
 162         bool untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
 163         struct dsa_loop_priv *ps = ds->priv;
 164         struct mii_bus *bus = ps->bus;
 165         struct dsa_loop_vlan *vl;
 166         u16 vid, pvid = ps->pvid;
 167
 168         dev_dbg(ds->dev, "%s\n", __func__);
 169
 170         /* Just do a sleeping operation to make lockdep checks effective */
 171         mdiobus_read(bus, ps->port_base + port, MII_BMSR);
 172
 173         for (vid = vlan->vid_begin; vid <= vlan->vid_end; ++vid) {
 174                 vl = &ps->vlans[vid];
 175
 176                 vl->members &= ~BIT(port);
 177                 if (untagged)
 178                         vl->untagged &= ~BIT(port);
 179
 180                 if (pvid == vid)
 181                         pvid = 1;
 182         }
 183         ps->pvid = pvid;
 184
 185         return 0;
 186 }
 187
 188 static int dsa_loop_port_vlan_dump(struct dsa_switch *ds, int port,
 189                                    struct switchdev_obj_port_vlan *vlan,
 190                                    int (*cb)(struct switchdev_obj *obj))
 191 {
 192         struct dsa_loop_priv *ps = ds->priv;
 193         struct mii_bus *bus = ps->bus;
 194         struct dsa_loop_vlan *vl;
 195         u16 vid, vid_start = 0;
 196         int err = 0;
 197
 198         dev_dbg(ds->dev, "%s\n", __func__);
 199
 200         /* Just do a sleeping operation to make lockdep checks effective */
 201         mdiobus_read(bus, ps->port_base + port, MII_BMSR);
 202
 203         for (vid = vid_start; vid < DSA_LOOP_VLANS; vid++) {
 204                 vl = &ps->vlans[vid];
 205
 206                 if (!(vl->members & BIT(port)))
 207                         continue;
 208
 209                 vlan->vid_begin = vlan->vid_end = vid;
 210                 vlan->flags = 0;
 211
 212                 if (vl->untagged & BIT(port))
 213                         vlan->flags |= BRIDGE_VLAN_INFO_UNTAGGED;
 214                 if (ps->pvid == vid)
 215                         vlan->flags |= BRIDGE_VLAN_INFO_PVID;
 216
 217                 err = cb(&vlan->obj);
 218                 if (err)
 219                         break;
 220         }
 221
 222         return err;
 223 }
 224
 225 static struct dsa_switch_ops dsa_loop_driver = {
 226         .get_tag_protocol       = dsa_loop_get_protocol,
 227         .setup                  = dsa_loop_setup,
 228         .set_addr               = dsa_loop_set_addr,
 229         .phy_read               = dsa_loop_phy_read,
 230         .phy_write              = dsa_loop_phy_write,
 231         .port_bridge_join       = dsa_loop_port_bridge_join,
 232         .port_bridge_leave      = dsa_loop_port_bridge_leave,
 233         .port_stp_state_set     = dsa_loop_port_stp_state_set,
 234         .port_vlan_filtering    = dsa_loop_port_vlan_filtering,
 235         .port_vlan_prepare      = dsa_loop_port_vlan_prepare,
 236         .port_vlan_add          = dsa_loop_port_vlan_add,
 237         .port_vlan_del          = dsa_loop_port_vlan_del,
 238         .port_vlan_dump         = dsa_loop_port_vlan_dump,
 239 };
 240
 241 static int dsa_loop_drv_probe(struct mdio_device *mdiodev)
 242 {
 243         struct dsa_loop_pdata *pdata = mdiodev->dev.platform_data;
 244         struct dsa_loop_priv *ps;
 245         struct dsa_switch *ds;
 246
 247         if (!pdata)
 248                 return -ENODEV;
 249
 250         dev_info(&mdiodev->dev, "%s: 0x%0x\n",
 251                  pdata->name, pdata->enabled_ports);
 252
 253         ds = dsa_switch_alloc(&mdiodev->dev, DSA_MAX_PORTS);
 254         if (!ds)
 255                 return -ENOMEM;
 256
 257         ps = devm_kzalloc(&mdiodev->dev, sizeof(*ps), GFP_KERNEL);
 258         if (!ps)
 259                 return -ENOMEM;
 260
 261         ps->netdev = dev_get_by_name(&init_net, pdata->netdev);
 262         if (!ps->netdev)
 263                 return -EPROBE_DEFER;
 264
 265         pdata->cd.netdev[DSA_LOOP_CPU_PORT] = &ps->netdev->dev;
 266
 267         ds->dev = &mdiodev->dev;
 268         ds->ops = &dsa_loop_driver;
 269         ds->priv = ps;
 270         ps->bus = mdiodev->bus;
 271
 272         dev_set_drvdata(&mdiodev->dev, ds);
 273
 274         return dsa_register_switch(ds, ds->dev);
 275 }
 276
 277 static void dsa_loop_drv_remove(struct mdio_device *mdiodev)
 278 {
 279         struct dsa_switch *ds = dev_get_drvdata(&mdiodev->dev);
 280         struct dsa_loop_priv *ps = ds->priv;
 281
 282         dsa_unregister_switch(ds);
 283         dev_put(ps->netdev);
 284 }
 285
 286 static struct mdio_driver dsa_loop_drv = {
 287         .mdiodrv.driver = {
 288                 .name   = "dsa-loop",
 289         },
 290         .probe  = dsa_loop_drv_probe,
 291         .remove = dsa_loop_drv_remove,
 292 };
 293
 294 #define NUM_FIXED_PHYS  (DSA_LOOP_NUM_PORTS - 2)
 295
 296 static void unregister_fixed_phys(void)
 297 {
 298         unsigned int i;
 299
 300         for (i = 0; i < NUM_FIXED_PHYS; i++)
 301                 if (phydevs[i])
 302                         fixed_phy_unregister(phydevs[i]);
 303 }
 304
 305 static int __init dsa_loop_init(void)
 306 {
 307         struct fixed_phy_status status = {
 308                 .link = 1,
 309                 .speed = SPEED_100,
 310                 .duplex = DUPLEX_FULL,
 311         };
 312         unsigned int i;
 313
 314         for (i = 0; i < NUM_FIXED_PHYS; i++)
 315                 phydevs[i] = fixed_phy_register(PHY_POLL, &status, -1, NULL);
 316
 317         return mdio_driver_register(&dsa_loop_drv);
 318 }
 319 module_init(dsa_loop_init);
 320
 321 static void __exit dsa_loop_exit(void)
 322 {
 323         mdio_driver_unregister(&dsa_loop_drv);
 324         unregister_fixed_phys();
 325 }
 326 module_exit(dsa_loop_exit);
 327
 328 MODULE_LICENSE("GPL");
 329 MODULE_AUTHOR("Florian Fainelli");
 330 MODULE_DESCRIPTION("DSA loopback driver");