271 lines · c
1// SPDX-License-Identifier: GPL-2.02/*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 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 58static 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);73out: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 */82static 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);98out:99 mutex_unlock(&pb->mii_bus->mdio_lock);100 101 return r;102}103 104static 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);120out:121 mutex_unlock(&pb->mii_bus->mdio_lock);122 123 return r;124}125 126static int parent_count;127 128static 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 139int 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: %pe\n",194 child_bus_node, ERR_PTR(r));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 if (parent_bus->read)218 cb->mii_bus->read = mdio_mux_read;219 if (parent_bus->write)220 cb->mii_bus->write = mdio_mux_write;221 if (parent_bus->read_c45)222 cb->mii_bus->read_c45 = mdio_mux_read_c45;223 if (parent_bus->write_c45)224 cb->mii_bus->write_c45 = mdio_mux_write_c45;225 r = of_mdiobus_register(cb->mii_bus, child_bus_node);226 if (r) {227 mdiobus_free(cb->mii_bus);228 if (r == -EPROBE_DEFER) {229 ret_val = r;230 goto err_loop;231 }232 devm_kfree(dev, cb);233 dev_err(dev,234 "Error: Failed to register MDIO bus for child %pOF: %pe\n",235 child_bus_node, ERR_PTR(r));236 } else {237 cb->next = pb->children;238 pb->children = cb;239 }240 }241 if (pb->children) {242 *mux_handle = pb;243 return 0;244 }245 246 dev_err(dev, "Error: No acceptable child buses found\n");247 248err_loop:249 mdio_mux_uninit_children(pb);250 of_node_put(child_bus_node);251err_pb_kz:252 put_device(&parent_bus->dev);253err_parent_bus:254 of_node_put(parent_bus_node);255 return ret_val;256}257EXPORT_SYMBOL_GPL(mdio_mux_init);258 259void mdio_mux_uninit(void *mux_handle)260{261 struct mdio_mux_parent_bus *pb = mux_handle;262 263 mdio_mux_uninit_children(pb);264 put_device(&pb->mii_bus->dev);265}266EXPORT_SYMBOL_GPL(mdio_mux_uninit);267 268MODULE_DESCRIPTION(DRV_DESCRIPTION);269MODULE_AUTHOR("David Daney");270MODULE_LICENSE("GPL v2");271