fixed_phy.c

  1// SPDX-License-Identifier: GPL-2.0+
  2/*
  3 * Fixed MDIO bus (MDIO bus emulation with fixed PHYs)
  4 *
  5 * Author: Vitaly Bordug <vbordug@ru.mvista.com>
  6 *         Anton Vorontsov <avorontsov@ru.mvista.com>
  7 *
  8 * Copyright (c) 2006-2007 MontaVista Software, Inc.
  9 */
 10
 11#include <linux/kernel.h>
 12#include <linux/module.h>
 13#include <linux/list.h>
 14#include <linux/mii.h>
 15#include <linux/phy.h>
 16#include <linux/phy_fixed.h>
 17#include <linux/err.h>
 18#include <linux/slab.h>
 19#include <linux/of.h>
 20#include <linux/idr.h>
 21#include <linux/netdevice.h>
 22
 23#include "swphy.h"
 24
 25struct fixed_phy {
 26	int addr;
 27	struct phy_device *phydev;
 28	struct fixed_phy_status status;
 29	int (*link_update)(struct net_device *, struct fixed_phy_status *);
 30	struct list_head node;
 31};
 32
 33static struct mii_bus *fmb_mii_bus;
 34static LIST_HEAD(fmb_phys);
 35
 36static struct fixed_phy *fixed_phy_find(int addr)
 37{
 38	struct fixed_phy *fp;
 39
 40	list_for_each_entry(fp, &fmb_phys, node) {
 41		if (fp->addr == addr)
 42			return fp;
 43	}
 44
 45	return NULL;
 46}
 47
 48int fixed_phy_change_carrier(struct net_device *dev, bool new_carrier)
 49{
 50	struct phy_device *phydev = dev->phydev;
 51	struct fixed_phy *fp;
 52
 53	if (!phydev || !phydev->mdio.bus)
 54		return -EINVAL;
 55
 56	fp = fixed_phy_find(phydev->mdio.addr);
 57	if (!fp)
 58		return -EINVAL;
 59
 60	fp->status.link = new_carrier;
 61
 62	return 0;
 63}
 64EXPORT_SYMBOL_GPL(fixed_phy_change_carrier);
 65
 66static int fixed_mdio_read(struct mii_bus *bus, int phy_addr, int reg_num)
 67{
 68	struct fixed_phy *fp;
 69
 70	fp = fixed_phy_find(phy_addr);
 71	if (!fp)
 72		return 0xffff;
 73
 74	if (fp->link_update)
 75		fp->link_update(fp->phydev->attached_dev, &fp->status);
 76
 77	return swphy_read_reg(reg_num, &fp->status);
 78}
 79
 80static int fixed_mdio_write(struct mii_bus *bus, int phy_addr, int reg_num,
 81			    u16 val)
 82{
 83	return 0;
 84}
 85
 86/*
 87 * If something weird is required to be done with link/speed,
 88 * network driver is able to assign a function to implement this.
 89 * May be useful for PHY's that need to be software-driven.
 90 */
 91int fixed_phy_set_link_update(struct phy_device *phydev,
 92			      int (*link_update)(struct net_device *,
 93						 struct fixed_phy_status *))
 94{
 95	struct fixed_phy *fp;
 96
 97	if (!phydev || !phydev->mdio.bus)
 98		return -EINVAL;
 99
100	fp = fixed_phy_find(phydev->mdio.addr);
101	if (!fp)
102		return -ENOENT;
103
104	fp->link_update = link_update;
105	fp->phydev = phydev;
106
107	return 0;
108}
109EXPORT_SYMBOL_GPL(fixed_phy_set_link_update);
110
111static int __fixed_phy_add(int phy_addr,
112			   const struct fixed_phy_status *status)
113{
114	struct fixed_phy *fp;
115	int ret;
116
117	ret = swphy_validate_state(status);
118	if (ret < 0)
119		return ret;
120
121	fp = kzalloc(sizeof(*fp), GFP_KERNEL);
122	if (!fp)
123		return -ENOMEM;
124
125	fp->addr = phy_addr;
126	fp->status = *status;
127	fp->status.link = true;
128
129	list_add_tail(&fp->node, &fmb_phys);
130
131	return 0;
132}
133
134static DEFINE_IDA(phy_fixed_ida);
135
136static void fixed_phy_del(int phy_addr)
137{
138	struct fixed_phy *fp;
139
140	fp = fixed_phy_find(phy_addr);
141	if (!fp)
142		return;
143
144	list_del(&fp->node);
145	kfree(fp);
146	ida_free(&phy_fixed_ida, phy_addr);
147}
148
149struct phy_device *fixed_phy_register(const struct fixed_phy_status *status,
150				      struct device_node *np)
151{
152	struct phy_device *phy;
153	int phy_addr;
154	int ret;
155
156	if (!fmb_mii_bus || fmb_mii_bus->state != MDIOBUS_REGISTERED)
157		return ERR_PTR(-EPROBE_DEFER);
158
159	/* Get the next available PHY address, up to PHY_MAX_ADDR */
160	phy_addr = ida_alloc_max(&phy_fixed_ida, PHY_MAX_ADDR - 1, GFP_KERNEL);
161	if (phy_addr < 0)
162		return ERR_PTR(phy_addr);
163
164	ret = __fixed_phy_add(phy_addr, status);
165	if (ret < 0) {
166		ida_free(&phy_fixed_ida, phy_addr);
167		return ERR_PTR(ret);
168	}
169
170	phy = get_phy_device(fmb_mii_bus, phy_addr, false);
171	if (IS_ERR(phy)) {
172		fixed_phy_del(phy_addr);
173		return ERR_PTR(-EINVAL);
174	}
175
176	of_node_get(np);
177	phy->mdio.dev.of_node = np;
178	phy->is_pseudo_fixed_link = true;
179
180	ret = phy_device_register(phy);
181	if (ret) {
182		phy_device_free(phy);
183		of_node_put(np);
184		fixed_phy_del(phy_addr);
185		return ERR_PTR(ret);
186	}
187
188	return phy;
189}
190EXPORT_SYMBOL_GPL(fixed_phy_register);
191
192struct phy_device *fixed_phy_register_100fd(void)
193{
194	static const struct fixed_phy_status status = {
195		.speed	= SPEED_100,
196		.duplex	= DUPLEX_FULL,
197	};
198
199	return fixed_phy_register(&status, NULL);
200}
201EXPORT_SYMBOL_GPL(fixed_phy_register_100fd);
202
203void fixed_phy_unregister(struct phy_device *phy)
204{
205	phy_device_remove(phy);
206	of_node_put(phy->mdio.dev.of_node);
207	fixed_phy_del(phy->mdio.addr);
208	phy_device_free(phy);
209}
210EXPORT_SYMBOL_GPL(fixed_phy_unregister);
211
212static int __init fixed_mdio_bus_init(void)
213{
214	int ret;
215
216	fmb_mii_bus = mdiobus_alloc();
217	if (!fmb_mii_bus)
218		return -ENOMEM;
219
220	snprintf(fmb_mii_bus->id, MII_BUS_ID_SIZE, "fixed-0");
221	fmb_mii_bus->name = "Fixed MDIO Bus";
222	fmb_mii_bus->read = &fixed_mdio_read;
223	fmb_mii_bus->write = &fixed_mdio_write;
224	fmb_mii_bus->phy_mask = ~0;
225
226	ret = mdiobus_register(fmb_mii_bus);
227	if (ret)
228		goto err_mdiobus_alloc;
229
230	return 0;
231
232err_mdiobus_alloc:
233	mdiobus_free(fmb_mii_bus);
234	return ret;
235}
236module_init(fixed_mdio_bus_init);
237
238static void __exit fixed_mdio_bus_exit(void)
239{
240	struct fixed_phy *fp, *tmp;
241
242	mdiobus_unregister(fmb_mii_bus);
243	mdiobus_free(fmb_mii_bus);
244
245	list_for_each_entry_safe(fp, tmp, &fmb_phys, node) {
246		list_del(&fp->node);
247		kfree(fp);
248	}
249	ida_destroy(&phy_fixed_ida);
250}
251module_exit(fixed_mdio_bus_exit);
252
253MODULE_DESCRIPTION("Fixed MDIO bus (MDIO bus emulation with fixed PHYs)");
254MODULE_AUTHOR("Vitaly Bordug");
255MODULE_LICENSE("GPL");