1
2
3
4
5
6
7
8
9
10
11
12
13
14
15#include <linux/kernel.h>
16#include <linux/module.h>
17#include <linux/platform_device.h>
18#include <linux/list.h>
19#include <linux/mii.h>
20#include <linux/phy.h>
21#include <linux/phy_fixed.h>
22#include <linux/err.h>
23
24#define MII_REGS_NUM 29
25
26struct fixed_mdio_bus {
27 int irqs[PHY_MAX_ADDR];
28 struct mii_bus *mii_bus;
29 struct list_head phys;
30};
31
32struct fixed_phy {
33 int id;
34 u16 regs[MII_REGS_NUM];
35 struct phy_device *phydev;
36 struct fixed_phy_status status;
37 int (*link_update)(struct net_device *, struct fixed_phy_status *);
38 struct list_head node;
39};
40
41static struct platform_device *pdev;
42static struct fixed_mdio_bus platform_fmb = {
43 .phys = LIST_HEAD_INIT(platform_fmb.phys),
44};
45
46static int fixed_phy_update_regs(struct fixed_phy *fp)
47{
48 u16 bmsr = BMSR_ANEGCAPABLE;
49 u16 bmcr = 0;
50 u16 lpagb = 0;
51 u16 lpa = 0;
52
53 if (fp->status.duplex) {
54 bmcr |= BMCR_FULLDPLX;
55
56 switch (fp->status.speed) {
57 case 1000:
58 bmsr |= BMSR_ESTATEN;
59 bmcr |= BMCR_SPEED1000;
60 lpagb |= LPA_1000FULL;
61 break;
62 case 100:
63 bmsr |= BMSR_100FULL;
64 bmcr |= BMCR_SPEED100;
65 lpa |= LPA_100FULL;
66 break;
67 case 10:
68 bmsr |= BMSR_10FULL;
69 lpa |= LPA_10FULL;
70 break;
71 default:
72 printk(KERN_WARNING "fixed phy: unknown speed\n");
73 return -EINVAL;
74 }
75 } else {
76 switch (fp->status.speed) {
77 case 1000:
78 bmsr |= BMSR_ESTATEN;
79 bmcr |= BMCR_SPEED1000;
80 lpagb |= LPA_1000HALF;
81 break;
82 case 100:
83 bmsr |= BMSR_100HALF;
84 bmcr |= BMCR_SPEED100;
85 lpa |= LPA_100HALF;
86 break;
87 case 10:
88 bmsr |= BMSR_10HALF;
89 lpa |= LPA_10HALF;
90 break;
91 default:
92 printk(KERN_WARNING "fixed phy: unknown speed\n");
93 return -EINVAL;
94 }
95 }
96
97 if (fp->status.link)
98 bmsr |= BMSR_LSTATUS | BMSR_ANEGCOMPLETE;
99
100 if (fp->status.pause)
101 lpa |= LPA_PAUSE_CAP;
102
103 if (fp->status.asym_pause)
104 lpa |= LPA_PAUSE_ASYM;
105
106 fp->regs[MII_PHYSID1] = fp->id >> 16;
107 fp->regs[MII_PHYSID2] = fp->id;
108
109 fp->regs[MII_BMSR] = bmsr;
110 fp->regs[MII_BMCR] = bmcr;
111 fp->regs[MII_LPA] = lpa;
112 fp->regs[MII_STAT1000] = lpagb;
113
114 return 0;
115}
116
117static int fixed_mdio_read(struct mii_bus *bus, int phy_id, int reg_num)
118{
119 struct fixed_mdio_bus *fmb = bus->priv;
120 struct fixed_phy *fp;
121
122 if (reg_num >= MII_REGS_NUM)
123 return -1;
124
125 list_for_each_entry(fp, &fmb->phys, node) {
126 if (fp->id == phy_id) {
127
128 if (fp->link_update) {
129 fp->link_update(fp->phydev->attached_dev,
130 &fp->status);
131 fixed_phy_update_regs(fp);
132 }
133 return fp->regs[reg_num];
134 }
135 }
136
137 return 0xFFFF;
138}
139
140static int fixed_mdio_write(struct mii_bus *bus, int phy_id, int reg_num,
141 u16 val)
142{
143 return 0;
144}
145
146
147
148
149
150
151int fixed_phy_set_link_update(struct phy_device *phydev,
152 int (*link_update)(struct net_device *,
153 struct fixed_phy_status *))
154{
155 struct fixed_mdio_bus *fmb = &platform_fmb;
156 struct fixed_phy *fp;
157
158 if (!link_update || !phydev || !phydev->bus)
159 return -EINVAL;
160
161 list_for_each_entry(fp, &fmb->phys, node) {
162 if (fp->id == phydev->phy_id) {
163 fp->link_update = link_update;
164 fp->phydev = phydev;
165 return 0;
166 }
167 }
168
169 return -ENOENT;
170}
171EXPORT_SYMBOL_GPL(fixed_phy_set_link_update);
172
173int fixed_phy_add(unsigned int irq, int phy_id,
174 struct fixed_phy_status *status)
175{
176 int ret;
177 struct fixed_mdio_bus *fmb = &platform_fmb;
178 struct fixed_phy *fp;
179
180 fp = kzalloc(sizeof(*fp), GFP_KERNEL);
181 if (!fp)
182 return -ENOMEM;
183
184 memset(fp->regs, 0xFF, sizeof(fp->regs[0]) * MII_REGS_NUM);
185
186 fmb->irqs[phy_id] = irq;
187
188 fp->id = phy_id;
189 fp->status = *status;
190
191 ret = fixed_phy_update_regs(fp);
192 if (ret)
193 goto err_regs;
194
195 list_add_tail(&fp->node, &fmb->phys);
196
197 return 0;
198
199err_regs:
200 kfree(fp);
201 return ret;
202}
203EXPORT_SYMBOL_GPL(fixed_phy_add);
204
205static int __init fixed_mdio_bus_init(void)
206{
207 struct fixed_mdio_bus *fmb = &platform_fmb;
208 int ret;
209
210 pdev = platform_device_register_simple("Fixed MDIO bus", 0, NULL, 0);
211 if (IS_ERR(pdev)) {
212 ret = PTR_ERR(pdev);
213 goto err_pdev;
214 }
215
216 fmb->mii_bus = mdiobus_alloc();
217 if (fmb->mii_bus == NULL) {
218 ret = -ENOMEM;
219 goto err_mdiobus_reg;
220 }
221
222 snprintf(fmb->mii_bus->id, MII_BUS_ID_SIZE, "0");
223 fmb->mii_bus->name = "Fixed MDIO Bus";
224 fmb->mii_bus->priv = fmb;
225 fmb->mii_bus->parent = &pdev->dev;
226 fmb->mii_bus->read = &fixed_mdio_read;
227 fmb->mii_bus->write = &fixed_mdio_write;
228 fmb->mii_bus->irq = fmb->irqs;
229
230 ret = mdiobus_register(fmb->mii_bus);
231 if (ret)
232 goto err_mdiobus_alloc;
233
234 return 0;
235
236err_mdiobus_alloc:
237 mdiobus_free(fmb->mii_bus);
238err_mdiobus_reg:
239 platform_device_unregister(pdev);
240err_pdev:
241 return ret;
242}
243module_init(fixed_mdio_bus_init);
244
245static void __exit fixed_mdio_bus_exit(void)
246{
247 struct fixed_mdio_bus *fmb = &platform_fmb;
248 struct fixed_phy *fp, *tmp;
249
250 mdiobus_unregister(fmb->mii_bus);
251 mdiobus_free(fmb->mii_bus);
252 platform_device_unregister(pdev);
253
254 list_for_each_entry_safe(fp, tmp, &fmb->phys, node) {
255 list_del(&fp->node);
256 kfree(fp);
257 }
258}
259module_exit(fixed_mdio_bus_exit);
260
261MODULE_DESCRIPTION("Fixed MDIO bus (MDIO bus emulation with fixed PHYs)");
262MODULE_AUTHOR("Vitaly Bordug");
263MODULE_LICENSE("GPL");
264
