forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-10-22 8ac6c7a54ed1b98d142dce24b11c6de6a1e239a5 
 kernel/drivers/net/dsa/mv88e6xxx/port.c
....@@ -1,3 +1,4 @@
1
+// SPDX-License-Identifier: GPL-2.0-or-later
12 /*
23  * Marvell 88E6xxx Switch Port Registers support
34  *
....@@ -5,11 +6,6 @@
56  *
67  * Copyright (c) 2016-2017 Savoir-faire Linux Inc.
78  *	Vivien Didelot <vivien.didelot@savoirfairelinux.com>
8
- *
9
- * This program is free software; you can redistribute it and/or modify
10
- * it under the terms of the GNU General Public License as published by
11
- * the Free Software Foundation; either version 2 of the License, or
12
- * (at your option) any later version.
139  */
1410 
1511 #include <linux/bitfield.h>
....@@ -166,46 +162,9 @@
166162 	return 0;
167163 }
168164 
169
-int mv88e6xxx_port_set_duplex(struct mv88e6xxx_chip *chip, int port, int dup)
170
-{
171
-	u16 reg;
172
-	int err;
173
-
174
-	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_MAC_CTL, &reg);
175
-	if (err)
176
-		return err;
177
-
178
-	reg &= ~(MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX |
179
-		 MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL);
180
-
181
-	switch (dup) {
182
-	case DUPLEX_HALF:
183
-		reg |= MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX;
184
-		break;
185
-	case DUPLEX_FULL:
186
-		reg |= MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX |
187
-			MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL;
188
-		break;
189
-	case DUPLEX_UNFORCED:
190
-		/* normal duplex detection */
191
-		break;
192
-	default:
193
-		return -EOPNOTSUPP;
194
-	}
195
-
196
-	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_MAC_CTL, reg);
197
-	if (err)
198
-		return err;
199
-
200
-	dev_dbg(chip->dev, "p%d: %s %s duplex\n", port,
201
-		reg & MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX ? "Force" : "Unforce",
202
-		reg & MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL ? "full" : "half");
203
-
204
-	return 0;
205
-}
206
-
207
-static int mv88e6xxx_port_set_speed(struct mv88e6xxx_chip *chip, int port,
208
-				    int speed, bool alt_bit, bool force_bit)
165
+static int mv88e6xxx_port_set_speed_duplex(struct mv88e6xxx_chip *chip,
166
+					   int port, int speed, bool alt_bit,
167
+					   bool force_bit, int duplex)
209168 {
210169 	u16 reg, ctrl;
211170 	int err;
....@@ -243,11 +202,29 @@
243202 		return -EOPNOTSUPP;
244203 	}
245204 
205
+	switch (duplex) {
206
+	case DUPLEX_HALF:
207
+		ctrl |= MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX;
208
+		break;
209
+	case DUPLEX_FULL:
210
+		ctrl |= MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX |
211
+			MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL;
212
+		break;
213
+	case DUPLEX_UNFORCED:
214
+		/* normal duplex detection */
215
+		break;
216
+	default:
217
+		return -EOPNOTSUPP;
218
+	}
219
+
246220 	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_MAC_CTL, &reg);
247221 	if (err)
248222 		return err;
249223 
250
-	reg &= ~MV88E6XXX_PORT_MAC_CTL_SPEED_MASK;
224
+	reg &= ~(MV88E6XXX_PORT_MAC_CTL_SPEED_MASK |
225
+		 MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX |
226
+		 MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL);
227
+
251228 	if (alt_bit)
252229 		reg &= ~MV88E6390_PORT_MAC_CTL_ALTSPEED;
253230 	if (force_bit) {
....@@ -265,12 +242,16 @@
265242 		dev_dbg(chip->dev, "p%d: Speed set to %d Mbps\n", port, speed);
266243 	else
267244 		dev_dbg(chip->dev, "p%d: Speed unforced\n", port);
245
+	dev_dbg(chip->dev, "p%d: %s %s duplex\n", port,
246
+		reg & MV88E6XXX_PORT_MAC_CTL_FORCE_DUPLEX ? "Force" : "Unforce",
247
+		reg & MV88E6XXX_PORT_MAC_CTL_DUPLEX_FULL ? "full" : "half");
268248 
269249 	return 0;
270250 }
271251 
272252 /* Support 10, 100, 200 Mbps (e.g. 88E6065 family) */
273
-int mv88e6065_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
253
+int mv88e6065_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
254
+				    int speed, int duplex)
274255 {
275256 	if (speed == SPEED_MAX)
276257 		speed = 200;
....@@ -279,11 +260,13 @@
279260 		return -EOPNOTSUPP;
280261 
281262 	/* Setting 200 Mbps on port 0 to 3 selects 100 Mbps */
282
-	return mv88e6xxx_port_set_speed(chip, port, speed, false, false);
263
+	return mv88e6xxx_port_set_speed_duplex(chip, port, speed, false, false,
264
+					       duplex);
283265 }
284266 
285267 /* Support 10, 100, 1000 Mbps (e.g. 88E6185 family) */
286
-int mv88e6185_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
268
+int mv88e6185_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
269
+				    int speed, int duplex)
287270 {
288271 	if (speed == SPEED_MAX)
289272 		speed = 1000;
....@@ -291,11 +274,27 @@
291274 	if (speed == 200 || speed > 1000)
292275 		return -EOPNOTSUPP;
293276 
294
-	return mv88e6xxx_port_set_speed(chip, port, speed, false, false);
277
+	return mv88e6xxx_port_set_speed_duplex(chip, port, speed, false, false,
278
+					       duplex);
279
+}
280
+
281
+/* Support 10, 100 Mbps (e.g. 88E6250 family) */
282
+int mv88e6250_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
283
+				    int speed, int duplex)
284
+{
285
+	if (speed == SPEED_MAX)
286
+		speed = 100;
287
+
288
+	if (speed > 100)
289
+		return -EOPNOTSUPP;
290
+
291
+	return mv88e6xxx_port_set_speed_duplex(chip, port, speed, false, false,
292
+					       duplex);
295293 }
296294 
297295 /* Support 10, 100, 200, 1000, 2500 Mbps (e.g. 88E6341) */
298
-int mv88e6341_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
296
+int mv88e6341_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
297
+				    int speed, int duplex)
299298 {
300299 	if (speed == SPEED_MAX)
301300 		speed = port < 5 ? 1000 : 2500;
....@@ -309,11 +308,21 @@
309308 	if (speed == 2500 && port < 5)
310309 		return -EOPNOTSUPP;
311310 
312
-	return mv88e6xxx_port_set_speed(chip, port, speed, !port, true);
311
+	return mv88e6xxx_port_set_speed_duplex(chip, port, speed, !port, true,
312
+					       duplex);
313
+}
314
+
315
+phy_interface_t mv88e6341_port_max_speed_mode(int port)
316
+{
317
+	if (port == 5)
318
+		return PHY_INTERFACE_MODE_2500BASEX;
319
+
320
+	return PHY_INTERFACE_MODE_NA;
313321 }
314322 
315323 /* Support 10, 100, 200, 1000 Mbps (e.g. 88E6352 family) */
316
-int mv88e6352_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
324
+int mv88e6352_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
325
+				    int speed, int duplex)
317326 {
318327 	if (speed == SPEED_MAX)
319328 		speed = 1000;
....@@ -324,11 +333,13 @@
324333 	if (speed == 200 && port < 5)
325334 		return -EOPNOTSUPP;
326335 
327
-	return mv88e6xxx_port_set_speed(chip, port, speed, true, false);
336
+	return mv88e6xxx_port_set_speed_duplex(chip, port, speed, true, false,
337
+					       duplex);
328338 }
329339 
330340 /* Support 10, 100, 200, 1000, 2500 Mbps (e.g. 88E6390) */
331
-int mv88e6390_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
341
+int mv88e6390_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
342
+				    int speed, int duplex)
332343 {
333344 	if (speed == SPEED_MAX)
334345 		speed = port < 9 ? 1000 : 2500;
....@@ -342,11 +353,21 @@
342353 	if (speed == 2500 && port < 9)
343354 		return -EOPNOTSUPP;
344355 
345
-	return mv88e6xxx_port_set_speed(chip, port, speed, true, true);
356
+	return mv88e6xxx_port_set_speed_duplex(chip, port, speed, true, true,
357
+					       duplex);
358
+}
359
+
360
+phy_interface_t mv88e6390_port_max_speed_mode(int port)
361
+{
362
+	if (port == 9 || port == 10)
363
+		return PHY_INTERFACE_MODE_2500BASEX;
364
+
365
+	return PHY_INTERFACE_MODE_NA;
346366 }
347367 
348368 /* Support 10, 100, 200, 1000, 2500, 10000 Mbps (e.g. 88E6190X) */
349
-int mv88e6390x_port_set_speed(struct mv88e6xxx_chip *chip, int port, int speed)
369
+int mv88e6390x_port_set_speed_duplex(struct mv88e6xxx_chip *chip, int port,
370
+				     int speed, int duplex)
350371 {
351372 	if (speed == SPEED_MAX)
352373 		speed = port < 9 ? 1000 : 10000;
....@@ -357,26 +378,35 @@
357378 	if (speed >= 2500 && port < 9)
358379 		return -EOPNOTSUPP;
359380 
360
-	return mv88e6xxx_port_set_speed(chip, port, speed, true, true);
381
+	return mv88e6xxx_port_set_speed_duplex(chip, port, speed, true, true,
382
+					       duplex);
361383 }
362384 
363
-int mv88e6390x_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
364
-			      phy_interface_t mode)
385
+phy_interface_t mv88e6390x_port_max_speed_mode(int port)
365386 {
366
-	int lane;
387
+	if (port == 9 || port == 10)
388
+		return PHY_INTERFACE_MODE_XAUI;
389
+
390
+	return PHY_INTERFACE_MODE_NA;
391
+}
392
+
393
+static int mv88e6xxx_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
394
+				    phy_interface_t mode, bool force)
395
+{
396
+	u8 lane;
367397 	u16 cmode;
368398 	u16 reg;
369399 	int err;
370400 
401
+	/* Default to a slow mode, so freeing up SERDES interfaces for
402
+	 * other ports which might use them for SFPs.
403
+	 */
371404 	if (mode == PHY_INTERFACE_MODE_NA)
372
-		return 0;
373
-
374
-	if (port != 9 && port != 10)
375
-		return -EOPNOTSUPP;
405
+		mode = PHY_INTERFACE_MODE_1000BASEX;
376406 
377407 	switch (mode) {
378408 	case PHY_INTERFACE_MODE_1000BASEX:
379
-		cmode = MV88E6XXX_PORT_STS_CMODE_1000BASE_X;
409
+		cmode = MV88E6XXX_PORT_STS_CMODE_1000BASEX;
380410 		break;
381411 	case PHY_INTERFACE_MODE_SGMII:
382412 		cmode = MV88E6XXX_PORT_STS_CMODE_SGMII;
....@@ -395,22 +425,19 @@
395425 		cmode = 0;
396426 	}
397427 
398
-	/* cmode doesn't change, nothing to do for us */
399
-	if (cmode == chip->ports[port].cmode)
428
+	/* cmode doesn't change, nothing to do for us unless forced */
429
+	if (cmode == chip->ports[port].cmode && !force)
400430 		return 0;
401431 
402
-	lane = mv88e6390x_serdes_get_lane(chip, port);
403
-	if (lane < 0 && lane != -ENODEV)
404
-		return lane;
405
-
406
-	if (lane >= 0) {
432
+	lane = mv88e6xxx_serdes_get_lane(chip, port);
433
+	if (lane) {
407434 		if (chip->ports[port].serdes_irq) {
408
-			err = mv88e6390_serdes_irq_disable(chip, port, lane);
435
+			err = mv88e6xxx_serdes_irq_disable(chip, port, lane);
409436 			if (err)
410437 				return err;
411438 		}
412439 
413
-		err = mv88e6390x_serdes_power(chip, port, false);
440
+		err = mv88e6xxx_serdes_power_down(chip, port, lane);
414441 		if (err)
415442 			return err;
416443 	}
....@@ -431,22 +458,102 @@
431458 
432459 		chip->ports[port].cmode = cmode;
433460 
434
-		lane = mv88e6390x_serdes_get_lane(chip, port);
435
-		if (lane < 0)
436
-			return lane;
461
+		lane = mv88e6xxx_serdes_get_lane(chip, port);
462
+		if (!lane)
463
+			return -ENODEV;
437464 
438
-		err = mv88e6390x_serdes_power(chip, port, true);
465
+		err = mv88e6xxx_serdes_power_up(chip, port, lane);
439466 		if (err)
440467 			return err;
441468 
442469 		if (chip->ports[port].serdes_irq) {
443
-			err = mv88e6390_serdes_irq_enable(chip, port, lane);
470
+			err = mv88e6xxx_serdes_irq_enable(chip, port, lane);
444471 			if (err)
445472 				return err;
446473 		}
447474 	}
448475 
449476 	return 0;
477
+}
478
+
479
+int mv88e6390x_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
480
+			      phy_interface_t mode)
481
+{
482
+	if (port != 9 && port != 10)
483
+		return -EOPNOTSUPP;
484
+
485
+	return mv88e6xxx_port_set_cmode(chip, port, mode, false);
486
+}
487
+
488
+int mv88e6390_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
489
+			     phy_interface_t mode)
490
+{
491
+	if (port != 9 && port != 10)
492
+		return -EOPNOTSUPP;
493
+
494
+	switch (mode) {
495
+	case PHY_INTERFACE_MODE_NA:
496
+		return 0;
497
+	case PHY_INTERFACE_MODE_XGMII:
498
+	case PHY_INTERFACE_MODE_XAUI:
499
+	case PHY_INTERFACE_MODE_RXAUI:
500
+		return -EINVAL;
501
+	default:
502
+		break;
503
+	}
504
+
505
+	return mv88e6xxx_port_set_cmode(chip, port, mode, false);
506
+}
507
+
508
+static int mv88e6341_port_set_cmode_writable(struct mv88e6xxx_chip *chip,
509
+					     int port)
510
+{
511
+	int err, addr;
512
+	u16 reg, bits;
513
+
514
+	if (port != 5)
515
+		return -EOPNOTSUPP;
516
+
517
+	addr = chip->info->port_base_addr + port;
518
+
519
+	err = mv88e6xxx_port_hidden_read(chip, 0x7, addr, 0, &reg);
520
+	if (err)
521
+		return err;
522
+
523
+	bits = MV88E6341_PORT_RESERVED_1A_FORCE_CMODE |
524
+	       MV88E6341_PORT_RESERVED_1A_SGMII_AN;
525
+
526
+	if ((reg & bits) == bits)
527
+		return 0;
528
+
529
+	reg |= bits;
530
+	return mv88e6xxx_port_hidden_write(chip, 0x7, addr, 0, reg);
531
+}
532
+
533
+int mv88e6341_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
534
+			     phy_interface_t mode)
535
+{
536
+	int err;
537
+
538
+	if (port != 5)
539
+		return -EOPNOTSUPP;
540
+
541
+	switch (mode) {
542
+	case PHY_INTERFACE_MODE_NA:
543
+		return 0;
544
+	case PHY_INTERFACE_MODE_XGMII:
545
+	case PHY_INTERFACE_MODE_XAUI:
546
+	case PHY_INTERFACE_MODE_RXAUI:
547
+		return -EINVAL;
548
+	default:
549
+		break;
550
+	}
551
+
552
+	err = mv88e6341_port_set_cmode_writable(chip, port);
553
+	if (err)
554
+		return err;
555
+
556
+	return mv88e6xxx_port_set_cmode(chip, port, mode, true);
450557 }
451558 
452559 int mv88e6185_port_get_cmode(struct mv88e6xxx_chip *chip, int port, u8 *cmode)
....@@ -475,80 +582,6 @@
475582 	*cmode = reg & MV88E6XXX_PORT_STS_CMODE_MASK;
476583 
477584 	return 0;
478
-}
479
-
480
-int mv88e6352_port_link_state(struct mv88e6xxx_chip *chip, int port,
481
-			      struct phylink_link_state *state)
482
-{
483
-	int err;
484
-	u16 reg;
485
-
486
-	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_STS, &reg);
487
-	if (err)
488
-		return err;
489
-
490
-	switch (reg & MV88E6XXX_PORT_STS_SPEED_MASK) {
491
-	case MV88E6XXX_PORT_STS_SPEED_10:
492
-		state->speed = SPEED_10;
493
-		break;
494
-	case MV88E6XXX_PORT_STS_SPEED_100:
495
-		state->speed = SPEED_100;
496
-		break;
497
-	case MV88E6XXX_PORT_STS_SPEED_1000:
498
-		state->speed = SPEED_1000;
499
-		break;
500
-	case MV88E6XXX_PORT_STS_SPEED_10000:
501
-		if ((reg & MV88E6XXX_PORT_STS_CMODE_MASK) ==
502
-		    MV88E6XXX_PORT_STS_CMODE_2500BASEX)
503
-			state->speed = SPEED_2500;
504
-		else
505
-			state->speed = SPEED_10000;
506
-		break;
507
-	}
508
-
509
-	state->duplex = reg & MV88E6XXX_PORT_STS_DUPLEX ?
510
-			DUPLEX_FULL : DUPLEX_HALF;
511
-	state->link = !!(reg & MV88E6XXX_PORT_STS_LINK);
512
-	state->an_enabled = 1;
513
-	state->an_complete = state->link;
514
-
515
-	return 0;
516
-}
517
-
518
-int mv88e6185_port_link_state(struct mv88e6xxx_chip *chip, int port,
519
-			      struct phylink_link_state *state)
520
-{
521
-	if (state->interface == PHY_INTERFACE_MODE_1000BASEX) {
522
-		u8 cmode = chip->ports[port].cmode;
523
-
524
-		/* When a port is in "Cross-chip serdes" mode, it uses
525
-		 * 1000Base-X full duplex mode, but there is no automatic
526
-		 * link detection. Use the sync OK status for link (as it
527
-		 * would do for 1000Base-X mode.)
528
-		 */
529
-		if (cmode == MV88E6185_PORT_STS_CMODE_SERDES) {
530
-			u16 mac;
531
-			int err;
532
-
533
-			err = mv88e6xxx_port_read(chip, port,
534
-						  MV88E6XXX_PORT_MAC_CTL, &mac);
535
-			if (err)
536
-				return err;
537
-
538
-			state->link = !!(mac & MV88E6185_PORT_MAC_CTL_SYNC_OK);
539
-			state->an_enabled = 1;
540
-			state->an_complete =
541
-				!!(mac & MV88E6185_PORT_MAC_CTL_AN_DONE);
542
-			state->duplex =
543
-				state->link ? DUPLEX_FULL : DUPLEX_UNKNOWN;
544
-			state->speed =
545
-				state->link ? SPEED_1000 : SPEED_UNKNOWN;
546
-
547
-			return 0;
548
-		}
549
-	}
550
-
551
-	return mv88e6352_port_link_state(chip, port, state);
552585 }
553586 
554587 /* Offset 0x02: Jamming Control
....@@ -969,6 +1002,43 @@
9691002 	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
9701003 }
9711004 
1005
+int mv88e6xxx_port_set_mirror(struct mv88e6xxx_chip *chip, int port,
1006
+			      enum mv88e6xxx_egress_direction direction,
1007
+			      bool mirror)
1008
+{
1009
+	bool *mirror_port;
1010
+	u16 reg;
1011
+	u16 bit;
1012
+	int err;
1013
+
1014
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
1015
+	if (err)
1016
+		return err;
1017
+
1018
+	switch (direction) {
1019
+	case MV88E6XXX_EGRESS_DIR_INGRESS:
1020
+		bit = MV88E6XXX_PORT_CTL2_INGRESS_MONITOR;
1021
+		mirror_port = &chip->ports[port].mirror_ingress;
1022
+		break;
1023
+	case MV88E6XXX_EGRESS_DIR_EGRESS:
1024
+		bit = MV88E6XXX_PORT_CTL2_EGRESS_MONITOR;
1025
+		mirror_port = &chip->ports[port].mirror_egress;
1026
+		break;
1027
+	default:
1028
+		return -EINVAL;
1029
+	}
1030
+
1031
+	reg &= ~bit;
1032
+	if (mirror)
1033
+		reg |= bit;
1034
+
1035
+	err = mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_CTL2, reg);
1036
+	if (!err)
1037
+		*mirror_port = mirror;
1038
+
1039
+	return err;
1040
+}
1041
+
9721042 int mv88e6xxx_port_set_8021q_mode(struct mv88e6xxx_chip *chip, int port,
9731043 				  u16 mode)
9741044 {
....@@ -1011,6 +1081,8 @@
10111081 {
10121082 	u16 reg;
10131083 	int err;
1084
+
1085
+	size += VLAN_ETH_HLEN + ETH_FCS_LEN;
10141086 
10151087 	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_CTL2, &reg);
10161088 	if (err)
....@@ -1129,3 +1201,77 @@
11291201 
11301202 	return 0;
11311203 }
1204
+
1205
+/* Offset 0x0E: Policy Control Register */
1206
+
1207
+int mv88e6352_port_set_policy(struct mv88e6xxx_chip *chip, int port,
1208
+			      enum mv88e6xxx_policy_mapping mapping,
1209
+			      enum mv88e6xxx_policy_action action)
1210
+{
1211
+	u16 reg, mask, val;
1212
+	int shift;
1213
+	int err;
1214
+
1215
+	switch (mapping) {
1216
+	case MV88E6XXX_POLICY_MAPPING_DA:
1217
+		shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_DA_MASK);
1218
+		mask = MV88E6XXX_PORT_POLICY_CTL_DA_MASK;
1219
+		break;
1220
+	case MV88E6XXX_POLICY_MAPPING_SA:
1221
+		shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_SA_MASK);
1222
+		mask = MV88E6XXX_PORT_POLICY_CTL_SA_MASK;
1223
+		break;
1224
+	case MV88E6XXX_POLICY_MAPPING_VTU:
1225
+		shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_VTU_MASK);
1226
+		mask = MV88E6XXX_PORT_POLICY_CTL_VTU_MASK;
1227
+		break;
1228
+	case MV88E6XXX_POLICY_MAPPING_ETYPE:
1229
+		shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_ETYPE_MASK);
1230
+		mask = MV88E6XXX_PORT_POLICY_CTL_ETYPE_MASK;
1231
+		break;
1232
+	case MV88E6XXX_POLICY_MAPPING_PPPOE:
1233
+		shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_PPPOE_MASK);
1234
+		mask = MV88E6XXX_PORT_POLICY_CTL_PPPOE_MASK;
1235
+		break;
1236
+	case MV88E6XXX_POLICY_MAPPING_VBAS:
1237
+		shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_VBAS_MASK);
1238
+		mask = MV88E6XXX_PORT_POLICY_CTL_VBAS_MASK;
1239
+		break;
1240
+	case MV88E6XXX_POLICY_MAPPING_OPT82:
1241
+		shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_OPT82_MASK);
1242
+		mask = MV88E6XXX_PORT_POLICY_CTL_OPT82_MASK;
1243
+		break;
1244
+	case MV88E6XXX_POLICY_MAPPING_UDP:
1245
+		shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_UDP_MASK);
1246
+		mask = MV88E6XXX_PORT_POLICY_CTL_UDP_MASK;
1247
+		break;
1248
+	default:
1249
+		return -EOPNOTSUPP;
1250
+	}
1251
+
1252
+	switch (action) {
1253
+	case MV88E6XXX_POLICY_ACTION_NORMAL:
1254
+		val = MV88E6XXX_PORT_POLICY_CTL_NORMAL;
1255
+		break;
1256
+	case MV88E6XXX_POLICY_ACTION_MIRROR:
1257
+		val = MV88E6XXX_PORT_POLICY_CTL_MIRROR;
1258
+		break;
1259
+	case MV88E6XXX_POLICY_ACTION_TRAP:
1260
+		val = MV88E6XXX_PORT_POLICY_CTL_TRAP;
1261
+		break;
1262
+	case MV88E6XXX_POLICY_ACTION_DISCARD:
1263
+		val = MV88E6XXX_PORT_POLICY_CTL_DISCARD;
1264
+		break;
1265
+	default:
1266
+		return -EOPNOTSUPP;
1267
+	}
1268
+
1269
+	err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_POLICY_CTL, &reg);
1270
+	if (err)
1271
+		return err;
1272
+
1273
+	reg &= ~mask;
1274
+	reg |= (val << shift) & mask;
1275
+
1276
+	return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_POLICY_CTL, reg);
1277
+}