1 // SPDX-License-Identifier: GPL-2.0+
2 
3 #include <linux/netdevice.h>
4 #include <linux/phy/phy.h>
5 
6 #include "lan966x_main.h"
7 
8 /* Watermark encode */
9 #define MULTIPLIER_BIT BIT(8)
10 static u32 lan966x_wm_enc(u32 value)
11 {
12 	value /= LAN966X_BUFFER_CELL_SZ;
13 
14 	if (value >= MULTIPLIER_BIT) {
15 		value /= 16;
16 		if (value >= MULTIPLIER_BIT)
17 			value = (MULTIPLIER_BIT - 1);
18 
19 		value |= MULTIPLIER_BIT;
20 	}
21 
22 	return value;
23 }
24 
25 static void lan966x_port_link_down(struct lan966x_port *port)
26 {
27 	struct lan966x *lan966x = port->lan966x;
28 	u32 val, delay = 0;
29 
30 	/* 0.5: Disable any AFI */
31 	lan_rmw(AFI_PORT_CFG_FC_SKIP_TTI_INJ_SET(1) |
32 		AFI_PORT_CFG_FRM_OUT_MAX_SET(0),
33 		AFI_PORT_CFG_FC_SKIP_TTI_INJ |
34 		AFI_PORT_CFG_FRM_OUT_MAX,
35 		lan966x, AFI_PORT_CFG(port->chip_port));
36 
37 	/* wait for reg afi_port_frm_out to become 0 for the port */
38 	while (true) {
39 		val = lan_rd(lan966x, AFI_PORT_FRM_OUT(port->chip_port));
40 		if (!AFI_PORT_FRM_OUT_FRM_OUT_CNT_GET(val))
41 			break;
42 
43 		usleep_range(USEC_PER_MSEC, 2 * USEC_PER_MSEC);
44 		delay++;
45 		if (delay == 2000) {
46 			pr_err("AFI timeout chip port %u", port->chip_port);
47 			break;
48 		}
49 	}
50 
51 	delay = 0;
52 
53 	/* 1: Reset the PCS Rx clock domain  */
54 	lan_rmw(DEV_CLOCK_CFG_PCS_RX_RST_SET(1),
55 		DEV_CLOCK_CFG_PCS_RX_RST,
56 		lan966x, DEV_CLOCK_CFG(port->chip_port));
57 
58 	/* 2: Disable MAC frame reception */
59 	lan_rmw(DEV_MAC_ENA_CFG_RX_ENA_SET(0),
60 		DEV_MAC_ENA_CFG_RX_ENA,
61 		lan966x, DEV_MAC_ENA_CFG(port->chip_port));
62 
63 	/* 3: Disable traffic being sent to or from switch port */
64 	lan_rmw(QSYS_SW_PORT_MODE_PORT_ENA_SET(0),
65 		QSYS_SW_PORT_MODE_PORT_ENA,
66 		lan966x, QSYS_SW_PORT_MODE(port->chip_port));
67 
68 	/* 4: Disable dequeuing from the egress queues  */
69 	lan_rmw(QSYS_PORT_MODE_DEQUEUE_DIS_SET(1),
70 		QSYS_PORT_MODE_DEQUEUE_DIS,
71 		lan966x, QSYS_PORT_MODE(port->chip_port));
72 
73 	/* 5: Disable Flowcontrol */
74 	lan_rmw(SYS_PAUSE_CFG_PAUSE_ENA_SET(0),
75 		SYS_PAUSE_CFG_PAUSE_ENA,
76 		lan966x, SYS_PAUSE_CFG(port->chip_port));
77 
78 	/* 5.1: Disable PFC */
79 	lan_rmw(QSYS_SW_PORT_MODE_TX_PFC_ENA_SET(0),
80 		QSYS_SW_PORT_MODE_TX_PFC_ENA,
81 		lan966x, QSYS_SW_PORT_MODE(port->chip_port));
82 
83 	/* 6: Wait a worst case time 8ms (jumbo/10Mbit) */
84 	usleep_range(8 * USEC_PER_MSEC, 9 * USEC_PER_MSEC);
85 
86 	/* 7: Disable HDX backpressure */
87 	lan_rmw(SYS_FRONT_PORT_MODE_HDX_MODE_SET(0),
88 		SYS_FRONT_PORT_MODE_HDX_MODE,
89 		lan966x, SYS_FRONT_PORT_MODE(port->chip_port));
90 
91 	/* 8: Flush the queues accociated with the port */
92 	lan_rmw(QSYS_SW_PORT_MODE_AGING_MODE_SET(3),
93 		QSYS_SW_PORT_MODE_AGING_MODE,
94 		lan966x, QSYS_SW_PORT_MODE(port->chip_port));
95 
96 	/* 9: Enable dequeuing from the egress queues */
97 	lan_rmw(QSYS_PORT_MODE_DEQUEUE_DIS_SET(0),
98 		QSYS_PORT_MODE_DEQUEUE_DIS,
99 		lan966x, QSYS_PORT_MODE(port->chip_port));
100 
101 	/* 10: Wait until flushing is complete */
102 	while (true) {
103 		val = lan_rd(lan966x, QSYS_SW_STATUS(port->chip_port));
104 		if (!QSYS_SW_STATUS_EQ_AVAIL_GET(val))
105 			break;
106 
107 		usleep_range(USEC_PER_MSEC, 2 * USEC_PER_MSEC);
108 		delay++;
109 		if (delay == 2000) {
110 			pr_err("Flush timeout chip port %u", port->chip_port);
111 			break;
112 		}
113 	}
114 
115 	/* 11: Reset the Port and MAC clock domains */
116 	lan_rmw(DEV_MAC_ENA_CFG_TX_ENA_SET(0),
117 		DEV_MAC_ENA_CFG_TX_ENA,
118 		lan966x, DEV_MAC_ENA_CFG(port->chip_port));
119 
120 	lan_rmw(DEV_CLOCK_CFG_PORT_RST_SET(1),
121 		DEV_CLOCK_CFG_PORT_RST,
122 		lan966x, DEV_CLOCK_CFG(port->chip_port));
123 
124 	usleep_range(USEC_PER_MSEC, 2 * USEC_PER_MSEC);
125 
126 	lan_rmw(DEV_CLOCK_CFG_MAC_TX_RST_SET(1) |
127 		DEV_CLOCK_CFG_MAC_RX_RST_SET(1) |
128 		DEV_CLOCK_CFG_PORT_RST_SET(1),
129 		DEV_CLOCK_CFG_MAC_TX_RST |
130 		DEV_CLOCK_CFG_MAC_RX_RST |
131 		DEV_CLOCK_CFG_PORT_RST,
132 		lan966x, DEV_CLOCK_CFG(port->chip_port));
133 
134 	/* 12: Clear flushing */
135 	lan_rmw(QSYS_SW_PORT_MODE_AGING_MODE_SET(2),
136 		QSYS_SW_PORT_MODE_AGING_MODE,
137 		lan966x, QSYS_SW_PORT_MODE(port->chip_port));
138 
139 	/* The port is disabled and flushed, now set up the port in the
140 	 * new operating mode
141 	 */
142 }
143 
144 static void lan966x_port_link_up(struct lan966x_port *port)
145 {
146 	struct lan966x_port_config *config = &port->config;
147 	struct lan966x *lan966x = port->lan966x;
148 	int speed = 0, mode = 0;
149 	int atop_wm = 0;
150 
151 	switch (config->speed) {
152 	case SPEED_10:
153 		speed = LAN966X_SPEED_10;
154 		break;
155 	case SPEED_100:
156 		speed = LAN966X_SPEED_100;
157 		break;
158 	case SPEED_1000:
159 		speed = LAN966X_SPEED_1000;
160 		mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA_SET(1);
161 		break;
162 	case SPEED_2500:
163 		speed = LAN966X_SPEED_2500;
164 		mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA_SET(1);
165 		break;
166 	}
167 
168 	lan966x_taprio_speed_set(port, config->speed);
169 
170 	/* Also the GIGA_MODE_ENA(1) needs to be set regardless of the
171 	 * port speed for QSGMII or SGMII ports.
172 	 */
173 	if (phy_interface_num_ports(config->portmode) == 4 ||
174 	    config->portmode == PHY_INTERFACE_MODE_SGMII)
175 		mode = DEV_MAC_MODE_CFG_GIGA_MODE_ENA_SET(1);
176 
177 	lan_wr(config->duplex | mode,
178 	       lan966x, DEV_MAC_MODE_CFG(port->chip_port));
179 
180 	lan_rmw(DEV_MAC_IFG_CFG_TX_IFG_SET(config->duplex ? 6 : 5) |
181 		DEV_MAC_IFG_CFG_RX_IFG1_SET(config->speed == SPEED_10 ? 2 : 1) |
182 		DEV_MAC_IFG_CFG_RX_IFG2_SET(2),
183 		DEV_MAC_IFG_CFG_TX_IFG |
184 		DEV_MAC_IFG_CFG_RX_IFG1 |
185 		DEV_MAC_IFG_CFG_RX_IFG2,
186 		lan966x, DEV_MAC_IFG_CFG(port->chip_port));
187 
188 	lan_rmw(DEV_MAC_HDX_CFG_SEED_SET(4) |
189 		DEV_MAC_HDX_CFG_SEED_LOAD_SET(1),
190 		DEV_MAC_HDX_CFG_SEED |
191 		DEV_MAC_HDX_CFG_SEED_LOAD,
192 		lan966x, DEV_MAC_HDX_CFG(port->chip_port));
193 
194 	if (config->portmode == PHY_INTERFACE_MODE_GMII) {
195 		if (config->speed == SPEED_1000)
196 			lan_rmw(CHIP_TOP_CUPHY_PORT_CFG_GTX_CLK_ENA_SET(1),
197 				CHIP_TOP_CUPHY_PORT_CFG_GTX_CLK_ENA,
198 				lan966x,
199 				CHIP_TOP_CUPHY_PORT_CFG(port->chip_port));
200 		else
201 			lan_rmw(CHIP_TOP_CUPHY_PORT_CFG_GTX_CLK_ENA_SET(0),
202 				CHIP_TOP_CUPHY_PORT_CFG_GTX_CLK_ENA,
203 				lan966x,
204 				CHIP_TOP_CUPHY_PORT_CFG(port->chip_port));
205 	}
206 
207 	/* No PFC */
208 	lan_wr(ANA_PFC_CFG_FC_LINK_SPEED_SET(speed),
209 	       lan966x, ANA_PFC_CFG(port->chip_port));
210 
211 	lan_rmw(DEV_PCS1G_CFG_PCS_ENA_SET(1),
212 		DEV_PCS1G_CFG_PCS_ENA,
213 		lan966x, DEV_PCS1G_CFG(port->chip_port));
214 
215 	lan_rmw(DEV_PCS1G_SD_CFG_SD_ENA_SET(0),
216 		DEV_PCS1G_SD_CFG_SD_ENA,
217 		lan966x, DEV_PCS1G_SD_CFG(port->chip_port));
218 
219 	/* Set Pause WM hysteresis, start/stop are in 1518 byte units */
220 	lan_wr(SYS_PAUSE_CFG_PAUSE_ENA_SET(1) |
221 	       SYS_PAUSE_CFG_PAUSE_STOP_SET(lan966x_wm_enc(4 * 1518)) |
222 	       SYS_PAUSE_CFG_PAUSE_START_SET(lan966x_wm_enc(6 * 1518)),
223 	       lan966x, SYS_PAUSE_CFG(port->chip_port));
224 
225 	/* Set SMAC of Pause frame (00:00:00:00:00:00) */
226 	lan_wr(0, lan966x, DEV_FC_MAC_LOW_CFG(port->chip_port));
227 	lan_wr(0, lan966x, DEV_FC_MAC_HIGH_CFG(port->chip_port));
228 
229 	/* Flow control */
230 	lan_rmw(SYS_MAC_FC_CFG_FC_LINK_SPEED_SET(speed) |
231 		SYS_MAC_FC_CFG_FC_LATENCY_CFG_SET(7) |
232 		SYS_MAC_FC_CFG_ZERO_PAUSE_ENA_SET(1) |
233 		SYS_MAC_FC_CFG_PAUSE_VAL_CFG_SET(0xffff) |
234 		SYS_MAC_FC_CFG_RX_FC_ENA_SET(config->pause & MLO_PAUSE_RX ? 1 : 0) |
235 		SYS_MAC_FC_CFG_TX_FC_ENA_SET(config->pause & MLO_PAUSE_TX ? 1 : 0),
236 		SYS_MAC_FC_CFG_FC_LINK_SPEED |
237 		SYS_MAC_FC_CFG_FC_LATENCY_CFG |
238 		SYS_MAC_FC_CFG_ZERO_PAUSE_ENA |
239 		SYS_MAC_FC_CFG_PAUSE_VAL_CFG |
240 		SYS_MAC_FC_CFG_RX_FC_ENA |
241 		SYS_MAC_FC_CFG_TX_FC_ENA,
242 		lan966x, SYS_MAC_FC_CFG(port->chip_port));
243 
244 	/* Tail dropping watermark */
245 	atop_wm = lan966x->shared_queue_sz;
246 
247 	/* The total memory size is diveded by number of front ports plus CPU
248 	 * port
249 	 */
250 	lan_wr(lan966x_wm_enc(atop_wm / lan966x->num_phys_ports + 1), lan966x,
251 	       SYS_ATOP(port->chip_port));
252 	lan_wr(lan966x_wm_enc(atop_wm), lan966x, SYS_ATOP_TOT_CFG);
253 
254 	/* This needs to be at the end */
255 	/* Enable MAC module */
256 	lan_wr(DEV_MAC_ENA_CFG_RX_ENA_SET(1) |
257 	       DEV_MAC_ENA_CFG_TX_ENA_SET(1),
258 	       lan966x, DEV_MAC_ENA_CFG(port->chip_port));
259 
260 	/* Take out the clock from reset */
261 	lan_wr(DEV_CLOCK_CFG_LINK_SPEED_SET(speed),
262 	       lan966x, DEV_CLOCK_CFG(port->chip_port));
263 
264 	/* Core: Enable port for frame transfer */
265 	lan_wr(QSYS_SW_PORT_MODE_PORT_ENA_SET(1) |
266 	       QSYS_SW_PORT_MODE_SCH_NEXT_CFG_SET(1) |
267 	       QSYS_SW_PORT_MODE_INGRESS_DROP_MODE_SET(1),
268 	       lan966x, QSYS_SW_PORT_MODE(port->chip_port));
269 
270 	lan_rmw(AFI_PORT_CFG_FC_SKIP_TTI_INJ_SET(0) |
271 		AFI_PORT_CFG_FRM_OUT_MAX_SET(16),
272 		AFI_PORT_CFG_FC_SKIP_TTI_INJ |
273 		AFI_PORT_CFG_FRM_OUT_MAX,
274 		lan966x, AFI_PORT_CFG(port->chip_port));
275 }
276 
277 void lan966x_port_config_down(struct lan966x_port *port)
278 {
279 	lan966x_port_link_down(port);
280 }
281 
282 void lan966x_port_config_up(struct lan966x_port *port)
283 {
284 	lan966x_port_link_up(port);
285 }
286 
287 void lan966x_port_status_get(struct lan966x_port *port,
288 			     struct phylink_link_state *state)
289 {
290 	struct lan966x *lan966x = port->lan966x;
291 	bool link_down;
292 	u16 bmsr = 0;
293 	u16 lp_adv;
294 	u32 val;
295 
296 	val = lan_rd(lan966x, DEV_PCS1G_STICKY(port->chip_port));
297 	link_down = DEV_PCS1G_STICKY_LINK_DOWN_STICKY_GET(val);
298 	if (link_down)
299 		lan_wr(val, lan966x, DEV_PCS1G_STICKY(port->chip_port));
300 
301 	/* Get both current Link and Sync status */
302 	val = lan_rd(lan966x, DEV_PCS1G_LINK_STATUS(port->chip_port));
303 	state->link = DEV_PCS1G_LINK_STATUS_LINK_STATUS_GET(val) &&
304 		      DEV_PCS1G_LINK_STATUS_SYNC_STATUS_GET(val);
305 	state->link &= !link_down;
306 
307 	/* Get PCS ANEG status register */
308 	val = lan_rd(lan966x, DEV_PCS1G_ANEG_STATUS(port->chip_port));
309 	/* Aneg complete provides more information  */
310 	if (DEV_PCS1G_ANEG_STATUS_ANEG_COMPLETE_GET(val)) {
311 		state->an_complete = true;
312 
313 		bmsr |= state->link ? BMSR_LSTATUS : 0;
314 		bmsr |= BMSR_ANEGCOMPLETE;
315 
316 		lp_adv = DEV_PCS1G_ANEG_STATUS_LP_ADV_GET(val);
317 		phylink_mii_c22_pcs_decode_state(state, bmsr, lp_adv);
318 	} else {
319 		if (!state->link)
320 			return;
321 
322 		if (state->interface == PHY_INTERFACE_MODE_1000BASEX)
323 			state->speed = SPEED_1000;
324 		else if (state->interface == PHY_INTERFACE_MODE_2500BASEX)
325 			state->speed = SPEED_2500;
326 
327 		state->duplex = DUPLEX_FULL;
328 	}
329 }
330 
331 int lan966x_port_pcs_set(struct lan966x_port *port,
332 			 struct lan966x_port_config *config)
333 {
334 	struct lan966x *lan966x = port->lan966x;
335 	bool inband_aneg = false;
336 	bool outband;
337 	bool full_preamble = false;
338 
339 	if (config->portmode == PHY_INTERFACE_MODE_QUSGMII)
340 		full_preamble = true;
341 
342 	if (config->inband) {
343 		if (config->portmode == PHY_INTERFACE_MODE_SGMII ||
344 		    phy_interface_num_ports(config->portmode) == 4)
345 			inband_aneg = true; /* Cisco-SGMII in-band-aneg */
346 		else if (config->portmode == PHY_INTERFACE_MODE_1000BASEX &&
347 			 config->autoneg)
348 			inband_aneg = true; /* Clause-37 in-band-aneg */
349 
350 		outband = false;
351 	} else {
352 		outband = true;
353 	}
354 
355 	/* Disable or enable inband.
356 	 * For QUSGMII, we rely on the preamble to transmit data such as
357 	 * timestamps, therefore force full preamble transmission, and prevent
358 	 * premable shortening
359 	 */
360 	lan_rmw(DEV_PCS1G_MODE_CFG_SGMII_MODE_ENA_SET(outband) |
361 		DEV_PCS1G_MODE_CFG_SAVE_PREAMBLE_ENA_SET(full_preamble),
362 		DEV_PCS1G_MODE_CFG_SGMII_MODE_ENA |
363 		DEV_PCS1G_MODE_CFG_SAVE_PREAMBLE_ENA,
364 		lan966x, DEV_PCS1G_MODE_CFG(port->chip_port));
365 
366 	/* Enable PCS */
367 	lan_wr(DEV_PCS1G_CFG_PCS_ENA_SET(1),
368 	       lan966x, DEV_PCS1G_CFG(port->chip_port));
369 
370 	if (inband_aneg) {
371 		int adv = phylink_mii_c22_pcs_encode_advertisement(config->portmode,
372 								   config->advertising);
373 		if (adv >= 0)
374 			/* Enable in-band aneg */
375 			lan_wr(DEV_PCS1G_ANEG_CFG_ADV_ABILITY_SET(adv) |
376 			       DEV_PCS1G_ANEG_CFG_SW_RESOLVE_ENA_SET(1) |
377 			       DEV_PCS1G_ANEG_CFG_ENA_SET(1) |
378 			       DEV_PCS1G_ANEG_CFG_RESTART_ONE_SHOT_SET(1),
379 			       lan966x, DEV_PCS1G_ANEG_CFG(port->chip_port));
380 	} else {
381 		lan_wr(0, lan966x, DEV_PCS1G_ANEG_CFG(port->chip_port));
382 	}
383 
384 	/* Take PCS out of reset */
385 	lan_rmw(DEV_CLOCK_CFG_LINK_SPEED_SET(LAN966X_SPEED_1000) |
386 		DEV_CLOCK_CFG_PCS_RX_RST_SET(0) |
387 		DEV_CLOCK_CFG_PCS_TX_RST_SET(0),
388 		DEV_CLOCK_CFG_LINK_SPEED |
389 		DEV_CLOCK_CFG_PCS_RX_RST |
390 		DEV_CLOCK_CFG_PCS_TX_RST,
391 		lan966x, DEV_CLOCK_CFG(port->chip_port));
392 
393 	port->config = *config;
394 
395 	return 0;
396 }
397 
398 static void lan966x_port_qos_pcp_set(struct lan966x_port *port,
399 				     struct lan966x_port_qos_pcp *qos)
400 {
401 	u8 *pcp_itr = qos->map;
402 	u8 pcp, dp;
403 
404 	lan_rmw(ANA_QOS_CFG_QOS_PCP_ENA_SET(qos->enable),
405 		ANA_QOS_CFG_QOS_PCP_ENA,
406 		port->lan966x, ANA_QOS_CFG(port->chip_port));
407 
408 	/* Map PCP and DEI to priority */
409 	for (int i = 0; i < ARRAY_SIZE(qos->map); i++) {
410 		pcp = *(pcp_itr + i);
411 		dp = (i < LAN966X_PORT_QOS_PCP_COUNT) ? 0 : 1;
412 
413 		lan_rmw(ANA_PCP_DEI_CFG_QOS_PCP_DEI_VAL_SET(pcp) |
414 			ANA_PCP_DEI_CFG_DP_PCP_DEI_VAL_SET(dp),
415 			ANA_PCP_DEI_CFG_QOS_PCP_DEI_VAL |
416 			ANA_PCP_DEI_CFG_DP_PCP_DEI_VAL,
417 			port->lan966x,
418 			ANA_PCP_DEI_CFG(port->chip_port, i));
419 	}
420 }
421 
422 static void lan966x_port_qos_dscp_set(struct lan966x_port *port,
423 				      struct lan966x_port_qos_dscp *qos)
424 {
425 	struct lan966x *lan966x = port->lan966x;
426 
427 	/* Enable/disable dscp for qos classification. */
428 	lan_rmw(ANA_QOS_CFG_QOS_DSCP_ENA_SET(qos->enable),
429 		ANA_QOS_CFG_QOS_DSCP_ENA,
430 		lan966x, ANA_QOS_CFG(port->chip_port));
431 
432 	/* Map each dscp value to priority and dp */
433 	for (int i = 0; i < ARRAY_SIZE(qos->map); i++)
434 		lan_rmw(ANA_DSCP_CFG_DP_DSCP_VAL_SET(0) |
435 			ANA_DSCP_CFG_QOS_DSCP_VAL_SET(*(qos->map + i)),
436 			ANA_DSCP_CFG_DP_DSCP_VAL |
437 			ANA_DSCP_CFG_QOS_DSCP_VAL,
438 			lan966x, ANA_DSCP_CFG(i));
439 
440 	/* Set per-dscp trust */
441 	for (int i = 0; i <  ARRAY_SIZE(qos->map); i++)
442 		lan_rmw(ANA_DSCP_CFG_DSCP_TRUST_ENA_SET(qos->enable),
443 			ANA_DSCP_CFG_DSCP_TRUST_ENA,
444 			lan966x, ANA_DSCP_CFG(i));
445 }
446 
447 static int lan966x_port_qos_default_set(struct lan966x_port *port,
448 					struct lan966x_port_qos *qos)
449 {
450 	/* Set default prio and dp level */
451 	lan_rmw(ANA_QOS_CFG_DP_DEFAULT_VAL_SET(0) |
452 		ANA_QOS_CFG_QOS_DEFAULT_VAL_SET(qos->default_prio),
453 		ANA_QOS_CFG_DP_DEFAULT_VAL |
454 		ANA_QOS_CFG_QOS_DEFAULT_VAL,
455 		port->lan966x, ANA_QOS_CFG(port->chip_port));
456 
457 	/* Set default pcp and dei for untagged frames */
458 	lan_rmw(ANA_VLAN_CFG_VLAN_DEI_SET(0) |
459 		ANA_VLAN_CFG_VLAN_PCP_SET(0),
460 		ANA_VLAN_CFG_VLAN_DEI |
461 		ANA_VLAN_CFG_VLAN_PCP,
462 		port->lan966x, ANA_VLAN_CFG(port->chip_port));
463 
464 	return 0;
465 }
466 
467 static void lan966x_port_qos_pcp_rewr_set(struct lan966x_port *port,
468 					  struct lan966x_port_qos_pcp_rewr *qos)
469 {
470 	u8 mode = LAN966X_PORT_REW_TAG_CTRL_CLASSIFIED;
471 	u8 pcp, dei;
472 
473 	if (qos->enable)
474 		mode = LAN966X_PORT_REW_TAG_CTRL_MAPPED;
475 
476 	/* Map the values only if it is enabled otherwise will be the classified
477 	 * value
478 	 */
479 	lan_rmw(REW_TAG_CFG_TAG_PCP_CFG_SET(mode) |
480 		REW_TAG_CFG_TAG_DEI_CFG_SET(mode),
481 		REW_TAG_CFG_TAG_PCP_CFG |
482 		REW_TAG_CFG_TAG_DEI_CFG,
483 		port->lan966x, REW_TAG_CFG(port->chip_port));
484 
485 	/* Map each value to pcp and dei */
486 	for (int i = 0; i < ARRAY_SIZE(qos->map); i++) {
487 		pcp = qos->map[i];
488 		if (pcp > LAN966X_PORT_QOS_PCP_COUNT)
489 			dei = 1;
490 		else
491 			dei = 0;
492 
493 		lan_rmw(REW_PCP_DEI_CFG_DEI_QOS_VAL_SET(dei) |
494 			REW_PCP_DEI_CFG_PCP_QOS_VAL_SET(pcp),
495 			REW_PCP_DEI_CFG_DEI_QOS_VAL |
496 			REW_PCP_DEI_CFG_PCP_QOS_VAL,
497 			port->lan966x,
498 			REW_PCP_DEI_CFG(port->chip_port,
499 					i + dei * LAN966X_PORT_QOS_PCP_COUNT));
500 	}
501 }
502 
503 static void lan966x_port_qos_dscp_rewr_set(struct lan966x_port *port,
504 					   struct lan966x_port_qos_dscp_rewr *qos)
505 {
506 	u16 dscp;
507 	u8 mode;
508 
509 	if (qos->enable)
510 		mode = LAN966X_PORT_REW_DSCP_ANALIZER;
511 	else
512 		mode = LAN966X_PORT_REW_DSCP_FRAME;
513 
514 	/* Enable the rewrite otherwise will use the values from the frame */
515 	lan_rmw(REW_DSCP_CFG_DSCP_REWR_CFG_SET(mode),
516 		REW_DSCP_CFG_DSCP_REWR_CFG,
517 		port->lan966x, REW_DSCP_CFG(port->chip_port));
518 
519 	/* Map each classified Qos class and DP to classified DSCP value */
520 	for (int i = 0; i < ARRAY_SIZE(qos->map); i++) {
521 		dscp = qos->map[i];
522 
523 		lan_rmw(ANA_DSCP_REWR_CFG_DSCP_QOS_REWR_VAL_SET(dscp),
524 			ANA_DSCP_REWR_CFG_DSCP_QOS_REWR_VAL,
525 			port->lan966x, ANA_DSCP_REWR_CFG(i));
526 	}
527 }
528 
529 void lan966x_port_qos_dscp_rewr_mode_set(struct lan966x_port *port,
530 					 int mode)
531 {
532 	lan_rmw(ANA_QOS_CFG_DSCP_REWR_CFG_SET(mode),
533 		ANA_QOS_CFG_DSCP_REWR_CFG,
534 		port->lan966x, ANA_QOS_CFG(port->chip_port));
535 }
536 
537 void lan966x_port_qos_set(struct lan966x_port *port,
538 			  struct lan966x_port_qos *qos)
539 {
540 	lan966x_port_qos_pcp_set(port, &qos->pcp);
541 	lan966x_port_qos_dscp_set(port, &qos->dscp);
542 	lan966x_port_qos_default_set(port, qos);
543 	lan966x_port_qos_pcp_rewr_set(port, &qos->pcp_rewr);
544 	lan966x_port_qos_dscp_rewr_set(port, &qos->dscp_rewr);
545 }
546 
547 void lan966x_port_init(struct lan966x_port *port)
548 {
549 	struct lan966x_port_config *config = &port->config;
550 	struct lan966x *lan966x = port->lan966x;
551 
552 	lan_rmw(ANA_PORT_CFG_LEARN_ENA_SET(0),
553 		ANA_PORT_CFG_LEARN_ENA,
554 		lan966x, ANA_PORT_CFG(port->chip_port));
555 
556 	lan966x_port_config_down(port);
557 
558 	if (lan966x->fdma)
559 		lan966x_fdma_netdev_init(lan966x, port->dev);
560 
561 	if (phy_interface_num_ports(config->portmode) != 4)
562 		return;
563 
564 	lan_rmw(DEV_CLOCK_CFG_PCS_RX_RST_SET(0) |
565 		DEV_CLOCK_CFG_PCS_TX_RST_SET(0) |
566 		DEV_CLOCK_CFG_LINK_SPEED_SET(LAN966X_SPEED_1000),
567 		DEV_CLOCK_CFG_PCS_RX_RST |
568 		DEV_CLOCK_CFG_PCS_TX_RST |
569 		DEV_CLOCK_CFG_LINK_SPEED,
570 		lan966x, DEV_CLOCK_CFG(port->chip_port));
571 }
572