net/dsa/dsa.c

   1 /*
   2  * net/dsa/dsa.c - Hardware switch handling
   3  * Copyright (c) 2008-2009 Marvell Semiconductor
   4  * Copyright (c) 2013 Florian Fainelli <florian@openwrt.org>
   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/device.h>
  13 #include <linux/list.h>
  14 #include <linux/platform_device.h>
  15 #include <linux/slab.h>
  16 #include <linux/module.h>
  17 #include <linux/of.h>
  18 #include <linux/of_mdio.h>
  19 #include <linux/of_platform.h>
  20 #include <linux/of_net.h>
  21 #include <linux/of_gpio.h>
  22 #include <linux/netdevice.h>
  23 #include <linux/sysfs.h>
  24 #include <linux/phy_fixed.h>
  25 #include <linux/gpio/consumer.h>
  26 #include <linux/etherdevice.h>
  27 #include <net/dsa.h>
  28 #include "dsa_priv.h"
  29
  30 static struct sk_buff *dsa_slave_notag_xmit(struct sk_buff *skb,
  31                                             struct net_device *dev)
  32 {
  33         /* Just return the original SKB */
  34         return skb;
  35 }
  36
  37 static const struct dsa_device_ops none_ops = {
  38         .xmit   = dsa_slave_notag_xmit,
  39         .rcv    = NULL,
  40 };
  41
  42 const struct dsa_device_ops *dsa_device_ops[DSA_TAG_LAST] = {
  43 #ifdef CONFIG_NET_DSA_TAG_DSA
  44         [DSA_TAG_PROTO_DSA] = &dsa_netdev_ops,
  45 #endif
  46 #ifdef CONFIG_NET_DSA_TAG_EDSA
  47         [DSA_TAG_PROTO_EDSA] = &edsa_netdev_ops,
  48 #endif
  49 #ifdef CONFIG_NET_DSA_TAG_TRAILER
  50         [DSA_TAG_PROTO_TRAILER] = &trailer_netdev_ops,
  51 #endif
  52 #ifdef CONFIG_NET_DSA_TAG_BRCM
  53         [DSA_TAG_PROTO_BRCM] = &brcm_netdev_ops,
  54 #endif
  55 #ifdef CONFIG_NET_DSA_TAG_QCA
  56         [DSA_TAG_PROTO_QCA] = &qca_netdev_ops,
  57 #endif
  58 #ifdef CONFIG_NET_DSA_TAG_MTK
  59         [DSA_TAG_PROTO_MTK] = &mtk_netdev_ops,
  60 #endif
  61         [DSA_TAG_PROTO_NONE] = &none_ops,
  62 };
  63
  64 /* switch driver registration ***********************************************/
  65 static DEFINE_MUTEX(dsa_switch_drivers_mutex);
  66 static LIST_HEAD(dsa_switch_drivers);
  67
  68 void register_switch_driver(struct dsa_switch_driver *drv)
  69 {
  70         mutex_lock(&dsa_switch_drivers_mutex);
  71         list_add_tail(&drv->list, &dsa_switch_drivers);
  72         mutex_unlock(&dsa_switch_drivers_mutex);
  73 }
  74 EXPORT_SYMBOL_GPL(register_switch_driver);
  75
  76 void unregister_switch_driver(struct dsa_switch_driver *drv)
  77 {
  78         mutex_lock(&dsa_switch_drivers_mutex);
  79         list_del_init(&drv->list);
  80         mutex_unlock(&dsa_switch_drivers_mutex);
  81 }
  82 EXPORT_SYMBOL_GPL(unregister_switch_driver);
  83
  84 static const struct dsa_switch_ops *
  85 dsa_switch_probe(struct device *parent, struct device *host_dev, int sw_addr,
  86                  const char **_name, void **priv)
  87 {
  88         const struct dsa_switch_ops *ret;
  89         struct list_head *list;
  90         const char *name;
  91
  92         ret = NULL;
  93         name = NULL;
  94
  95         mutex_lock(&dsa_switch_drivers_mutex);
  96         list_for_each(list, &dsa_switch_drivers) {
  97                 const struct dsa_switch_ops *ops;
  98                 struct dsa_switch_driver *drv;
  99
 100                 drv = list_entry(list, struct dsa_switch_driver, list);
 101                 ops = drv->ops;
 102
 103                 name = ops->probe(parent, host_dev, sw_addr, priv);
 104                 if (name != NULL) {
 105                         ret = ops;
 106                         break;
 107                 }
 108         }
 109         mutex_unlock(&dsa_switch_drivers_mutex);
 110
 111         *_name = name;
 112
 113         return ret;
 114 }
 115
 116 /* basic switch operations **************************************************/
 117 int dsa_cpu_dsa_setup(struct dsa_switch *ds, struct device *dev,
 118                       struct dsa_port *dport, int port)
 119 {
 120         struct device_node *port_dn = dport->dn;
 121         struct phy_device *phydev;
 122         int ret, mode;
 123
 124         if (of_phy_is_fixed_link(port_dn)) {
 125                 ret = of_phy_register_fixed_link(port_dn);
 126                 if (ret) {
 127                         dev_err(dev, "failed to register fixed PHY\n");
 128                         return ret;
 129                 }
 130                 phydev = of_phy_find_device(port_dn);
 131
 132                 mode = of_get_phy_mode(port_dn);
 133                 if (mode < 0)
 134                         mode = PHY_INTERFACE_MODE_NA;
 135                 phydev->interface = mode;
 136
 137                 genphy_config_init(phydev);
 138                 genphy_read_status(phydev);
 139                 if (ds->ops->adjust_link)
 140                         ds->ops->adjust_link(ds, port, phydev);
 141
 142                 put_device(&phydev->mdio.dev);
 143         }
 144
 145         return 0;
 146 }
 147
 148 static int dsa_cpu_dsa_setups(struct dsa_switch *ds, struct device *dev)
 149 {
 150         struct dsa_port *dport;
 151         int ret, port;
 152
 153         for (port = 0; port < ds->num_ports; port++) {
 154                 if (!(dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port)))
 155                         continue;
 156
 157                 dport = &ds->ports[port];
 158                 ret = dsa_cpu_dsa_setup(ds, dev, dport, port);
 159                 if (ret)
 160                         return ret;
 161         }
 162         return 0;
 163 }
 164
 165 const struct dsa_device_ops *dsa_resolve_tag_protocol(int tag_protocol)
 166 {
 167         const struct dsa_device_ops *ops;
 168
 169         if (tag_protocol >= DSA_TAG_LAST)
 170                 return ERR_PTR(-EINVAL);
 171         ops = dsa_device_ops[tag_protocol];
 172
 173         if (!ops)
 174                 return ERR_PTR(-ENOPROTOOPT);
 175
 176         return ops;
 177 }
 178
 179 int dsa_cpu_port_ethtool_setup(struct dsa_switch *ds)
 180 {
 181         struct net_device *master;
 182         struct ethtool_ops *cpu_ops;
 183
 184         master = ds->dst->master_netdev;
 185         if (ds->master_netdev)
 186                 master = ds->master_netdev;
 187
 188         cpu_ops = devm_kzalloc(ds->dev, sizeof(*cpu_ops), GFP_KERNEL);
 189         if (!cpu_ops)
 190                 return -ENOMEM;
 191
 192         memcpy(&ds->dst->master_ethtool_ops, master->ethtool_ops,
 193                sizeof(struct ethtool_ops));
 194         ds->dst->master_orig_ethtool_ops = master->ethtool_ops;
 195         memcpy(cpu_ops, &ds->dst->master_ethtool_ops,
 196                sizeof(struct ethtool_ops));
 197         dsa_cpu_port_ethtool_init(cpu_ops);
 198         master->ethtool_ops = cpu_ops;
 199
 200         return 0;
 201 }
 202
 203 void dsa_cpu_port_ethtool_restore(struct dsa_switch *ds)
 204 {
 205         struct net_device *master;
 206
 207         master = ds->dst->master_netdev;
 208         if (ds->master_netdev)
 209                 master = ds->master_netdev;
 210
 211         master->ethtool_ops = ds->dst->master_orig_ethtool_ops;
 212 }
 213
 214 static int dsa_switch_setup_one(struct dsa_switch *ds, struct device *parent)
 215 {
 216         const struct dsa_switch_ops *ops = ds->ops;
 217         struct dsa_switch_tree *dst = ds->dst;
 218         struct dsa_chip_data *cd = ds->cd;
 219         bool valid_name_found = false;
 220         int index = ds->index;
 221         int i, ret;
 222
 223         /*
 224          * Validate supplied switch configuration.
 225          */
 226         for (i = 0; i < ds->num_ports; i++) {
 227                 char *name;
 228
 229                 name = cd->port_names[i];
 230                 if (name == NULL)
 231                         continue;
 232
 233                 if (!strcmp(name, "cpu")) {
 234                         if (dst->cpu_switch) {
 235                                 netdev_err(dst->master_netdev,
 236                                            "multiple cpu ports?!\n");
 237                                 return -EINVAL;
 238                         }
 239                         dst->cpu_switch = ds;
 240                         dst->cpu_port = i;
 241                         ds->cpu_port_mask |= 1 << i;
 242                 } else if (!strcmp(name, "dsa")) {
 243                         ds->dsa_port_mask |= 1 << i;
 244                 } else {
 245                         ds->enabled_port_mask |= 1 << i;
 246                 }
 247                 valid_name_found = true;
 248         }
 249
 250         if (!valid_name_found && i == ds->num_ports)
 251                 return -EINVAL;
 252
 253         /* Make the built-in MII bus mask match the number of ports,
 254          * switch drivers can override this later
 255          */
 256         ds->phys_mii_mask = ds->enabled_port_mask;
 257
 258         /*
 259          * If the CPU connects to this switch, set the switch tree
 260          * tagging protocol to the preferred tagging format of this
 261          * switch.
 262          */
 263         if (dst->cpu_switch == ds) {
 264                 enum dsa_tag_protocol tag_protocol;
 265
 266                 tag_protocol = ops->get_tag_protocol(ds);
 267                 dst->tag_ops = dsa_resolve_tag_protocol(tag_protocol);
 268                 if (IS_ERR(dst->tag_ops))
 269                         return PTR_ERR(dst->tag_ops);
 270
 271                 dst->rcv = dst->tag_ops->rcv;
 272         }
 273
 274         memcpy(ds->rtable, cd->rtable, sizeof(ds->rtable));
 275
 276         /*
 277          * Do basic register setup.
 278          */
 279         ret = ops->setup(ds);
 280         if (ret < 0)
 281                 return ret;
 282
 283         ret = dsa_switch_register_notifier(ds);
 284         if (ret)
 285                 return ret;
 286
 287         if (ops->set_addr) {
 288                 ret = ops->set_addr(ds, dst->master_netdev->dev_addr);
 289                 if (ret < 0)
 290                         return ret;
 291         }
 292
 293         if (!ds->slave_mii_bus && ops->phy_read) {
 294                 ds->slave_mii_bus = devm_mdiobus_alloc(parent);
 295                 if (!ds->slave_mii_bus)
 296                         return -ENOMEM;
 297                 dsa_slave_mii_bus_init(ds);
 298
 299                 ret = mdiobus_register(ds->slave_mii_bus);
 300                 if (ret < 0)
 301                         return ret;
 302         }
 303
 304         /*
 305          * Create network devices for physical switch ports.
 306          */
 307         for (i = 0; i < ds->num_ports; i++) {
 308                 ds->ports[i].dn = cd->port_dn[i];
 309
 310                 if (!(ds->enabled_port_mask & (1 << i)))
 311                         continue;
 312
 313                 ret = dsa_slave_create(ds, parent, i, cd->port_names[i]);
 314                 if (ret < 0)
 315                         netdev_err(dst->master_netdev, "[%d]: can't create dsa slave device for port %d(%s): %d\n",
 316                                    index, i, cd->port_names[i], ret);
 317         }
 318
 319         /* Perform configuration of the CPU and DSA ports */
 320         ret = dsa_cpu_dsa_setups(ds, parent);
 321         if (ret < 0)
 322                 netdev_err(dst->master_netdev, "[%d] : can't configure CPU and DSA ports\n",
 323                            index);
 324
 325         ret = dsa_cpu_port_ethtool_setup(ds);
 326         if (ret)
 327                 return ret;
 328
 329         return 0;
 330 }
 331
 332 static struct dsa_switch *
 333 dsa_switch_setup(struct dsa_switch_tree *dst, int index,
 334                  struct device *parent, struct device *host_dev)
 335 {
 336         struct dsa_chip_data *cd = dst->pd->chip + index;
 337         const struct dsa_switch_ops *ops;
 338         struct dsa_switch *ds;
 339         int ret;
 340         const char *name;
 341         void *priv;
 342
 343         /*
 344          * Probe for switch model.
 345          */
 346         ops = dsa_switch_probe(parent, host_dev, cd->sw_addr, &name, &priv);
 347         if (!ops) {
 348                 netdev_err(dst->master_netdev, "[%d]: could not detect attached switch\n",
 349                            index);
 350                 return ERR_PTR(-EINVAL);
 351         }
 352         netdev_info(dst->master_netdev, "[%d]: detected a %s switch\n",
 353                     index, name);
 354
 355
 356         /*
 357          * Allocate and initialise switch state.
 358          */
 359         ds = dsa_switch_alloc(parent, DSA_MAX_PORTS);
 360         if (!ds)
 361                 return ERR_PTR(-ENOMEM);
 362
 363         ds->dst = dst;
 364         ds->index = index;
 365         ds->cd = cd;
 366         ds->ops = ops;
 367         ds->priv = priv;
 368
 369         ret = dsa_switch_setup_one(ds, parent);
 370         if (ret)
 371                 return ERR_PTR(ret);
 372
 373         return ds;
 374 }
 375
 376 void dsa_cpu_dsa_destroy(struct dsa_port *port)
 377 {
 378         struct device_node *port_dn = port->dn;
 379
 380         if (of_phy_is_fixed_link(port_dn))
 381                 of_phy_deregister_fixed_link(port_dn);
 382 }
 383
 384 static void dsa_switch_destroy(struct dsa_switch *ds)
 385 {
 386         int port;
 387
 388         /* Destroy network devices for physical switch ports. */
 389         for (port = 0; port < ds->num_ports; port++) {
 390                 if (!(ds->enabled_port_mask & (1 << port)))
 391                         continue;
 392
 393                 if (!ds->ports[port].netdev)
 394                         continue;
 395
 396                 dsa_slave_destroy(ds->ports[port].netdev);
 397         }
 398
 399         /* Disable configuration of the CPU and DSA ports */
 400         for (port = 0; port < ds->num_ports; port++) {
 401                 if (!(dsa_is_cpu_port(ds, port) || dsa_is_dsa_port(ds, port)))
 402                         continue;
 403                 dsa_cpu_dsa_destroy(&ds->ports[port]);
 404
 405                 /* Clearing a bit which is not set does no harm */
 406                 ds->cpu_port_mask |= ~(1 << port);
 407                 ds->dsa_port_mask |= ~(1 << port);
 408         }
 409
 410         if (ds->slave_mii_bus && ds->ops->phy_read)
 411                 mdiobus_unregister(ds->slave_mii_bus);
 412
 413         dsa_switch_unregister_notifier(ds);
 414 }
 415
 416 #ifdef CONFIG_PM_SLEEP
 417 int dsa_switch_suspend(struct dsa_switch *ds)
 418 {
 419         int i, ret = 0;
 420
 421         /* Suspend slave network devices */
 422         for (i = 0; i < ds->num_ports; i++) {
 423                 if (!dsa_is_port_initialized(ds, i))
 424                         continue;
 425
 426                 ret = dsa_slave_suspend(ds->ports[i].netdev);
 427                 if (ret)
 428                         return ret;
 429         }
 430
 431         if (ds->ops->suspend)
 432                 ret = ds->ops->suspend(ds);
 433
 434         return ret;
 435 }
 436 EXPORT_SYMBOL_GPL(dsa_switch_suspend);
 437
 438 int dsa_switch_resume(struct dsa_switch *ds)
 439 {
 440         int i, ret = 0;
 441
 442         if (ds->ops->resume)
 443                 ret = ds->ops->resume(ds);
 444
 445         if (ret)
 446                 return ret;
 447
 448         /* Resume slave network devices */
 449         for (i = 0; i < ds->num_ports; i++) {
 450                 if (!dsa_is_port_initialized(ds, i))
 451                         continue;
 452
 453                 ret = dsa_slave_resume(ds->ports[i].netdev);
 454                 if (ret)
 455                         return ret;
 456         }
 457
 458         return 0;
 459 }
 460 EXPORT_SYMBOL_GPL(dsa_switch_resume);
 461 #endif
 462
 463 /* platform driver init and cleanup *****************************************/
 464 static int dev_is_class(struct device *dev, void *class)
 465 {
 466         if (dev->class != NULL && !strcmp(dev->class->name, class))
 467                 return 1;
 468
 469         return 0;
 470 }
 471
 472 static struct device *dev_find_class(struct device *parent, char *class)
 473 {
 474         if (dev_is_class(parent, class)) {
 475                 get_device(parent);
 476                 return parent;
 477         }
 478
 479         return device_find_child(parent, class, dev_is_class);
 480 }
 481
 482 struct mii_bus *dsa_host_dev_to_mii_bus(struct device *dev)
 483 {
 484         struct device *d;
 485
 486         d = dev_find_class(dev, "mdio_bus");
 487         if (d != NULL) {
 488                 struct mii_bus *bus;
 489
 490                 bus = to_mii_bus(d);
 491                 put_device(d);
 492
 493                 return bus;
 494         }
 495
 496         return NULL;
 497 }
 498 EXPORT_SYMBOL_GPL(dsa_host_dev_to_mii_bus);
 499
 500 struct net_device *dsa_dev_to_net_device(struct device *dev)
 501 {
 502         struct device *d;
 503
 504         d = dev_find_class(dev, "net");
 505         if (d != NULL) {
 506                 struct net_device *nd;
 507
 508                 nd = to_net_dev(d);
 509                 dev_hold(nd);
 510                 put_device(d);
 511
 512                 return nd;
 513         }
 514
 515         return NULL;
 516 }
 517 EXPORT_SYMBOL_GPL(dsa_dev_to_net_device);
 518
 519 #ifdef CONFIG_OF
 520 static int dsa_of_setup_routing_table(struct dsa_platform_data *pd,
 521                                         struct dsa_chip_data *cd,
 522                                         int chip_index, int port_index,
 523                                         struct device_node *link)
 524 {
 525         const __be32 *reg;
 526         int link_sw_addr;
 527         struct device_node *parent_sw;
 528         int len;
 529
 530         parent_sw = of_get_parent(link);
 531         if (!parent_sw)
 532                 return -EINVAL;
 533
 534         reg = of_get_property(parent_sw, "reg", &len);
 535         if (!reg || (len != sizeof(*reg) * 2))
 536                 return -EINVAL;
 537
 538         /*
 539          * Get the destination switch number from the second field of its 'reg'
 540          * property, i.e. for "reg = <0x19 1>" sw_addr is '1'.
 541          */
 542         link_sw_addr = be32_to_cpup(reg + 1);
 543
 544         if (link_sw_addr >= pd->nr_chips)
 545                 return -EINVAL;
 546
 547         cd->rtable[link_sw_addr] = port_index;
 548
 549         return 0;
 550 }
 551
 552 static int dsa_of_probe_links(struct dsa_platform_data *pd,
 553                               struct dsa_chip_data *cd,
 554                               int chip_index, int port_index,
 555                               struct device_node *port,
 556                               const char *port_name)
 557 {
 558         struct device_node *link;
 559         int link_index;
 560         int ret;
 561
 562         for (link_index = 0;; link_index++) {
 563                 link = of_parse_phandle(port, "link", link_index);
 564                 if (!link)
 565                         break;
 566
 567                 if (!strcmp(port_name, "dsa") && pd->nr_chips > 1) {
 568                         ret = dsa_of_setup_routing_table(pd, cd, chip_index,
 569                                                          port_index, link);
 570                         if (ret)
 571                                 return ret;
 572                 }
 573         }
 574         return 0;
 575 }
 576
 577 static void dsa_of_free_platform_data(struct dsa_platform_data *pd)
 578 {
 579         int i;
 580         int port_index;
 581
 582         for (i = 0; i < pd->nr_chips; i++) {
 583                 port_index = 0;
 584                 while (port_index < DSA_MAX_PORTS) {
 585                         kfree(pd->chip[i].port_names[port_index]);
 586                         port_index++;
 587                 }
 588
 589                 /* Drop our reference to the MDIO bus device */
 590                 if (pd->chip[i].host_dev)
 591                         put_device(pd->chip[i].host_dev);
 592         }
 593         kfree(pd->chip);
 594 }
 595
 596 static int dsa_of_probe(struct device *dev)
 597 {
 598         struct device_node *np = dev->of_node;
 599         struct device_node *child, *mdio, *ethernet, *port;
 600         struct mii_bus *mdio_bus, *mdio_bus_switch;
 601         struct net_device *ethernet_dev;
 602         struct dsa_platform_data *pd;
 603         struct dsa_chip_data *cd;
 604         const char *port_name;
 605         int chip_index, port_index;
 606         const unsigned int *sw_addr, *port_reg;
 607         u32 eeprom_len;
 608         int ret;
 609
 610         mdio = of_parse_phandle(np, "dsa,mii-bus", 0);
 611         if (!mdio)
 612                 return -EINVAL;
 613
 614         mdio_bus = of_mdio_find_bus(mdio);
 615         if (!mdio_bus)
 616                 return -EPROBE_DEFER;
 617
 618         ethernet = of_parse_phandle(np, "dsa,ethernet", 0);
 619         if (!ethernet) {
 620                 ret = -EINVAL;
 621                 goto out_put_mdio;
 622         }
 623
 624         ethernet_dev = of_find_net_device_by_node(ethernet);
 625         if (!ethernet_dev) {
 626                 ret = -EPROBE_DEFER;
 627                 goto out_put_mdio;
 628         }
 629
 630         pd = kzalloc(sizeof(*pd), GFP_KERNEL);
 631         if (!pd) {
 632                 ret = -ENOMEM;
 633                 goto out_put_ethernet;
 634         }
 635
 636         dev->platform_data = pd;
 637         pd->of_netdev = ethernet_dev;
 638         pd->nr_chips = of_get_available_child_count(np);
 639         if (pd->nr_chips > DSA_MAX_SWITCHES)
 640                 pd->nr_chips = DSA_MAX_SWITCHES;
 641
 642         pd->chip = kcalloc(pd->nr_chips, sizeof(struct dsa_chip_data),
 643                            GFP_KERNEL);
 644         if (!pd->chip) {
 645                 ret = -ENOMEM;
 646                 goto out_free;
 647         }
 648
 649         chip_index = -1;
 650         for_each_available_child_of_node(np, child) {
 651                 int i;
 652
 653                 chip_index++;
 654                 cd = &pd->chip[chip_index];
 655
 656                 cd->of_node = child;
 657
 658                 /* Initialize the routing table */
 659                 for (i = 0; i < DSA_MAX_SWITCHES; ++i)
 660                         cd->rtable[i] = DSA_RTABLE_NONE;
 661
 662                 /* When assigning the host device, increment its refcount */
 663                 cd->host_dev = get_device(&mdio_bus->dev);
 664
 665                 sw_addr = of_get_property(child, "reg", NULL);
 666                 if (!sw_addr)
 667                         continue;
 668
 669                 cd->sw_addr = be32_to_cpup(sw_addr);
 670                 if (cd->sw_addr >= PHY_MAX_ADDR)
 671                         continue;
 672
 673                 if (!of_property_read_u32(child, "eeprom-length", &eeprom_len))
 674                         cd->eeprom_len = eeprom_len;
 675
 676                 mdio = of_parse_phandle(child, "mii-bus", 0);
 677                 if (mdio) {
 678                         mdio_bus_switch = of_mdio_find_bus(mdio);
 679                         if (!mdio_bus_switch) {
 680                                 ret = -EPROBE_DEFER;
 681                                 goto out_free_chip;
 682                         }
 683
 684                         /* Drop the mdio_bus device ref, replacing the host
 685                          * device with the mdio_bus_switch device, keeping
 686                          * the refcount from of_mdio_find_bus() above.
 687                          */
 688                         put_device(cd->host_dev);
 689                         cd->host_dev = &mdio_bus_switch->dev;
 690                 }
 691
 692                 for_each_available_child_of_node(child, port) {
 693                         port_reg = of_get_property(port, "reg", NULL);
 694                         if (!port_reg)
 695                                 continue;
 696
 697                         port_index = be32_to_cpup(port_reg);
 698                         if (port_index >= DSA_MAX_PORTS)
 699                                 break;
 700
 701                         port_name = of_get_property(port, "label", NULL);
 702                         if (!port_name)
 703                                 continue;
 704
 705                         cd->port_dn[port_index] = port;
 706
 707                         cd->port_names[port_index] = kstrdup(port_name,
 708                                         GFP_KERNEL);
 709                         if (!cd->port_names[port_index]) {
 710                                 ret = -ENOMEM;
 711                                 goto out_free_chip;
 712                         }
 713
 714                         ret = dsa_of_probe_links(pd, cd, chip_index,
 715                                                  port_index, port, port_name);
 716                         if (ret)
 717                                 goto out_free_chip;
 718
 719                 }
 720         }
 721
 722         /* The individual chips hold their own refcount on the mdio bus,
 723          * so drop ours */
 724         put_device(&mdio_bus->dev);
 725
 726         return 0;
 727
 728 out_free_chip:
 729         dsa_of_free_platform_data(pd);
 730 out_free:
 731         kfree(pd);
 732         dev->platform_data = NULL;
 733 out_put_ethernet:
 734         put_device(&ethernet_dev->dev);
 735 out_put_mdio:
 736         put_device(&mdio_bus->dev);
 737         return ret;
 738 }
 739
 740 static void dsa_of_remove(struct device *dev)
 741 {
 742         struct dsa_platform_data *pd = dev->platform_data;
 743
 744         if (!dev->of_node)
 745                 return;
 746
 747         dsa_of_free_platform_data(pd);
 748         put_device(&pd->of_netdev->dev);
 749         kfree(pd);
 750 }
 751 #else
 752 static inline int dsa_of_probe(struct device *dev)
 753 {
 754         return 0;
 755 }
 756
 757 static inline void dsa_of_remove(struct device *dev)
 758 {
 759 }
 760 #endif
 761
 762 static int dsa_setup_dst(struct dsa_switch_tree *dst, struct net_device *dev,
 763                          struct device *parent, struct dsa_platform_data *pd)
 764 {
 765         int i;
 766         unsigned configured = 0;
 767
 768         dst->pd = pd;
 769         dst->master_netdev = dev;
 770         dst->cpu_port = -1;
 771
 772         for (i = 0; i < pd->nr_chips; i++) {
 773                 struct dsa_switch *ds;
 774
 775                 ds = dsa_switch_setup(dst, i, parent, pd->chip[i].host_dev);
 776                 if (IS_ERR(ds)) {
 777                         netdev_err(dev, "[%d]: couldn't create dsa switch instance (error %ld)\n",
 778                                    i, PTR_ERR(ds));
 779                         continue;
 780                 }
 781
 782                 dst->ds[i] = ds;
 783
 784                 ++configured;
 785         }
 786
 787         /*
 788          * If no switch was found, exit cleanly
 789          */
 790         if (!configured)
 791                 return -EPROBE_DEFER;
 792
 793         /*
 794          * If we use a tagging format that doesn't have an ethertype
 795          * field, make sure that all packets from this point on get
 796          * sent to the tag format's receive function.
 797          */
 798         wmb();
 799         dev->dsa_ptr = (void *)dst;
 800
 801         return 0;
 802 }
 803
 804 static int dsa_probe(struct platform_device *pdev)
 805 {
 806         struct dsa_platform_data *pd = pdev->dev.platform_data;
 807         struct net_device *dev;
 808         struct dsa_switch_tree *dst;
 809         int ret;
 810
 811         if (pdev->dev.of_node) {
 812                 ret = dsa_of_probe(&pdev->dev);
 813                 if (ret)
 814                         return ret;
 815
 816                 pd = pdev->dev.platform_data;
 817         }
 818
 819         if (pd == NULL || (pd->netdev == NULL && pd->of_netdev == NULL))
 820                 return -EINVAL;
 821
 822         if (pd->of_netdev) {
 823                 dev = pd->of_netdev;
 824                 dev_hold(dev);
 825         } else {
 826                 dev = dsa_dev_to_net_device(pd->netdev);
 827         }
 828         if (dev == NULL) {
 829                 ret = -EPROBE_DEFER;
 830                 goto out;
 831         }
 832
 833         if (dev->dsa_ptr != NULL) {
 834                 dev_put(dev);
 835                 ret = -EEXIST;
 836                 goto out;
 837         }
 838
 839         dst = devm_kzalloc(&pdev->dev, sizeof(*dst), GFP_KERNEL);
 840         if (dst == NULL) {
 841                 dev_put(dev);
 842                 ret = -ENOMEM;
 843                 goto out;
 844         }
 845
 846         platform_set_drvdata(pdev, dst);
 847
 848         ret = dsa_setup_dst(dst, dev, &pdev->dev, pd);
 849         if (ret) {
 850                 dev_put(dev);
 851                 goto out;
 852         }
 853
 854         return 0;
 855
 856 out:
 857         dsa_of_remove(&pdev->dev);
 858
 859         return ret;
 860 }
 861
 862 static void dsa_remove_dst(struct dsa_switch_tree *dst)
 863 {
 864         int i;
 865
 866         dst->master_netdev->dsa_ptr = NULL;
 867
 868         /* If we used a tagging format that doesn't have an ethertype
 869          * field, make sure that all packets from this point get sent
 870          * without the tag and go through the regular receive path.
 871          */
 872         wmb();
 873
 874         for (i = 0; i < dst->pd->nr_chips; i++) {
 875                 struct dsa_switch *ds = dst->ds[i];
 876
 877                 if (ds)
 878                         dsa_switch_destroy(ds);
 879         }
 880
 881         dsa_cpu_port_ethtool_restore(dst->cpu_switch);
 882
 883         dev_put(dst->master_netdev);
 884 }
 885
 886 static int dsa_remove(struct platform_device *pdev)
 887 {
 888         struct dsa_switch_tree *dst = platform_get_drvdata(pdev);
 889
 890         dsa_remove_dst(dst);
 891         dsa_of_remove(&pdev->dev);
 892
 893         return 0;
 894 }
 895
 896 static void dsa_shutdown(struct platform_device *pdev)
 897 {
 898 }
 899
 900 static int dsa_switch_rcv(struct sk_buff *skb, struct net_device *dev,
 901                           struct packet_type *pt, struct net_device *orig_dev)
 902 {
 903         struct dsa_switch_tree *dst = dev->dsa_ptr;
 904         struct sk_buff *nskb = NULL;
 905
 906         if (unlikely(dst == NULL)) {
 907                 kfree_skb(skb);
 908                 return 0;
 909         }
 910
 911         skb = skb_unshare(skb, GFP_ATOMIC);
 912         if (!skb)
 913                 return 0;
 914
 915         nskb = dst->rcv(skb, dev, pt, orig_dev);
 916         if (!nskb) {
 917                 kfree_skb(skb);
 918                 return 0;
 919         }
 920
 921         skb = nskb;
 922         skb_push(skb, ETH_HLEN);
 923         skb->pkt_type = PACKET_HOST;
 924         skb->protocol = eth_type_trans(skb, skb->dev);
 925
 926         skb->dev->stats.rx_packets++;
 927         skb->dev->stats.rx_bytes += skb->len;
 928
 929         netif_receive_skb(skb);
 930
 931         return 0;
 932 }
 933
 934 static struct packet_type dsa_pack_type __read_mostly = {
 935         .type   = cpu_to_be16(ETH_P_XDSA),
 936         .func   = dsa_switch_rcv,
 937 };
 938
 939 #ifdef CONFIG_PM_SLEEP
 940 static int dsa_suspend(struct device *d)
 941 {
 942         struct platform_device *pdev = to_platform_device(d);
 943         struct dsa_switch_tree *dst = platform_get_drvdata(pdev);
 944         int i, ret = 0;
 945
 946         for (i = 0; i < dst->pd->nr_chips; i++) {
 947                 struct dsa_switch *ds = dst->ds[i];
 948
 949                 if (ds != NULL)
 950                         ret = dsa_switch_suspend(ds);
 951         }
 952
 953         return ret;
 954 }
 955
 956 static int dsa_resume(struct device *d)
 957 {
 958         struct platform_device *pdev = to_platform_device(d);
 959         struct dsa_switch_tree *dst = platform_get_drvdata(pdev);
 960         int i, ret = 0;
 961
 962         for (i = 0; i < dst->pd->nr_chips; i++) {
 963                 struct dsa_switch *ds = dst->ds[i];
 964
 965                 if (ds != NULL)
 966                         ret = dsa_switch_resume(ds);
 967         }
 968
 969         return ret;
 970 }
 971 #endif
 972
 973 static SIMPLE_DEV_PM_OPS(dsa_pm_ops, dsa_suspend, dsa_resume);
 974
 975 static const struct of_device_id dsa_of_match_table[] = {
 976         { .compatible = "marvell,dsa", },
 977         {}
 978 };
 979 MODULE_DEVICE_TABLE(of, dsa_of_match_table);
 980
 981 static struct platform_driver dsa_driver = {
 982         .probe          = dsa_probe,
 983         .remove         = dsa_remove,
 984         .shutdown       = dsa_shutdown,
 985         .driver = {
 986                 .name   = "dsa",
 987                 .of_match_table = dsa_of_match_table,
 988                 .pm     = &dsa_pm_ops,
 989         },
 990 };
 991
 992 static int __init dsa_init_module(void)
 993 {
 994         int rc;
 995
 996         rc = dsa_slave_register_notifier();
 997         if (rc)
 998                 return rc;
 999
1000         rc = platform_driver_register(&dsa_driver);
1001         if (rc)
1002                 return rc;
1003
1004         dev_add_pack(&dsa_pack_type);
1005
1006         return 0;
1007 }
1008 module_init(dsa_init_module);
1009
1010 static void __exit dsa_cleanup_module(void)
1011 {
1012         dsa_slave_unregister_notifier();
1013         dev_remove_pack(&dsa_pack_type);
1014         platform_driver_unregister(&dsa_driver);
1015 }
1016 module_exit(dsa_cleanup_module);
1017
1018 MODULE_AUTHOR("Lennert Buytenhek <buytenh@wantstofly.org>");
1019 MODULE_DESCRIPTION("Driver for Distributed Switch Architecture switch chips");
1020 MODULE_LICENSE("GPL");
1021 MODULE_ALIAS("platform:dsa");