linux/drivers/net/mdio/mdio-mux.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * Copyright (C) 2011, 2012 Cavium, Inc.
   4 */
   5
   6#include <linux/platform_device.h>
   7#include <linux/mdio-mux.h>
   8#include <linux/of_mdio.h>
   9#include <linux/device.h>
  10#include <linux/module.h>
  11#include <linux/phy.h>
  12
  13#define DRV_DESCRIPTION "MDIO bus multiplexer driver"
  14
  15struct mdio_mux_child_bus;
  16
  17struct mdio_mux_parent_bus {
  18        struct mii_bus *mii_bus;
  19        int current_child;
  20        int parent_id;
  21        void *switch_data;
  22        int (*switch_fn)(int current_child, int desired_child, void *data);
  23
  24        /* List of our children linked through their next fields. */
  25        struct mdio_mux_child_bus *children;
  26};
  27
  28struct mdio_mux_child_bus {
  29        struct mii_bus *mii_bus;
  30        struct mdio_mux_parent_bus *parent;
  31        struct mdio_mux_child_bus *next;
  32        int bus_number;
  33};
  34
  35/*
  36 * The parent bus' lock is used to order access to the switch_fn.
  37 */
  38static int mdio_mux_read(struct mii_bus *bus, int phy_id, int regnum)
  39{
  40        struct mdio_mux_child_bus *cb = bus->priv;
  41        struct mdio_mux_parent_bus *pb = cb->parent;
  42        int r;
  43
  44        mutex_lock_nested(&pb->mii_bus->mdio_lock, MDIO_MUTEX_MUX);
  45        r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
  46        if (r)
  47                goto out;
  48
  49        pb->current_child = cb->bus_number;
  50
  51        r = pb->mii_bus->read(pb->mii_bus, phy_id, regnum);
  52out:
  53        mutex_unlock(&pb->mii_bus->mdio_lock);
  54
  55        return r;
  56}
  57
  58/*
  59 * The parent bus' lock is used to order access to the switch_fn.
  60 */
  61static int mdio_mux_write(struct mii_bus *bus, int phy_id,
  62                          int regnum, u16 val)
  63{
  64        struct mdio_mux_child_bus *cb = bus->priv;
  65        struct mdio_mux_parent_bus *pb = cb->parent;
  66
  67        int r;
  68
  69        mutex_lock_nested(&pb->mii_bus->mdio_lock, MDIO_MUTEX_MUX);
  70        r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
  71        if (r)
  72                goto out;
  73
  74        pb->current_child = cb->bus_number;
  75
  76        r = pb->mii_bus->write(pb->mii_bus, phy_id, regnum, val);
  77out:
  78        mutex_unlock(&pb->mii_bus->mdio_lock);
  79
  80        return r;
  81}
  82
  83static int parent_count;
  84
  85int mdio_mux_init(struct device *dev,
  86                  struct device_node *mux_node,
  87                  int (*switch_fn)(int cur, int desired, void *data),
  88                  void **mux_handle,
  89                  void *data,
  90                  struct mii_bus *mux_bus)
  91{
  92        struct device_node *parent_bus_node;
  93        struct device_node *child_bus_node;
  94        int r, ret_val;
  95        struct mii_bus *parent_bus;
  96        struct mdio_mux_parent_bus *pb;
  97        struct mdio_mux_child_bus *cb;
  98
  99        if (!mux_node)
 100                return -ENODEV;
 101
 102        if (!mux_bus) {
 103                parent_bus_node = of_parse_phandle(mux_node,
 104                                                   "mdio-parent-bus", 0);
 105
 106                if (!parent_bus_node)
 107                        return -ENODEV;
 108
 109                parent_bus = of_mdio_find_bus(parent_bus_node);
 110                if (!parent_bus) {
 111                        ret_val = -EPROBE_DEFER;
 112                        goto err_parent_bus;
 113                }
 114        } else {
 115                parent_bus_node = NULL;
 116                parent_bus = mux_bus;
 117                get_device(&parent_bus->dev);
 118        }
 119
 120        pb = devm_kzalloc(dev, sizeof(*pb), GFP_KERNEL);
 121        if (!pb) {
 122                ret_val = -ENOMEM;
 123                goto err_pb_kz;
 124        }
 125
 126        pb->switch_data = data;
 127        pb->switch_fn = switch_fn;
 128        pb->current_child = -1;
 129        pb->parent_id = parent_count++;
 130        pb->mii_bus = parent_bus;
 131
 132        ret_val = -ENODEV;
 133        for_each_available_child_of_node(mux_node, child_bus_node) {
 134                int v;
 135
 136                r = of_property_read_u32(child_bus_node, "reg", &v);
 137                if (r) {
 138                        dev_err(dev,
 139                                "Error: Failed to find reg for child %pOF\n",
 140                                child_bus_node);
 141                        continue;
 142                }
 143
 144                cb = devm_kzalloc(dev, sizeof(*cb), GFP_KERNEL);
 145                if (!cb) {
 146                        ret_val = -ENOMEM;
 147                        continue;
 148                }
 149                cb->bus_number = v;
 150                cb->parent = pb;
 151
 152                cb->mii_bus = mdiobus_alloc();
 153                if (!cb->mii_bus) {
 154                        ret_val = -ENOMEM;
 155                        devm_kfree(dev, cb);
 156                        continue;
 157                }
 158                cb->mii_bus->priv = cb;
 159
 160                cb->mii_bus->name = "mdio_mux";
 161                snprintf(cb->mii_bus->id, MII_BUS_ID_SIZE, "%x.%x",
 162                         pb->parent_id, v);
 163                cb->mii_bus->parent = dev;
 164                cb->mii_bus->read = mdio_mux_read;
 165                cb->mii_bus->write = mdio_mux_write;
 166                r = of_mdiobus_register(cb->mii_bus, child_bus_node);
 167                if (r) {
 168                        dev_err(dev,
 169                                "Error: Failed to register MDIO bus for child %pOF\n",
 170                                child_bus_node);
 171                        mdiobus_free(cb->mii_bus);
 172                        devm_kfree(dev, cb);
 173                } else {
 174                        cb->next = pb->children;
 175                        pb->children = cb;
 176                }
 177        }
 178        if (pb->children) {
 179                *mux_handle = pb;
 180                return 0;
 181        }
 182
 183        dev_err(dev, "Error: No acceptable child buses found\n");
 184        devm_kfree(dev, pb);
 185err_pb_kz:
 186        put_device(&parent_bus->dev);
 187err_parent_bus:
 188        of_node_put(parent_bus_node);
 189        return ret_val;
 190}
 191EXPORT_SYMBOL_GPL(mdio_mux_init);
 192
 193void mdio_mux_uninit(void *mux_handle)
 194{
 195        struct mdio_mux_parent_bus *pb = mux_handle;
 196        struct mdio_mux_child_bus *cb = pb->children;
 197
 198        while (cb) {
 199                mdiobus_unregister(cb->mii_bus);
 200                mdiobus_free(cb->mii_bus);
 201                cb = cb->next;
 202        }
 203
 204        put_device(&pb->mii_bus->dev);
 205}
 206EXPORT_SYMBOL_GPL(mdio_mux_uninit);
 207
 208MODULE_DESCRIPTION(DRV_DESCRIPTION);
 209MODULE_AUTHOR("David Daney");
 210MODULE_LICENSE("GPL v2");
 211