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(lock: &pb->mii_bus->mdio_lock, subclass: 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(lock: &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(lock: &pb->mii_bus->mdio_lock, subclass: 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(lock: &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(lock: &pb->mii_bus->mdio_lock, subclass: 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(lock: &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(lock: &pb->mii_bus->mdio_lock, subclass: 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(lock: &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(bus: cb->mii_bus); |
134 | mdiobus_free(bus: 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(np: mux_node, |
158 | phandle_name: "mdio-parent-bus" , index: 0); |
159 | |
160 | if (!parent_bus_node) |
161 | return -ENODEV; |
162 | |
163 | parent_bus = of_mdio_find_bus(mdio_np: 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(dev: &parent_bus->dev); |
172 | } |
173 | |
174 | pb = devm_kzalloc(dev, size: 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(np: child_bus_node, propname: "reg" , out_value: &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, size: 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(buf: cb->mii_bus->id, MII_BUS_ID_SIZE, fmt: "%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(mdio: cb->mii_bus, np: child_bus_node); |
226 | if (r) { |
227 | mdiobus_free(bus: cb->mii_bus); |
228 | if (r == -EPROBE_DEFER) { |
229 | ret_val = r; |
230 | goto err_loop; |
231 | } |
232 | devm_kfree(dev, p: 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(node: child_bus_node); |
251 | err_pb_kz: |
252 | put_device(dev: &parent_bus->dev); |
253 | err_parent_bus: |
254 | of_node_put(node: parent_bus_node); |
255 | return ret_val; |
256 | } |
257 | EXPORT_SYMBOL_GPL(mdio_mux_init); |
258 | |
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(dev: &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 | |