embox

1
/**
2
 * @file phy.c
3
 * @brief Common PHY config and auto negotiate
4
 * @author Denis Deryugin <deryugin.denis@gmail.com>
5
 * @version
6
 * @date 28.05.2018
7
 */
8

9
#include <unistd.h>
10

11
#include <net/phy.h>
12
#include <util/log.h>
13

14
static int phy_read(struct net_device *dev, int reg) {
15
	assert(dev);
16
	assert(dev->drv_ops);
17
	assert(dev->drv_ops->mdio_read);
18

19
	return dev->drv_ops->mdio_read(dev, reg);
20
}
21

22
static int phy_write(struct net_device *dev, int reg, int data) {
23
	assert(dev);
24
	assert(dev->drv_ops);
25
	assert(dev->drv_ops->mdio_write);
26

27
	return dev->drv_ops->mdio_write(dev, reg, data);
28
}
29

30
static void set_phyid(struct net_device *dev, int phyid) {
31
	assert(dev);
32
	assert(dev->drv_ops);
33
	assert(dev->drv_ops->set_phyid);
34

35
	return dev->drv_ops->set_phyid(dev, phyid);
36
}
37

38
int phy_detect(struct net_device *dev) {
39
	uint32_t phyid[2] ;
40

41
	assert(dev);
42
	for (int i = 0; i < 32; i++) {
43
		set_phyid(dev, i);
44
		if ((phyid[0] = phy_read(dev, MII_PHYSID1)) != 0xffff) {
45
			phyid[1] = phy_read(dev, MII_PHYSID2);
46
			log_debug("Detected phyaddr=%d ,ID=%X:%X", i, phyid[0], phyid[1]);
47
			return 0;
48
		}
49
	}
50

51
	return -1;
52
}
53

54
int phy_reset(struct net_device *dev) {
55
	uint32_t reg, retry = 0;
56
	assert(dev);
57
	log_debug("Reset PHY");
58
	reg = phy_read(dev, MII_BMCR);
59
	phy_write(dev, MII_BMCR, reg | BMCR_RESET);
60

61
	while (phy_read(dev, MII_BMCR) & BMCR_RESET) {
62
		usleep(1000);
63
		if (retry++ > 0xffff) {
64
			log_error("PHY reset failed");
65
			return -1;
66
		}
67
	}
68

69
	return 0;
70
}
71

72
int phy_wait_autoneg(struct net_device *dev) {
73
	int retry = 0;
74

75
	while (!(phy_read(dev, MII_BMSR) & BMSR_LSTATUS)) {
76
		if (retry++ > 10000) {
77
			log_error("Autonegotiation timeout");
78
			return -1;
79
		}
80

81
		usleep(1000);
82
	}
83

84
	return 0;
85
}
86

87
int phy_try_speed(struct net_device *dev, int speed) {
88
	uint32_t reg;
89
	uint32_t gbit = 0;
90

91
	phy_write(dev, MII_ADVERTISE, net_speed_to_adv(speed));
92

93
	if (net_is_1000(speed)) {
94
		reg = phy_read(dev, MII_BMCR);
95

96
		phy_write(dev, MII_BMCR, reg | BMCR_ANRESTART);
97

98
		phy_wait_autoneg(dev);
99

100
		log_debug("Try 1Gbit speed");
101
		reg = phy_read(dev, MII_BMSR);
102
		if (reg & BMSR_ERCAP) {
103
			/* Try to enable 1000mbit mode */
104
			reg = phy_read(dev, MII_CTRL1000);
105
			reg |= ADVERTISE_1000HALF;
106
			if (net_is_fullduplex(speed)) {
107
				reg |= ADVERTISE_1000FULL;
108
			}
109
			phy_write(dev, MII_CTRL1000, reg);
110

111
			/* Make sure it's supported both by link and PHY */
112
			gbit = phy_read(dev, MII_STAT1000) >> 2;
113
			gbit &= phy_read(dev, MII_CTRL1000);
114

115
			/* If 1gbit is supported, we are done */
116
			if (gbit & ADVERTISE_1000FULL) {
117
				log_debug("Have 1Gbit full");
118
				return adv_to_net_speed(ADVERTISE_1000XFULL, 1);
119
			}
120

121
			if (gbit & ADVERTISE_1000HALF) {
122
				log_debug("Have 1Gbit half");
123
				return adv_to_net_speed(ADVERTISE_1000XHALF, 1);
124
			}
125
		}
126

127
		log_debug("Failed to setup 1Gbit link");
128
		speed &= ~NET_GBIT;
129
		if (speed == 0) {
130
			return 0;
131
		}
132
	}
133

134
	/* Try 100mpbs/10mpbs */
135
	phy_write(dev, MII_CTRL1000, 0);
136
	phy_write(dev, MII_ADVERTISE, net_speed_to_adv(speed));
137

138
	reg = phy_read(dev, MII_BMCR);
139

140
	phy_write(dev, MII_BMCR, reg | BMCR_ANRESTART);
141

142
	phy_wait_autoneg(dev);
143

144
	return adv_to_net_speed(phy_read(dev, MII_LPA) &
145
				phy_read(dev, MII_ADVERTISE), 0);
146
}
147

148
int phy_autoneg(struct net_device *dev, int fixed_speed) {
149
	int ret;
150
	assert(dev);
151
	/* Wake up from sleep if necessary
152
	 * Reset PHY, then delay 300ns */
153
	phy_reset(dev);
154

155
	phy_write(dev, MII_BMCR, BMCR_ANENABLE);
156

157
	switch (fixed_speed) {
158
	case 1000:
159
		if (phy_try_speed(dev, NET_1000FULL)) {
160
			log_info("\t1000 Mbps FULL\n");
161
			dev->drv_ops->set_speed(dev, NET_1000FULL);
162
		}
163
		break;
164
	case 100:
165
		if (phy_try_speed(dev, NET_100FULL)) {
166
			log_info("\t100 Mbps FULL\n");
167
			dev->drv_ops->set_speed(dev, NET_100FULL);
168
		}
169
		break;
170
	case 10:
171
		if (phy_try_speed(dev, NET_10FULL)) {
172
			log_info("\t10 Mbps FULL\n");
173
			dev->drv_ops->set_speed(dev, NET_10FULL);
174
		}
175
		break;
176
	default:
177
	/* Find out best speed */
178
		if ((ret = phy_try_speed(dev, NET_1000FULL | NET_1000HALF))) {
179
			if (ret & NET_1000FULL) {
180
				log_info("\t1000 Mpbs FULL\n");
181
				dev->drv_ops->set_speed(dev, NET_1000FULL);
182
			} else {
183
				log_info("\t1000 Mpbs HALF\n");
184
				dev->drv_ops->set_speed(dev, NET_1000HALF);
185
			}
186
			return 0;
187
		} else {
188
			ret = phy_try_speed(dev, NET_10HALF | NET_10FULL |
189
						NET_100HALF | NET_100FULL);
190

191
			if (0 == (ret = net_top_speed(ret))) {
192
				goto err_out;
193
			}
194

195

196
			log_info("\t%d Mbps %s\n", net_to_mbps(ret),
197
					net_is_fullduplex(ret) ? "FULL" : "HALF");
198

199
			assert(dev->drv_ops->set_speed);
200

201
			dev->drv_ops->set_speed(dev, ret);
202
		}
203
	}
204

205
	return 0;
206
err_out:
207
	log_error("Autonegotiation failed");
208
	return -1;
209
}
210

211
int phy_init(struct net_device *dev) {
212
	phy_detect(dev);
213
	phy_autoneg(dev, 0);
214

215
	return 0;
216
}
217