drivers/net/mdio/mdio-mux.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2011, 2012 Cavium, Inc.
   4  */
   5
   6 #include <linux/device.h>
   7 #include <linux/mdio-mux.h>
   8 #include <linux/module.h>
   9 #include <linux/of_mdio.h>
  10 #include <linux/phy.h>
  11 #include <linux/platform_device.h>
  12
  13 #define DRV_DESCRIPTION "MDIO bus multiplexer driver"
  14
  15 struct mdio_mux_child_bus;
  16
  17 struct 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
  28 struct 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  */
  38 static 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);
  52 out:
  53         mutex_unlock(&pb->mii_bus->mdio_lock);
  54
  55         return r;
  56 }
  57
  58 static int mdio_mux_read_c45(struct mii_bus *bus, int phy_id, int dev_addr,
  59                              int regnum)
  60 {
  61         struct mdio_mux_child_bus *cb = bus->priv;
  62         struct mdio_mux_parent_bus *pb = cb->parent;
  63         int r;
  64
  65         mutex_lock_nested(&pb->mii_bus->mdio_lock, MDIO_MUTEX_MUX);
  66         r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
  67         if (r)
  68                 goto out;
  69
  70         pb->current_child = cb->bus_number;
  71
  72         r = pb->mii_bus->read_c45(pb->mii_bus, phy_id, dev_addr, regnum);
  73 out:
  74         mutex_unlock(&pb->mii_bus->mdio_lock);
  75
  76         return r;
  77 }
  78
  79 /*
  80  * The parent bus' lock is used to order access to the switch_fn.
  81  */
  82 static int mdio_mux_write(struct mii_bus *bus, int phy_id,
  83                           int regnum, u16 val)
  84 {
  85         struct mdio_mux_child_bus *cb = bus->priv;
  86         struct mdio_mux_parent_bus *pb = cb->parent;
  87
  88         int r;
  89
  90         mutex_lock_nested(&pb->mii_bus->mdio_lock, MDIO_MUTEX_MUX);
  91         r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
  92         if (r)
  93                 goto out;
  94
  95         pb->current_child = cb->bus_number;
  96
  97         r = pb->mii_bus->write(pb->mii_bus, phy_id, regnum, val);
  98 out:
  99         mutex_unlock(&pb->mii_bus->mdio_lock);
 100
 101         return r;
 102 }
 103
 104 static int mdio_mux_write_c45(struct mii_bus *bus, int phy_id, int dev_addr,
 105                               int regnum, u16 val)
 106 {
 107         struct mdio_mux_child_bus *cb = bus->priv;
 108         struct mdio_mux_parent_bus *pb = cb->parent;
 109
 110         int r;
 111
 112         mutex_lock_nested(&pb->mii_bus->mdio_lock, MDIO_MUTEX_MUX);
 113         r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
 114         if (r)
 115                 goto out;
 116
 117         pb->current_child = cb->bus_number;
 118
 119         r = pb->mii_bus->write_c45(pb->mii_bus, phy_id, dev_addr, regnum, val);
 120 out:
 121         mutex_unlock(&pb->mii_bus->mdio_lock);
 122
 123         return r;
 124 }
 125
 126 static int parent_count;
 127
 128 static void mdio_mux_uninit_children(struct mdio_mux_parent_bus *pb)
 129 {
 130         struct mdio_mux_child_bus *cb = pb->children;
 131
 132         while (cb) {
 133                 mdiobus_unregister(cb->mii_bus);
 134                 mdiobus_free(cb->mii_bus);
 135                 cb = cb->next;
 136         }
 137 }
 138
 139 int mdio_mux_init(struct device *dev,
 140                   struct device_node *mux_node,
 141                   int (*switch_fn)(int cur, int desired, void *data),
 142                   void **mux_handle,
 143                   void *data,
 144                   struct mii_bus *mux_bus)
 145 {
 146         struct device_node *parent_bus_node;
 147         struct device_node *child_bus_node;
 148         int r, ret_val;
 149         struct mii_bus *parent_bus;
 150         struct mdio_mux_parent_bus *pb;
 151         struct mdio_mux_child_bus *cb;
 152
 153         if (!mux_node)
 154                 return -ENODEV;
 155
 156         if (!mux_bus) {
 157                 parent_bus_node = of_parse_phandle(mux_node,
 158                                                    "mdio-parent-bus", 0);
 159
 160                 if (!parent_bus_node)
 161                         return -ENODEV;
 162
 163                 parent_bus = of_mdio_find_bus(parent_bus_node);
 164                 if (!parent_bus) {
 165                         ret_val = -EPROBE_DEFER;
 166                         goto err_parent_bus;
 167                 }
 168         } else {
 169                 parent_bus_node = NULL;
 170                 parent_bus = mux_bus;
 171                 get_device(&parent_bus->dev);
 172         }
 173
 174         pb = devm_kzalloc(dev, sizeof(*pb), GFP_KERNEL);
 175         if (!pb) {
 176                 ret_val = -ENOMEM;
 177                 goto err_pb_kz;
 178         }
 179
 180         pb->switch_data = data;
 181         pb->switch_fn = switch_fn;
 182         pb->current_child = -1;
 183         pb->parent_id = parent_count++;
 184         pb->mii_bus = parent_bus;
 185
 186         ret_val = -ENODEV;
 187         for_each_available_child_of_node(mux_node, child_bus_node) {
 188                 int v;
 189
 190                 r = of_property_read_u32(child_bus_node, "reg", &v);
 191                 if (r) {
 192                         dev_err(dev,
 193                                 "Error: Failed to find reg for child %pOF\n",
 194                                 child_bus_node);
 195                         continue;
 196                 }
 197
 198                 cb = devm_kzalloc(dev, sizeof(*cb), GFP_KERNEL);
 199                 if (!cb) {
 200                         ret_val = -ENOMEM;
 201                         goto err_loop;
 202                 }
 203                 cb->bus_number = v;
 204                 cb->parent = pb;
 205
 206                 cb->mii_bus = mdiobus_alloc();
 207                 if (!cb->mii_bus) {
 208                         ret_val = -ENOMEM;
 209                         goto err_loop;
 210                 }
 211                 cb->mii_bus->priv = cb;
 212
 213                 cb->mii_bus->name = "mdio_mux";
 214                 snprintf(cb->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x.%x",
 215                          cb->mii_bus->name, pb->parent_id, v);
 216                 cb->mii_bus->parent = dev;
 217                 cb->mii_bus->read = mdio_mux_read;
 218                 cb->mii_bus->write = mdio_mux_write;
 219                 if (parent_bus->read_c45)
 220                         cb->mii_bus->read_c45 = mdio_mux_read_c45;
 221                 if (parent_bus->write_c45)
 222                         cb->mii_bus->write_c45 = mdio_mux_write_c45;
 223                 r = of_mdiobus_register(cb->mii_bus, child_bus_node);
 224                 if (r) {
 225                         mdiobus_free(cb->mii_bus);
 226                         if (r == -EPROBE_DEFER) {
 227                                 ret_val = r;
 228                                 goto err_loop;
 229                         }
 230                         devm_kfree(dev, cb);
 231                         dev_err(dev,
 232                                 "Error: Failed to register MDIO bus for child %pOF\n",
 233                                 child_bus_node);
 234                 } else {
 235                         cb->next = pb->children;
 236                         pb->children = cb;
 237                 }
 238         }
 239         if (pb->children) {
 240                 *mux_handle = pb;
 241                 return 0;
 242         }
 243
 244         dev_err(dev, "Error: No acceptable child buses found\n");
 245
 246 err_loop:
 247         mdio_mux_uninit_children(pb);
 248         of_node_put(child_bus_node);
 249 err_pb_kz:
 250         put_device(&parent_bus->dev);
 251 err_parent_bus:
 252         of_node_put(parent_bus_node);
 253         return ret_val;
 254 }
 255 EXPORT_SYMBOL_GPL(mdio_mux_init);
 256
 257 void mdio_mux_uninit(void *mux_handle)
 258 {
 259         struct mdio_mux_parent_bus *pb = mux_handle;
 260
 261         mdio_mux_uninit_children(pb);
 262         put_device(&pb->mii_bus->dev);
 263 }
 264 EXPORT_SYMBOL_GPL(mdio_mux_uninit);
 265
 266 MODULE_DESCRIPTION(DRV_DESCRIPTION);
 267 MODULE_AUTHOR("David Daney");
 268 MODULE_LICENSE("GPL v2");