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