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