forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-01-04 1543e317f1da31b75942316931e8f491a8920811 
 kernel/drivers/phy/marvell/phy-mvebu-cp110-comphy.c
....@@ -5,10 +5,13 @@
55  * Antoine Tenart <antoine.tenart@free-electrons.com>
66  */
77 
8
+#include <linux/arm-smccc.h>
9
+#include <linux/clk.h>
810 #include <linux/io.h>
911 #include <linux/iopoll.h>
1012 #include <linux/mfd/syscon.h>
1113 #include <linux/module.h>
14
+#include <linux/phy.h>
1215 #include <linux/phy/phy.h>
1316 #include <linux/platform_device.h>
1417 #include <linux/regmap.h>
....@@ -21,6 +24,7 @@
2124 #define     MVEBU_COMPHY_SERDES_CFG0_PU_RX	BIT(11)
2225 #define     MVEBU_COMPHY_SERDES_CFG0_PU_TX	BIT(12)
2326 #define     MVEBU_COMPHY_SERDES_CFG0_HALF_BUS	BIT(14)
27
+#define     MVEBU_COMPHY_SERDES_CFG0_RXAUI_MODE	BIT(15)
2428 #define MVEBU_COMPHY_SERDES_CFG1(n)		(0x4 + (n) * 0x1000)
2529 #define     MVEBU_COMPHY_SERDES_CFG1_RESET	BIT(3)
2630 #define     MVEBU_COMPHY_SERDES_CFG1_RX_INIT	BIT(4)
....@@ -76,8 +80,8 @@
7680 #define MVEBU_COMPHY_TX_SLEW_RATE(n)		(0x974 + (n) * 0x1000)
7781 #define     MVEBU_COMPHY_TX_SLEW_RATE_EMPH(n)	((n) << 5)
7882 #define     MVEBU_COMPHY_TX_SLEW_RATE_SLC(n)	((n) << 10)
79
-#define MVEBU_COMPHY_DLT_CTRL(n)		(0x984 + (n) * 0x1000)
80
-#define     MVEBU_COMPHY_DLT_CTRL_DTL_FLOOP_EN	BIT(2)
83
+#define MVEBU_COMPHY_DTL_CTRL(n)		(0x984 + (n) * 0x1000)
84
+#define     MVEBU_COMPHY_DTL_CTRL_DTL_FLOOP_EN	BIT(2)
8185 #define MVEBU_COMPHY_FRAME_DETECT0(n)		(0xa14 + (n) * 0x1000)
8286 #define     MVEBU_COMPHY_FRAME_DETECT0_PATN(n)	((n) << 7)
8387 #define MVEBU_COMPHY_FRAME_DETECT3(n)		(0xa20 + (n) * 0x1000)
....@@ -110,85 +114,222 @@
110114 #define     MVEBU_COMPHY_SELECTOR_PHY(n)	((n) * 0x4)
111115 #define MVEBU_COMPHY_PIPE_SELECTOR		0x1144
112116 #define     MVEBU_COMPHY_PIPE_SELECTOR_PIPE(n)	((n) * 0x4)
117
+#define MVEBU_COMPHY_SD1_CTRL1			0x1148
118
+#define     MVEBU_COMPHY_SD1_CTRL1_RXAUI1_EN	BIT(26)
119
+#define     MVEBU_COMPHY_SD1_CTRL1_RXAUI0_EN	BIT(27)
113120 
114121 #define MVEBU_COMPHY_LANES	6
115122 #define MVEBU_COMPHY_PORTS	3
116123 
117
-struct mvebu_comhy_conf {
124
+#define COMPHY_SIP_POWER_ON	0x82000001
125
+#define COMPHY_SIP_POWER_OFF	0x82000002
126
+
127
+/*
128
+ * A lane is described by the following bitfields:
129
+ * [ 1- 0]: COMPHY polarity invertion
130
+ * [ 2- 7]: COMPHY speed
131
+ * [ 5-11]: COMPHY port index
132
+ * [12-16]: COMPHY mode
133
+ * [17]: Clock source
134
+ * [18-20]: PCIe width (x1, x2, x4)
135
+ */
136
+#define COMPHY_FW_POL_OFFSET	0
137
+#define COMPHY_FW_POL_MASK	GENMASK(1, 0)
138
+#define COMPHY_FW_SPEED_OFFSET	2
139
+#define COMPHY_FW_SPEED_MASK	GENMASK(7, 2)
140
+#define COMPHY_FW_SPEED_MAX	COMPHY_FW_SPEED_MASK
141
+#define COMPHY_FW_SPEED_1250	0
142
+#define COMPHY_FW_SPEED_3125	2
143
+#define COMPHY_FW_SPEED_5000	3
144
+#define COMPHY_FW_SPEED_103125	6
145
+#define COMPHY_FW_PORT_OFFSET	8
146
+#define COMPHY_FW_PORT_MASK	GENMASK(11, 8)
147
+#define COMPHY_FW_MODE_OFFSET	12
148
+#define COMPHY_FW_MODE_MASK	GENMASK(16, 12)
149
+#define COMPHY_FW_WIDTH_OFFSET	18
150
+#define COMPHY_FW_WIDTH_MASK	GENMASK(20, 18)
151
+
152
+#define COMPHY_FW_PARAM_FULL(mode, port, speed, pol, width)		\
153
+	((((pol) << COMPHY_FW_POL_OFFSET) & COMPHY_FW_POL_MASK) |	\
154
+	 (((mode) << COMPHY_FW_MODE_OFFSET) & COMPHY_FW_MODE_MASK) |	\
155
+	 (((port) << COMPHY_FW_PORT_OFFSET) & COMPHY_FW_PORT_MASK) |	\
156
+	 (((speed) << COMPHY_FW_SPEED_OFFSET) & COMPHY_FW_SPEED_MASK) |	\
157
+	 (((width) << COMPHY_FW_WIDTH_OFFSET) & COMPHY_FW_WIDTH_MASK))
158
+
159
+#define COMPHY_FW_PARAM(mode, port)					\
160
+	COMPHY_FW_PARAM_FULL(mode, port, COMPHY_FW_SPEED_MAX, 0, 0)
161
+
162
+#define COMPHY_FW_PARAM_ETH(mode, port, speed)				\
163
+	COMPHY_FW_PARAM_FULL(mode, port, speed, 0, 0)
164
+
165
+#define COMPHY_FW_PARAM_PCIE(mode, port, width)				\
166
+	COMPHY_FW_PARAM_FULL(mode, port, COMPHY_FW_SPEED_5000, 0, width)
167
+
168
+#define COMPHY_FW_MODE_SATA		0x1
169
+#define COMPHY_FW_MODE_SGMII		0x2 /* SGMII 1G */
170
+#define COMPHY_FW_MODE_HS_SGMII		0x3 /* SGMII 2.5G */
171
+#define COMPHY_FW_MODE_USB3H		0x4
172
+#define COMPHY_FW_MODE_USB3D		0x5
173
+#define COMPHY_FW_MODE_PCIE		0x6
174
+#define COMPHY_FW_MODE_RXAUI		0x7
175
+#define COMPHY_FW_MODE_XFI		0x8 /* SFI: 0x9 (is treated like XFI) */
176
+
177
+struct mvebu_comphy_conf {
118178 	enum phy_mode mode;
179
+	int submode;
119180 	unsigned lane;
120181 	unsigned port;
121182 	u32 mux;
183
+	u32 fw_mode;
122184 };
123185 
124
-#define MVEBU_COMPHY_CONF(_lane, _port, _mode, _mux)	\
186
+#define ETH_CONF(_lane, _port, _submode, _mux, _fw)	\
187
+	{						\
188
+		.lane = _lane,				\
189
+		.port = _port,				\
190
+		.mode = PHY_MODE_ETHERNET,		\
191
+		.submode = _submode,			\
192
+		.mux = _mux,				\
193
+		.fw_mode = _fw,				\
194
+	}
195
+
196
+#define GEN_CONF(_lane, _port, _mode, _fw)		\
125197 	{						\
126198 		.lane = _lane,				\
127199 		.port = _port,				\
128200 		.mode = _mode,				\
129
-		.mux = _mux,				\
201
+		.submode = PHY_INTERFACE_MODE_NA,	\
202
+		.mux = -1,				\
203
+		.fw_mode = _fw,				\
130204 	}
131205 
132
-static const struct mvebu_comhy_conf mvebu_comphy_cp110_modes[] = {
206
+static const struct mvebu_comphy_conf mvebu_comphy_cp110_modes[] = {
133207 	/* lane 0 */
134
-	MVEBU_COMPHY_CONF(0, 1, PHY_MODE_SGMII, 0x1),
135
-	MVEBU_COMPHY_CONF(0, 1, PHY_MODE_2500SGMII, 0x1),
208
+	GEN_CONF(0, 0, PHY_MODE_PCIE, COMPHY_FW_MODE_PCIE),
209
+	ETH_CONF(0, 1, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
210
+	ETH_CONF(0, 1, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
211
+	GEN_CONF(0, 1, PHY_MODE_SATA, COMPHY_FW_MODE_SATA),
136212 	/* lane 1 */
137
-	MVEBU_COMPHY_CONF(1, 2, PHY_MODE_SGMII, 0x1),
138
-	MVEBU_COMPHY_CONF(1, 2, PHY_MODE_2500SGMII, 0x1),
213
+	GEN_CONF(1, 0, PHY_MODE_USB_HOST_SS, COMPHY_FW_MODE_USB3H),
214
+	GEN_CONF(1, 0, PHY_MODE_USB_DEVICE_SS, COMPHY_FW_MODE_USB3D),
215
+	GEN_CONF(1, 0, PHY_MODE_SATA, COMPHY_FW_MODE_SATA),
216
+	GEN_CONF(1, 0, PHY_MODE_PCIE, COMPHY_FW_MODE_PCIE),
217
+	ETH_CONF(1, 2, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
218
+	ETH_CONF(1, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
139219 	/* lane 2 */
140
-	MVEBU_COMPHY_CONF(2, 0, PHY_MODE_SGMII, 0x1),
141
-	MVEBU_COMPHY_CONF(2, 0, PHY_MODE_2500SGMII, 0x1),
142
-	MVEBU_COMPHY_CONF(2, 0, PHY_MODE_10GKR, 0x1),
220
+	ETH_CONF(2, 0, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
221
+	ETH_CONF(2, 0, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
222
+	ETH_CONF(2, 0, PHY_INTERFACE_MODE_RXAUI, 0x1, COMPHY_FW_MODE_RXAUI),
223
+	ETH_CONF(2, 0, PHY_INTERFACE_MODE_10GBASER, 0x1, COMPHY_FW_MODE_XFI),
224
+	GEN_CONF(2, 0, PHY_MODE_USB_HOST_SS, COMPHY_FW_MODE_USB3H),
225
+	GEN_CONF(2, 0, PHY_MODE_SATA, COMPHY_FW_MODE_SATA),
226
+	GEN_CONF(2, 0, PHY_MODE_PCIE, COMPHY_FW_MODE_PCIE),
143227 	/* lane 3 */
144
-	MVEBU_COMPHY_CONF(3, 1, PHY_MODE_SGMII, 0x2),
145
-	MVEBU_COMPHY_CONF(3, 1, PHY_MODE_2500SGMII, 0x2),
228
+	GEN_CONF(3, 0, PHY_MODE_PCIE, COMPHY_FW_MODE_PCIE),
229
+	ETH_CONF(3, 1, PHY_INTERFACE_MODE_SGMII, 0x2, COMPHY_FW_MODE_SGMII),
230
+	ETH_CONF(3, 1, PHY_INTERFACE_MODE_2500BASEX, 0x2, COMPHY_FW_MODE_HS_SGMII),
231
+	ETH_CONF(3, 1, PHY_INTERFACE_MODE_RXAUI, 0x1, COMPHY_FW_MODE_RXAUI),
232
+	GEN_CONF(3, 1, PHY_MODE_USB_HOST_SS, COMPHY_FW_MODE_USB3H),
233
+	GEN_CONF(3, 1, PHY_MODE_SATA, COMPHY_FW_MODE_SATA),
146234 	/* lane 4 */
147
-	MVEBU_COMPHY_CONF(4, 0, PHY_MODE_SGMII, 0x2),
148
-	MVEBU_COMPHY_CONF(4, 0, PHY_MODE_2500SGMII, 0x2),
149
-	MVEBU_COMPHY_CONF(4, 0, PHY_MODE_10GKR, 0x2),
150
-	MVEBU_COMPHY_CONF(4, 1, PHY_MODE_SGMII, 0x1),
235
+	ETH_CONF(4, 0, PHY_INTERFACE_MODE_SGMII, 0x2, COMPHY_FW_MODE_SGMII),
236
+	ETH_CONF(4, 0, PHY_INTERFACE_MODE_2500BASEX, 0x2, COMPHY_FW_MODE_HS_SGMII),
237
+	ETH_CONF(4, 0, PHY_INTERFACE_MODE_10GBASER, 0x2, COMPHY_FW_MODE_XFI),
238
+	ETH_CONF(4, 0, PHY_INTERFACE_MODE_RXAUI, 0x2, COMPHY_FW_MODE_RXAUI),
239
+	GEN_CONF(4, 0, PHY_MODE_USB_DEVICE_SS, COMPHY_FW_MODE_USB3D),
240
+	GEN_CONF(4, 1, PHY_MODE_USB_HOST_SS, COMPHY_FW_MODE_USB3H),
241
+	GEN_CONF(4, 1, PHY_MODE_PCIE, COMPHY_FW_MODE_PCIE),
242
+	ETH_CONF(4, 1, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
243
+	ETH_CONF(4, 1, PHY_INTERFACE_MODE_2500BASEX, -1, COMPHY_FW_MODE_HS_SGMII),
244
+	ETH_CONF(4, 1, PHY_INTERFACE_MODE_10GBASER, -1, COMPHY_FW_MODE_XFI),
151245 	/* lane 5 */
152
-	MVEBU_COMPHY_CONF(5, 2, PHY_MODE_SGMII, 0x1),
153
-	MVEBU_COMPHY_CONF(5, 2, PHY_MODE_2500SGMII, 0x1),
246
+	ETH_CONF(5, 1, PHY_INTERFACE_MODE_RXAUI, 0x2, COMPHY_FW_MODE_RXAUI),
247
+	GEN_CONF(5, 1, PHY_MODE_SATA, COMPHY_FW_MODE_SATA),
248
+	ETH_CONF(5, 2, PHY_INTERFACE_MODE_SGMII, 0x1, COMPHY_FW_MODE_SGMII),
249
+	ETH_CONF(5, 2, PHY_INTERFACE_MODE_2500BASEX, 0x1, COMPHY_FW_MODE_HS_SGMII),
250
+	GEN_CONF(5, 2, PHY_MODE_PCIE, COMPHY_FW_MODE_PCIE),
154251 };
155252 
156253 struct mvebu_comphy_priv {
157254 	void __iomem *base;
158255 	struct regmap *regmap;
159256 	struct device *dev;
257
+	struct clk *mg_domain_clk;
258
+	struct clk *mg_core_clk;
259
+	struct clk *axi_clk;
260
+	unsigned long cp_phys;
160261 };
161262 
162263 struct mvebu_comphy_lane {
163264 	struct mvebu_comphy_priv *priv;
164265 	unsigned id;
165266 	enum phy_mode mode;
267
+	int submode;
166268 	int port;
167269 };
168270 
169
-static int mvebu_comphy_get_mux(int lane, int port, enum phy_mode mode)
271
+static int mvebu_comphy_smc(unsigned long function, unsigned long phys,
272
+			    unsigned long lane, unsigned long mode)
273
+{
274
+	struct arm_smccc_res res;
275
+	s32 ret;
276
+
277
+	arm_smccc_smc(function, phys, lane, mode, 0, 0, 0, 0, &res);
278
+	ret = res.a0;
279
+
280
+	switch (ret) {
281
+	case SMCCC_RET_SUCCESS:
282
+		return 0;
283
+	case SMCCC_RET_NOT_SUPPORTED:
284
+		return -EOPNOTSUPP;
285
+	default:
286
+		return -EINVAL;
287
+	}
288
+}
289
+
290
+static int mvebu_comphy_get_mode(bool fw_mode, int lane, int port,
291
+				 enum phy_mode mode, int submode)
170292 {
171293 	int i, n = ARRAY_SIZE(mvebu_comphy_cp110_modes);
294
+	/* Ignore PCIe submode: it represents the width */
295
+	bool ignore_submode = (mode == PHY_MODE_PCIE);
296
+	const struct mvebu_comphy_conf *conf;
172297 
173298 	/* Unused PHY mux value is 0x0 */
174299 	if (mode == PHY_MODE_INVALID)
175300 		return 0;
176301 
177302 	for (i = 0; i < n; i++) {
178
-		if (mvebu_comphy_cp110_modes[i].lane == lane &&
179
-		    mvebu_comphy_cp110_modes[i].port == port &&
180
-		    mvebu_comphy_cp110_modes[i].mode == mode)
303
+		conf = &mvebu_comphy_cp110_modes[i];
304
+		if (conf->lane == lane &&
305
+		    conf->port == port &&
306
+		    conf->mode == mode &&
307
+		    (conf->submode == submode || ignore_submode))
181308 			break;
182309 	}
183310 
184311 	if (i == n)
185312 		return -EINVAL;
186313 
187
-	return mvebu_comphy_cp110_modes[i].mux;
314
+	if (fw_mode)
315
+		return conf->fw_mode;
316
+	else
317
+		return conf->mux;
188318 }
189319 
190
-static void mvebu_comphy_ethernet_init_reset(struct mvebu_comphy_lane *lane,
191
-					     enum phy_mode mode)
320
+static inline int mvebu_comphy_get_mux(int lane, int port,
321
+				       enum phy_mode mode, int submode)
322
+{
323
+	return mvebu_comphy_get_mode(false, lane, port, mode, submode);
324
+}
325
+
326
+static inline int mvebu_comphy_get_fw_mode(int lane, int port,
327
+					   enum phy_mode mode, int submode)
328
+{
329
+	return mvebu_comphy_get_mode(true, lane, port, mode, submode);
330
+}
331
+
332
+static int mvebu_comphy_ethernet_init_reset(struct mvebu_comphy_lane *lane)
192333 {
193334 	struct mvebu_comphy_priv *priv = lane->priv;
194335 	u32 val;
....@@ -205,19 +346,60 @@
205346 		 MVEBU_COMPHY_SERDES_CFG0_PU_TX |
206347 		 MVEBU_COMPHY_SERDES_CFG0_HALF_BUS |
207348 		 MVEBU_COMPHY_SERDES_CFG0_GEN_RX(0xf) |
208
-		 MVEBU_COMPHY_SERDES_CFG0_GEN_TX(0xf));
209
-	if (mode == PHY_MODE_10GKR)
349
+		 MVEBU_COMPHY_SERDES_CFG0_GEN_TX(0xf) |
350
+		 MVEBU_COMPHY_SERDES_CFG0_RXAUI_MODE);
351
+
352
+	switch (lane->submode) {
353
+	case PHY_INTERFACE_MODE_10GBASER:
210354 		val |= MVEBU_COMPHY_SERDES_CFG0_GEN_RX(0xe) |
211355 		       MVEBU_COMPHY_SERDES_CFG0_GEN_TX(0xe);
212
-	else if (mode == PHY_MODE_2500SGMII)
356
+		break;
357
+	case PHY_INTERFACE_MODE_RXAUI:
358
+		val |= MVEBU_COMPHY_SERDES_CFG0_GEN_RX(0xb) |
359
+		       MVEBU_COMPHY_SERDES_CFG0_GEN_TX(0xb) |
360
+		       MVEBU_COMPHY_SERDES_CFG0_RXAUI_MODE;
361
+		break;
362
+	case PHY_INTERFACE_MODE_2500BASEX:
213363 		val |= MVEBU_COMPHY_SERDES_CFG0_GEN_RX(0x8) |
214364 		       MVEBU_COMPHY_SERDES_CFG0_GEN_TX(0x8) |
215365 		       MVEBU_COMPHY_SERDES_CFG0_HALF_BUS;
216
-	else if (mode == PHY_MODE_SGMII)
366
+		break;
367
+	case PHY_INTERFACE_MODE_SGMII:
217368 		val |= MVEBU_COMPHY_SERDES_CFG0_GEN_RX(0x6) |
218369 		       MVEBU_COMPHY_SERDES_CFG0_GEN_TX(0x6) |
219370 		       MVEBU_COMPHY_SERDES_CFG0_HALF_BUS;
371
+		break;
372
+	default:
373
+		dev_err(priv->dev,
374
+			"unsupported comphy submode (%d) on lane %d\n",
375
+			lane->submode,
376
+			lane->id);
377
+		return -ENOTSUPP;
378
+	}
379
+
220380 	writel(val, priv->base + MVEBU_COMPHY_SERDES_CFG0(lane->id));
381
+
382
+	if (lane->submode == PHY_INTERFACE_MODE_RXAUI) {
383
+		regmap_read(priv->regmap, MVEBU_COMPHY_SD1_CTRL1, &val);
384
+
385
+		switch (lane->id) {
386
+		case 2:
387
+		case 3:
388
+			val |= MVEBU_COMPHY_SD1_CTRL1_RXAUI0_EN;
389
+			break;
390
+		case 4:
391
+		case 5:
392
+			val |= MVEBU_COMPHY_SD1_CTRL1_RXAUI1_EN;
393
+			break;
394
+		default:
395
+			dev_err(priv->dev,
396
+				"RXAUI is not supported on comphy lane %d\n",
397
+				lane->id);
398
+			return -EINVAL;
399
+		}
400
+
401
+		regmap_write(priv->regmap, MVEBU_COMPHY_SD1_CTRL1, val);
402
+	}
221403 
222404 	/* reset */
223405 	val = readl(priv->base + MVEBU_COMPHY_SERDES_CFG1(lane->id));
....@@ -243,7 +425,7 @@
243425 	/* refclk selection */
244426 	val = readl(priv->base + MVEBU_COMPHY_MISC_CTRL0(lane->id));
245427 	val &= ~MVEBU_COMPHY_MISC_CTRL0_REFCLK_SEL;
246
-	if (mode == PHY_MODE_10GKR)
428
+	if (lane->submode == PHY_INTERFACE_MODE_10GBASER)
247429 		val |= MVEBU_COMPHY_MISC_CTRL0_ICP_FORCE;
248430 	writel(val, priv->base + MVEBU_COMPHY_MISC_CTRL0(lane->id));
249431 
....@@ -259,10 +441,11 @@
259441 	val &= ~MVEBU_COMPHY_LOOPBACK_DBUS_WIDTH(0x7);
260442 	val |= MVEBU_COMPHY_LOOPBACK_DBUS_WIDTH(0x1);
261443 	writel(val, priv->base + MVEBU_COMPHY_LOOPBACK(lane->id));
444
+
445
+	return 0;
262446 }
263447 
264
-static int mvebu_comphy_init_plls(struct mvebu_comphy_lane *lane,
265
-				  enum phy_mode mode)
448
+static int mvebu_comphy_init_plls(struct mvebu_comphy_lane *lane)
266449 {
267450 	struct mvebu_comphy_priv *priv = lane->priv;
268451 	u32 val;
....@@ -303,22 +486,25 @@
303486 	return 0;
304487 }
305488 
306
-static int mvebu_comphy_set_mode_sgmii(struct phy *phy, enum phy_mode mode)
489
+static int mvebu_comphy_set_mode_sgmii(struct phy *phy)
307490 {
308491 	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
309492 	struct mvebu_comphy_priv *priv = lane->priv;
310493 	u32 val;
494
+	int err;
311495 
312
-	mvebu_comphy_ethernet_init_reset(lane, mode);
496
+	err = mvebu_comphy_ethernet_init_reset(lane);
497
+	if (err)
498
+		return err;
313499 
314500 	val = readl(priv->base + MVEBU_COMPHY_RX_CTRL1(lane->id));
315501 	val &= ~MVEBU_COMPHY_RX_CTRL1_CLK8T_EN;
316502 	val |= MVEBU_COMPHY_RX_CTRL1_RXCLK2X_SEL;
317503 	writel(val, priv->base + MVEBU_COMPHY_RX_CTRL1(lane->id));
318504 
319
-	val = readl(priv->base + MVEBU_COMPHY_DLT_CTRL(lane->id));
320
-	val &= ~MVEBU_COMPHY_DLT_CTRL_DTL_FLOOP_EN;
321
-	writel(val, priv->base + MVEBU_COMPHY_DLT_CTRL(lane->id));
505
+	val = readl(priv->base + MVEBU_COMPHY_DTL_CTRL(lane->id));
506
+	val &= ~MVEBU_COMPHY_DTL_CTRL_DTL_FLOOP_EN;
507
+	writel(val, priv->base + MVEBU_COMPHY_DTL_CTRL(lane->id));
322508 
323509 	regmap_read(priv->regmap, MVEBU_COMPHY_CONF1(lane->id), &val);
324510 	val &= ~MVEBU_COMPHY_CONF1_USB_PCIE;
....@@ -330,25 +516,81 @@
330516 	val |= MVEBU_COMPHY_GEN1_S0_TX_EMPH(0x1);
331517 	writel(val, priv->base + MVEBU_COMPHY_GEN1_S0(lane->id));
332518 
333
-	return mvebu_comphy_init_plls(lane, PHY_MODE_SGMII);
519
+	return mvebu_comphy_init_plls(lane);
334520 }
335521 
336
-static int mvebu_comphy_set_mode_10gkr(struct phy *phy)
522
+static int mvebu_comphy_set_mode_rxaui(struct phy *phy)
337523 {
338524 	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
339525 	struct mvebu_comphy_priv *priv = lane->priv;
340526 	u32 val;
527
+	int err;
341528 
342
-	mvebu_comphy_ethernet_init_reset(lane, PHY_MODE_10GKR);
529
+	err = mvebu_comphy_ethernet_init_reset(lane);
530
+	if (err)
531
+		return err;
343532 
344533 	val = readl(priv->base + MVEBU_COMPHY_RX_CTRL1(lane->id));
345534 	val |= MVEBU_COMPHY_RX_CTRL1_RXCLK2X_SEL |
346535 	       MVEBU_COMPHY_RX_CTRL1_CLK8T_EN;
347536 	writel(val, priv->base + MVEBU_COMPHY_RX_CTRL1(lane->id));
348537 
349
-	val = readl(priv->base + MVEBU_COMPHY_DLT_CTRL(lane->id));
350
-	val |= MVEBU_COMPHY_DLT_CTRL_DTL_FLOOP_EN;
351
-	writel(val, priv->base + MVEBU_COMPHY_DLT_CTRL(lane->id));
538
+	val = readl(priv->base + MVEBU_COMPHY_DTL_CTRL(lane->id));
539
+	val |= MVEBU_COMPHY_DTL_CTRL_DTL_FLOOP_EN;
540
+	writel(val, priv->base + MVEBU_COMPHY_DTL_CTRL(lane->id));
541
+
542
+	val = readl(priv->base + MVEBU_COMPHY_SERDES_CFG2(lane->id));
543
+	val |= MVEBU_COMPHY_SERDES_CFG2_DFE_EN;
544
+	writel(val, priv->base + MVEBU_COMPHY_SERDES_CFG2(lane->id));
545
+
546
+	val = readl(priv->base + MVEBU_COMPHY_DFE_RES(lane->id));
547
+	val |= MVEBU_COMPHY_DFE_RES_FORCE_GEN_TBL;
548
+	writel(val, priv->base + MVEBU_COMPHY_DFE_RES(lane->id));
549
+
550
+	val = readl(priv->base + MVEBU_COMPHY_GEN1_S0(lane->id));
551
+	val &= ~MVEBU_COMPHY_GEN1_S0_TX_EMPH(0xf);
552
+	val |= MVEBU_COMPHY_GEN1_S0_TX_EMPH(0xd);
553
+	writel(val, priv->base + MVEBU_COMPHY_GEN1_S0(lane->id));
554
+
555
+	val = readl(priv->base + MVEBU_COMPHY_GEN1_S1(lane->id));
556
+	val &= ~(MVEBU_COMPHY_GEN1_S1_RX_MUL_PI(0x7) |
557
+		 MVEBU_COMPHY_GEN1_S1_RX_MUL_PF(0x7));
558
+	val |= MVEBU_COMPHY_GEN1_S1_RX_MUL_PI(0x1) |
559
+	       MVEBU_COMPHY_GEN1_S1_RX_MUL_PF(0x1) |
560
+	       MVEBU_COMPHY_GEN1_S1_RX_DFE_EN;
561
+	writel(val, priv->base + MVEBU_COMPHY_GEN1_S1(lane->id));
562
+
563
+	val = readl(priv->base + MVEBU_COMPHY_COEF(lane->id));
564
+	val &= ~(MVEBU_COMPHY_COEF_DFE_EN | MVEBU_COMPHY_COEF_DFE_CTRL);
565
+	writel(val, priv->base + MVEBU_COMPHY_COEF(lane->id));
566
+
567
+	val = readl(priv->base + MVEBU_COMPHY_GEN1_S4(lane->id));
568
+	val &= ~MVEBU_COMPHY_GEN1_S4_DFE_RES(0x3);
569
+	val |= MVEBU_COMPHY_GEN1_S4_DFE_RES(0x1);
570
+	writel(val, priv->base + MVEBU_COMPHY_GEN1_S4(lane->id));
571
+
572
+	return mvebu_comphy_init_plls(lane);
573
+}
574
+
575
+static int mvebu_comphy_set_mode_10gbaser(struct phy *phy)
576
+{
577
+	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
578
+	struct mvebu_comphy_priv *priv = lane->priv;
579
+	u32 val;
580
+	int err;
581
+
582
+	err = mvebu_comphy_ethernet_init_reset(lane);
583
+	if (err)
584
+		return err;
585
+
586
+	val = readl(priv->base + MVEBU_COMPHY_RX_CTRL1(lane->id));
587
+	val |= MVEBU_COMPHY_RX_CTRL1_RXCLK2X_SEL |
588
+	       MVEBU_COMPHY_RX_CTRL1_CLK8T_EN;
589
+	writel(val, priv->base + MVEBU_COMPHY_RX_CTRL1(lane->id));
590
+
591
+	val = readl(priv->base + MVEBU_COMPHY_DTL_CTRL(lane->id));
592
+	val |= MVEBU_COMPHY_DTL_CTRL_DTL_FLOOP_EN;
593
+	writel(val, priv->base + MVEBU_COMPHY_DTL_CTRL(lane->id));
352594 
353595 	/* Speed divider */
354596 	val = readl(priv->base + MVEBU_COMPHY_SPEED_DIV(lane->id));
....@@ -469,17 +711,18 @@
469711 	val |= MVEBU_COMPHY_EXT_SELV_RX_SAMPL(0x1a);
470712 	writel(val, priv->base + MVEBU_COMPHY_EXT_SELV(lane->id));
471713 
472
-	return mvebu_comphy_init_plls(lane, PHY_MODE_10GKR);
714
+	return mvebu_comphy_init_plls(lane);
473715 }
474716 
475
-static int mvebu_comphy_power_on(struct phy *phy)
717
+static int mvebu_comphy_power_on_legacy(struct phy *phy)
476718 {
477719 	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
478720 	struct mvebu_comphy_priv *priv = lane->priv;
479721 	int ret, mux;
480722 	u32 val;
481723 
482
-	mux = mvebu_comphy_get_mux(lane->id, lane->port, lane->mode);
724
+	mux = mvebu_comphy_get_mux(lane->id, lane->port,
725
+				   lane->mode, lane->submode);
483726 	if (mux < 0)
484727 		return -ENOTSUPP;
485728 
....@@ -492,13 +735,16 @@
492735 	val |= mux << MVEBU_COMPHY_SELECTOR_PHY(lane->id);
493736 	regmap_write(priv->regmap, MVEBU_COMPHY_SELECTOR, val);
494737 
495
-	switch (lane->mode) {
496
-	case PHY_MODE_SGMII:
497
-	case PHY_MODE_2500SGMII:
498
-		ret = mvebu_comphy_set_mode_sgmii(phy, lane->mode);
738
+	switch (lane->submode) {
739
+	case PHY_INTERFACE_MODE_SGMII:
740
+	case PHY_INTERFACE_MODE_2500BASEX:
741
+		ret = mvebu_comphy_set_mode_sgmii(phy);
499742 		break;
500
-	case PHY_MODE_10GKR:
501
-		ret = mvebu_comphy_set_mode_10gkr(phy);
743
+	case PHY_INTERFACE_MODE_RXAUI:
744
+		ret = mvebu_comphy_set_mode_rxaui(phy);
745
+		break;
746
+	case PHY_INTERFACE_MODE_10GBASER:
747
+		ret = mvebu_comphy_set_mode_10gbaser(phy);
502748 		break;
503749 	default:
504750 		return -ENOTSUPP;
....@@ -512,18 +758,110 @@
512758 	return ret;
513759 }
514760 
515
-static int mvebu_comphy_set_mode(struct phy *phy, enum phy_mode mode)
761
+static int mvebu_comphy_power_on(struct phy *phy)
762
+{
763
+	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
764
+	struct mvebu_comphy_priv *priv = lane->priv;
765
+	int fw_mode, fw_speed;
766
+	u32 fw_param = 0;
767
+	int ret;
768
+
769
+	fw_mode = mvebu_comphy_get_fw_mode(lane->id, lane->port,
770
+					   lane->mode, lane->submode);
771
+	if (fw_mode < 0)
772
+		goto try_legacy;
773
+
774
+	/* Try SMC flow first */
775
+	switch (lane->mode) {
776
+	case PHY_MODE_ETHERNET:
777
+		switch (lane->submode) {
778
+		case PHY_INTERFACE_MODE_RXAUI:
779
+			dev_dbg(priv->dev, "set lane %d to RXAUI mode\n",
780
+				lane->id);
781
+			fw_speed = 0;
782
+			break;
783
+		case PHY_INTERFACE_MODE_SGMII:
784
+			dev_dbg(priv->dev, "set lane %d to 1000BASE-X mode\n",
785
+				lane->id);
786
+			fw_speed = COMPHY_FW_SPEED_1250;
787
+			break;
788
+		case PHY_INTERFACE_MODE_2500BASEX:
789
+			dev_dbg(priv->dev, "set lane %d to 2500BASE-X mode\n",
790
+				lane->id);
791
+			fw_speed = COMPHY_FW_SPEED_3125;
792
+			break;
793
+		case PHY_INTERFACE_MODE_10GBASER:
794
+			dev_dbg(priv->dev, "set lane %d to 10GBASE-R mode\n",
795
+				lane->id);
796
+			fw_speed = COMPHY_FW_SPEED_103125;
797
+			break;
798
+		default:
799
+			dev_err(priv->dev, "unsupported Ethernet mode (%d)\n",
800
+				lane->submode);
801
+			return -ENOTSUPP;
802
+		}
803
+		fw_param = COMPHY_FW_PARAM_ETH(fw_mode, lane->port, fw_speed);
804
+		break;
805
+	case PHY_MODE_USB_HOST_SS:
806
+	case PHY_MODE_USB_DEVICE_SS:
807
+		dev_dbg(priv->dev, "set lane %d to USB3 mode\n", lane->id);
808
+		fw_param = COMPHY_FW_PARAM(fw_mode, lane->port);
809
+		break;
810
+	case PHY_MODE_SATA:
811
+		dev_dbg(priv->dev, "set lane %d to SATA mode\n", lane->id);
812
+		fw_param = COMPHY_FW_PARAM(fw_mode, lane->port);
813
+		break;
814
+	case PHY_MODE_PCIE:
815
+		dev_dbg(priv->dev, "set lane %d to PCIe mode (x%d)\n", lane->id,
816
+			lane->submode);
817
+		fw_param = COMPHY_FW_PARAM_PCIE(fw_mode, lane->port,
818
+						lane->submode);
819
+		break;
820
+	default:
821
+		dev_err(priv->dev, "unsupported PHY mode (%d)\n", lane->mode);
822
+		return -ENOTSUPP;
823
+	}
824
+
825
+	ret = mvebu_comphy_smc(COMPHY_SIP_POWER_ON, priv->cp_phys, lane->id,
826
+			       fw_param);
827
+	if (!ret)
828
+		return ret;
829
+
830
+	if (ret == -EOPNOTSUPP)
831
+		dev_err(priv->dev,
832
+			"unsupported SMC call, try updating your firmware\n");
833
+
834
+	dev_warn(priv->dev,
835
+		 "Firmware could not configure PHY %d with mode %d (ret: %d), trying legacy method\n",
836
+		 lane->id, lane->mode, ret);
837
+
838
+try_legacy:
839
+	/* Fallback to Linux's implementation */
840
+	return mvebu_comphy_power_on_legacy(phy);
841
+}
842
+
843
+static int mvebu_comphy_set_mode(struct phy *phy,
844
+				 enum phy_mode mode, int submode)
516845 {
517846 	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
518847 
519
-	if (mvebu_comphy_get_mux(lane->id, lane->port, mode) < 0)
848
+	if (submode == PHY_INTERFACE_MODE_1000BASEX)
849
+		submode = PHY_INTERFACE_MODE_SGMII;
850
+
851
+	if (mvebu_comphy_get_fw_mode(lane->id, lane->port, mode, submode) < 0)
520852 		return -EINVAL;
521853 
522854 	lane->mode = mode;
855
+	lane->submode = submode;
856
+
857
+	/* PCIe submode represents the width */
858
+	if (mode == PHY_MODE_PCIE && !lane->submode)
859
+		lane->submode = 1;
860
+
523861 	return 0;
524862 }
525863 
526
-static int mvebu_comphy_power_off(struct phy *phy)
864
+static int mvebu_comphy_power_off_legacy(struct phy *phy)
527865 {
528866 	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
529867 	struct mvebu_comphy_priv *priv = lane->priv;
....@@ -544,6 +882,21 @@
544882 	regmap_write(priv->regmap, MVEBU_COMPHY_PIPE_SELECTOR, val);
545883 
546884 	return 0;
885
+}
886
+
887
+static int mvebu_comphy_power_off(struct phy *phy)
888
+{
889
+	struct mvebu_comphy_lane *lane = phy_get_drvdata(phy);
890
+	struct mvebu_comphy_priv *priv = lane->priv;
891
+	int ret;
892
+
893
+	ret = mvebu_comphy_smc(COMPHY_SIP_POWER_OFF, priv->cp_phys,
894
+			       lane->id, 0);
895
+	if (!ret)
896
+		return ret;
897
+
898
+	/* Fallback to Linux's implementation */
899
+	return mvebu_comphy_power_off_legacy(phy);
547900 }
548901 
549902 static const struct phy_ops mvebu_comphy_ops = {
....@@ -567,11 +920,68 @@
567920 		return phy;
568921 
569922 	lane = phy_get_drvdata(phy);
570
-	if (lane->port >= 0)
571
-		return ERR_PTR(-EBUSY);
572923 	lane->port = args->args[0];
573924 
574925 	return phy;
926
+}
927
+
928
+static int mvebu_comphy_init_clks(struct mvebu_comphy_priv *priv)
929
+{
930
+	int ret;
931
+
932
+	priv->mg_domain_clk = devm_clk_get(priv->dev, "mg_clk");
933
+	if (IS_ERR(priv->mg_domain_clk))
934
+		return PTR_ERR(priv->mg_domain_clk);
935
+
936
+	ret = clk_prepare_enable(priv->mg_domain_clk);
937
+	if (ret < 0)
938
+		return ret;
939
+
940
+	priv->mg_core_clk = devm_clk_get(priv->dev, "mg_core_clk");
941
+	if (IS_ERR(priv->mg_core_clk)) {
942
+		ret = PTR_ERR(priv->mg_core_clk);
943
+		goto dis_mg_domain_clk;
944
+	}
945
+
946
+	ret = clk_prepare_enable(priv->mg_core_clk);
947
+	if (ret < 0)
948
+		goto dis_mg_domain_clk;
949
+
950
+	priv->axi_clk = devm_clk_get(priv->dev, "axi_clk");
951
+	if (IS_ERR(priv->axi_clk)) {
952
+		ret = PTR_ERR(priv->axi_clk);
953
+		goto dis_mg_core_clk;
954
+	}
955
+
956
+	ret = clk_prepare_enable(priv->axi_clk);
957
+	if (ret < 0)
958
+		goto dis_mg_core_clk;
959
+
960
+	return 0;
961
+
962
+dis_mg_core_clk:
963
+	clk_disable_unprepare(priv->mg_core_clk);
964
+
965
+dis_mg_domain_clk:
966
+	clk_disable_unprepare(priv->mg_domain_clk);
967
+
968
+	priv->mg_domain_clk = NULL;
969
+	priv->mg_core_clk = NULL;
970
+	priv->axi_clk = NULL;
971
+
972
+	return ret;
973
+};
974
+
975
+static void mvebu_comphy_disable_unprepare_clks(struct mvebu_comphy_priv *priv)
976
+{
977
+	if (priv->axi_clk)
978
+		clk_disable_unprepare(priv->axi_clk);
979
+
980
+	if (priv->mg_core_clk)
981
+		clk_disable_unprepare(priv->mg_core_clk);
982
+
983
+	if (priv->mg_domain_clk)
984
+		clk_disable_unprepare(priv->mg_domain_clk);
575985 }
576986 
577987 static int mvebu_comphy_probe(struct platform_device *pdev)
....@@ -580,6 +990,7 @@
580990 	struct phy_provider *provider;
581991 	struct device_node *child;
582992 	struct resource *res;
993
+	int ret;
583994 
584995 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
585996 	if (!priv)
....@@ -596,10 +1007,26 @@
5961007 	if (IS_ERR(priv->base))
5971008 		return PTR_ERR(priv->base);
5981009 
1010
+	/*
1011
+	 * Ignore error if clocks have not been initialized properly for DT
1012
+	 * compatibility reasons.
1013
+	 */
1014
+	ret = mvebu_comphy_init_clks(priv);
1015
+	if (ret) {
1016
+		if (ret == -EPROBE_DEFER)
1017
+			return ret;
1018
+		dev_warn(&pdev->dev, "cannot initialize clocks\n");
1019
+	}
1020
+
1021
+	/*
1022
+	 * Hack to retrieve a physical offset relative to this CP that will be
1023
+	 * given to the firmware
1024
+	 */
1025
+	priv->cp_phys = res->start;
1026
+
5991027 	for_each_available_child_of_node(pdev->dev.of_node, child) {
6001028 		struct mvebu_comphy_lane *lane;
6011029 		struct phy *phy;
602
-		int ret;
6031030 		u32 val;
6041031 
6051032 		ret = of_property_read_u32(child, "reg", &val);
....@@ -615,30 +1042,45 @@
6151042 		}
6161043 
6171044 		lane = devm_kzalloc(&pdev->dev, sizeof(*lane), GFP_KERNEL);
618
-		if (!lane)
619
-			return -ENOMEM;
1045
+		if (!lane) {
1046
+			of_node_put(child);
1047
+			ret = -ENOMEM;
1048
+			goto disable_clks;
1049
+		}
6201050 
6211051 		phy = devm_phy_create(&pdev->dev, child, &mvebu_comphy_ops);
622
-		if (IS_ERR(phy))
623
-			return PTR_ERR(phy);
1052
+		if (IS_ERR(phy)) {
1053
+			of_node_put(child);
1054
+			ret = PTR_ERR(phy);
1055
+			goto disable_clks;
1056
+		}
6241057 
6251058 		lane->priv = priv;
6261059 		lane->mode = PHY_MODE_INVALID;
1060
+		lane->submode = PHY_INTERFACE_MODE_NA;
6271061 		lane->id = val;
6281062 		lane->port = -1;
6291063 		phy_set_drvdata(phy, lane);
6301064 
6311065 		/*
632
-		 * Once all modes are supported in this driver we should call
1066
+		 * All modes are supported in this driver so we could call
6331067 		 * mvebu_comphy_power_off(phy) here to avoid relying on the
634
-		 * bootloader/firmware configuration.
1068
+		 * bootloader/firmware configuration, but for compatibility
1069
+		 * reasons we cannot de-configure the COMPHY without being sure
1070
+		 * that the firmware is up-to-date and fully-featured.
6351071 		 */
6361072 	}
6371073 
6381074 	dev_set_drvdata(&pdev->dev, priv);
6391075 	provider = devm_of_phy_provider_register(&pdev->dev,
6401076 						 mvebu_comphy_xlate);
1077
+
6411078 	return PTR_ERR_OR_ZERO(provider);
1079
+
1080
+disable_clks:
1081
+	mvebu_comphy_disable_unprepare_clks(priv);
1082
+
1083
+	return ret;
6421084 }
6431085 
6441086 static const struct of_device_id mvebu_comphy_of_match_table[] = {