net/mdio/mdio-mux.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (C) 2011, 2012 Cavium, Inc.
0004  */
0005
0006 #include <linux/device.h>
0007 #include <linux/mdio-mux.h>
0008 #include <linux/module.h>
0009 #include <linux/of_mdio.h>
0010 #include <linux/phy.h>
0011 #include <linux/platform_device.h>
0012
0013 #define DRV_DESCRIPTION "MDIO bus multiplexer driver"
0014
0015 struct mdio_mux_child_bus;
0016
0017 struct mdio_mux_parent_bus {
0018     struct mii_bus *mii_bus;
0019     int current_child;
0020     int parent_id;
0021     void *switch_data;
0022     int (*switch_fn)(int current_child, int desired_child, void *data);
0023
0024     /* List of our children linked through their next fields. */
0025     struct mdio_mux_child_bus *children;
0026 };
0027
0028 struct mdio_mux_child_bus {
0029     struct mii_bus *mii_bus;
0030     struct mdio_mux_parent_bus *parent;
0031     struct mdio_mux_child_bus *next;
0032     int bus_number;
0033 };
0034
0035 /*
0036  * The parent bus' lock is used to order access to the switch_fn.
0037  */
0038 static int mdio_mux_read(struct mii_bus *bus, int phy_id, int regnum)
0039 {
0040     struct mdio_mux_child_bus *cb = bus->priv;
0041     struct mdio_mux_parent_bus *pb = cb->parent;
0042     int r;
0043
0044     mutex_lock_nested(&pb->mii_bus->mdio_lock, MDIO_MUTEX_MUX);
0045     r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
0046     if (r)
0047         goto out;
0048
0049     pb->current_child = cb->bus_number;
0050
0051     r = pb->mii_bus->read(pb->mii_bus, phy_id, regnum);
0052 out:
0053     mutex_unlock(&pb->mii_bus->mdio_lock);
0054
0055     return r;
0056 }
0057
0058 /*
0059  * The parent bus' lock is used to order access to the switch_fn.
0060  */
0061 static int mdio_mux_write(struct mii_bus *bus, int phy_id,
0062               int regnum, u16 val)
0063 {
0064     struct mdio_mux_child_bus *cb = bus->priv;
0065     struct mdio_mux_parent_bus *pb = cb->parent;
0066
0067     int r;
0068
0069     mutex_lock_nested(&pb->mii_bus->mdio_lock, MDIO_MUTEX_MUX);
0070     r = pb->switch_fn(pb->current_child, cb->bus_number, pb->switch_data);
0071     if (r)
0072         goto out;
0073
0074     pb->current_child = cb->bus_number;
0075
0076     r = pb->mii_bus->write(pb->mii_bus, phy_id, regnum, val);
0077 out:
0078     mutex_unlock(&pb->mii_bus->mdio_lock);
0079
0080     return r;
0081 }
0082
0083 static int parent_count;
0084
0085 static void mdio_mux_uninit_children(struct mdio_mux_parent_bus *pb)
0086 {
0087     struct mdio_mux_child_bus *cb = pb->children;
0088
0089     while (cb) {
0090         mdiobus_unregister(cb->mii_bus);
0091         mdiobus_free(cb->mii_bus);
0092         cb = cb->next;
0093     }
0094 }
0095
0096 int mdio_mux_init(struct device *dev,
0097           struct device_node *mux_node,
0098           int (*switch_fn)(int cur, int desired, void *data),
0099           void **mux_handle,
0100           void *data,
0101           struct mii_bus *mux_bus)
0102 {
0103     struct device_node *parent_bus_node;
0104     struct device_node *child_bus_node;
0105     int r, ret_val;
0106     struct mii_bus *parent_bus;
0107     struct mdio_mux_parent_bus *pb;
0108     struct mdio_mux_child_bus *cb;
0109
0110     if (!mux_node)
0111         return -ENODEV;
0112
0113     if (!mux_bus) {
0114         parent_bus_node = of_parse_phandle(mux_node,
0115                            "mdio-parent-bus", 0);
0116
0117         if (!parent_bus_node)
0118             return -ENODEV;
0119
0120         parent_bus = of_mdio_find_bus(parent_bus_node);
0121         if (!parent_bus) {
0122             ret_val = -EPROBE_DEFER;
0123             goto err_parent_bus;
0124         }
0125     } else {
0126         parent_bus_node = NULL;
0127         parent_bus = mux_bus;
0128         get_device(&parent_bus->dev);
0129     }
0130
0131     pb = devm_kzalloc(dev, sizeof(*pb), GFP_KERNEL);
0132     if (!pb) {
0133         ret_val = -ENOMEM;
0134         goto err_pb_kz;
0135     }
0136
0137     pb->switch_data = data;
0138     pb->switch_fn = switch_fn;
0139     pb->current_child = -1;
0140     pb->parent_id = parent_count++;
0141     pb->mii_bus = parent_bus;
0142
0143     ret_val = -ENODEV;
0144     for_each_available_child_of_node(mux_node, child_bus_node) {
0145         int v;
0146
0147         r = of_property_read_u32(child_bus_node, "reg", &v);
0148         if (r) {
0149             dev_err(dev,
0150                 "Error: Failed to find reg for child %pOF\n",
0151                 child_bus_node);
0152             continue;
0153         }
0154
0155         cb = devm_kzalloc(dev, sizeof(*cb), GFP_KERNEL);
0156         if (!cb) {
0157             ret_val = -ENOMEM;
0158             goto err_loop;
0159         }
0160         cb->bus_number = v;
0161         cb->parent = pb;
0162
0163         cb->mii_bus = mdiobus_alloc();
0164         if (!cb->mii_bus) {
0165             ret_val = -ENOMEM;
0166             goto err_loop;
0167         }
0168         cb->mii_bus->priv = cb;
0169
0170         cb->mii_bus->name = "mdio_mux";
0171         snprintf(cb->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x.%x",
0172              cb->mii_bus->name, pb->parent_id, v);
0173         cb->mii_bus->parent = dev;
0174         cb->mii_bus->read = mdio_mux_read;
0175         cb->mii_bus->write = mdio_mux_write;
0176         r = of_mdiobus_register(cb->mii_bus, child_bus_node);
0177         if (r) {
0178             mdiobus_free(cb->mii_bus);
0179             if (r == -EPROBE_DEFER) {
0180                 ret_val = r;
0181                 goto err_loop;
0182             }
0183             devm_kfree(dev, cb);
0184             dev_err(dev,
0185                 "Error: Failed to register MDIO bus for child %pOF\n",
0186                 child_bus_node);
0187         } else {
0188             cb->next = pb->children;
0189             pb->children = cb;
0190         }
0191     }
0192     if (pb->children) {
0193         *mux_handle = pb;
0194         return 0;
0195     }
0196
0197     dev_err(dev, "Error: No acceptable child buses found\n");
0198
0199 err_loop:
0200     mdio_mux_uninit_children(pb);
0201     of_node_put(child_bus_node);
0202 err_pb_kz:
0203     put_device(&parent_bus->dev);
0204 err_parent_bus:
0205     of_node_put(parent_bus_node);
0206     return ret_val;
0207 }
0208 EXPORT_SYMBOL_GPL(mdio_mux_init);
0209
0210 void mdio_mux_uninit(void *mux_handle)
0211 {
0212     struct mdio_mux_parent_bus *pb = mux_handle;
0213
0214     mdio_mux_uninit_children(pb);
0215     put_device(&pb->mii_bus->dev);
0216 }
0217 EXPORT_SYMBOL_GPL(mdio_mux_uninit);
0218
0219 MODULE_DESCRIPTION(DRV_DESCRIPTION);
0220 MODULE_AUTHOR("David Daney");
0221 MODULE_LICENSE("GPL v2");