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 */
mdio_mux_read(struct mii_bus * bus,int phy_id,int regnum)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
mdio_mux_read_c45(struct mii_bus * bus,int phy_id,int dev_addr,int regnum)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 */
mdio_mux_write(struct mii_bus * bus,int phy_id,int regnum,u16 val)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
mdio_mux_write_c45(struct mii_bus * bus,int phy_id,int dev_addr,int regnum,u16 val)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
mdio_mux_uninit_children(struct mdio_mux_parent_bus * pb)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
mdio_mux_init(struct device * dev,struct device_node * mux_node,int (* switch_fn)(int cur,int desired,void * data),void ** mux_handle,void * data,struct mii_bus * mux_bus)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: %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
248 err_loop:
249 mdio_mux_uninit_children(pb);
250 of_node_put(child_bus_node);
251 err_pb_kz:
252 put_device(&parent_bus->dev);
253 err_parent_bus:
254 of_node_put(parent_bus_node);
255 return ret_val;
256 }
257 EXPORT_SYMBOL_GPL(mdio_mux_init);
258
mdio_mux_uninit(void * mux_handle)259 void 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 }
266 EXPORT_SYMBOL_GPL(mdio_mux_uninit);
267
268 MODULE_DESCRIPTION(DRV_DESCRIPTION);
269 MODULE_AUTHOR("David Daney");
270 MODULE_LICENSE("GPL v2");
271