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 #include <linux/linkmode.h> 23 24 #include "swphy.h" 25 26 struct fixed_phy { 27 int addr; 28 struct phy_device *phydev; 29 struct fixed_phy_status status; 30 int (*link_update)(struct net_device *, struct fixed_phy_status *); 31 struct list_head node; 32 }; 33 34 static struct mii_bus *fmb_mii_bus; 35 static LIST_HEAD(fmb_phys); 36 37 static struct fixed_phy *fixed_phy_find(int addr) 38 { 39 struct fixed_phy *fp; 40 41 list_for_each_entry(fp, &fmb_phys, node) { 42 if (fp->addr == addr) 43 return fp; 44 } 45 46 return NULL; 47 } 48 49 int fixed_phy_change_carrier(struct net_device *dev, bool new_carrier) 50 { 51 struct phy_device *phydev = dev->phydev; 52 struct fixed_phy *fp; 53 54 if (!phydev || !phydev->mdio.bus) 55 return -EINVAL; 56 57 fp = fixed_phy_find(phydev->mdio.addr); 58 if (!fp) 59 return -EINVAL; 60 61 fp->status.link = new_carrier; 62 63 return 0; 64 } 65 EXPORT_SYMBOL_GPL(fixed_phy_change_carrier); 66 67 static int fixed_mdio_read(struct mii_bus *bus, int phy_addr, int reg_num) 68 { 69 struct fixed_phy *fp; 70 71 fp = fixed_phy_find(phy_addr); 72 if (!fp) 73 return 0xffff; 74 75 if (fp->link_update) 76 fp->link_update(fp->phydev->attached_dev, &fp->status); 77 78 return swphy_read_reg(reg_num, &fp->status); 79 } 80 81 static int fixed_mdio_write(struct mii_bus *bus, int phy_addr, int reg_num, 82 u16 val) 83 { 84 return 0; 85 } 86 87 /* 88 * If something weird is required to be done with link/speed, 89 * network driver is able to assign a function to implement this. 90 * May be useful for PHY's that need to be software-driven. 91 */ 92 int fixed_phy_set_link_update(struct phy_device *phydev, 93 int (*link_update)(struct net_device *, 94 struct fixed_phy_status *)) 95 { 96 struct fixed_phy *fp; 97 98 if (!phydev || !phydev->mdio.bus) 99 return -EINVAL; 100 101 fp = fixed_phy_find(phydev->mdio.addr); 102 if (!fp) 103 return -ENOENT; 104 105 fp->link_update = link_update; 106 fp->phydev = phydev; 107 108 return 0; 109 } 110 EXPORT_SYMBOL_GPL(fixed_phy_set_link_update); 111 112 static int __fixed_phy_add(int phy_addr, 113 const struct fixed_phy_status *status) 114 { 115 struct fixed_phy *fp; 116 int ret; 117 118 ret = swphy_validate_state(status); 119 if (ret < 0) 120 return ret; 121 122 fp = kzalloc(sizeof(*fp), GFP_KERNEL); 123 if (!fp) 124 return -ENOMEM; 125 126 fp->addr = phy_addr; 127 fp->status = *status; 128 129 list_add_tail(&fp->node, &fmb_phys); 130 131 return 0; 132 } 133 134 void fixed_phy_add(const struct fixed_phy_status *status) 135 { 136 __fixed_phy_add(0, status); 137 } 138 EXPORT_SYMBOL_GPL(fixed_phy_add); 139 140 static DEFINE_IDA(phy_fixed_ida); 141 142 static void fixed_phy_del(int phy_addr) 143 { 144 struct fixed_phy *fp; 145 146 fp = fixed_phy_find(phy_addr); 147 if (!fp) 148 return; 149 150 list_del(&fp->node); 151 kfree(fp); 152 ida_free(&phy_fixed_ida, phy_addr); 153 } 154 155 struct phy_device *fixed_phy_register(const struct fixed_phy_status *status, 156 struct device_node *np) 157 { 158 struct phy_device *phy; 159 int phy_addr; 160 int ret; 161 162 if (!fmb_mii_bus || fmb_mii_bus->state != MDIOBUS_REGISTERED) 163 return ERR_PTR(-EPROBE_DEFER); 164 165 /* Get the next available PHY address, up to PHY_MAX_ADDR */ 166 phy_addr = ida_alloc_max(&phy_fixed_ida, PHY_MAX_ADDR - 1, GFP_KERNEL); 167 if (phy_addr < 0) 168 return ERR_PTR(phy_addr); 169 170 ret = __fixed_phy_add(phy_addr, status); 171 if (ret < 0) { 172 ida_free(&phy_fixed_ida, phy_addr); 173 return ERR_PTR(ret); 174 } 175 176 phy = get_phy_device(fmb_mii_bus, phy_addr, false); 177 if (IS_ERR(phy)) { 178 fixed_phy_del(phy_addr); 179 return ERR_PTR(-EINVAL); 180 } 181 182 /* propagate the fixed link values to struct phy_device */ 183 phy->link = status->link; 184 if (status->link) { 185 phy->speed = status->speed; 186 phy->duplex = status->duplex; 187 phy->pause = status->pause; 188 phy->asym_pause = status->asym_pause; 189 } 190 191 of_node_get(np); 192 phy->mdio.dev.of_node = np; 193 phy->is_pseudo_fixed_link = true; 194 195 switch (status->speed) { 196 case SPEED_1000: 197 linkmode_set_bit(ETHTOOL_LINK_MODE_1000baseT_Half_BIT, 198 phy->supported); 199 linkmode_set_bit(ETHTOOL_LINK_MODE_1000baseT_Full_BIT, 200 phy->supported); 201 fallthrough; 202 case SPEED_100: 203 linkmode_set_bit(ETHTOOL_LINK_MODE_100baseT_Half_BIT, 204 phy->supported); 205 linkmode_set_bit(ETHTOOL_LINK_MODE_100baseT_Full_BIT, 206 phy->supported); 207 fallthrough; 208 case SPEED_10: 209 default: 210 linkmode_set_bit(ETHTOOL_LINK_MODE_10baseT_Half_BIT, 211 phy->supported); 212 linkmode_set_bit(ETHTOOL_LINK_MODE_10baseT_Full_BIT, 213 phy->supported); 214 } 215 216 phy_advertise_supported(phy); 217 218 ret = phy_device_register(phy); 219 if (ret) { 220 phy_device_free(phy); 221 of_node_put(np); 222 fixed_phy_del(phy_addr); 223 return ERR_PTR(ret); 224 } 225 226 return phy; 227 } 228 EXPORT_SYMBOL_GPL(fixed_phy_register); 229 230 void fixed_phy_unregister(struct phy_device *phy) 231 { 232 phy_device_remove(phy); 233 of_node_put(phy->mdio.dev.of_node); 234 fixed_phy_del(phy->mdio.addr); 235 phy_device_free(phy); 236 } 237 EXPORT_SYMBOL_GPL(fixed_phy_unregister); 238 239 static int __init fixed_mdio_bus_init(void) 240 { 241 int ret; 242 243 fmb_mii_bus = mdiobus_alloc(); 244 if (!fmb_mii_bus) 245 return -ENOMEM; 246 247 snprintf(fmb_mii_bus->id, MII_BUS_ID_SIZE, "fixed-0"); 248 fmb_mii_bus->name = "Fixed MDIO Bus"; 249 fmb_mii_bus->read = &fixed_mdio_read; 250 fmb_mii_bus->write = &fixed_mdio_write; 251 fmb_mii_bus->phy_mask = ~0; 252 253 ret = mdiobus_register(fmb_mii_bus); 254 if (ret) 255 goto err_mdiobus_alloc; 256 257 return 0; 258 259 err_mdiobus_alloc: 260 mdiobus_free(fmb_mii_bus); 261 return ret; 262 } 263 module_init(fixed_mdio_bus_init); 264 265 static void __exit fixed_mdio_bus_exit(void) 266 { 267 struct fixed_phy *fp, *tmp; 268 269 mdiobus_unregister(fmb_mii_bus); 270 mdiobus_free(fmb_mii_bus); 271 272 list_for_each_entry_safe(fp, tmp, &fmb_phys, node) { 273 list_del(&fp->node); 274 kfree(fp); 275 } 276 ida_destroy(&phy_fixed_ida); 277 } 278 module_exit(fixed_mdio_bus_exit); 279 280 MODULE_DESCRIPTION("Fixed MDIO bus (MDIO bus emulation with fixed PHYs)"); 281 MODULE_AUTHOR("Vitaly Bordug"); 282 MODULE_LICENSE("GPL"); 283