]> git.sur5r.net Git - u-boot/blob - drivers/net/vsc9953.c
arm: imx6ul: Add Engicam GEAM6UL Starter Kit initial support
[u-boot] / drivers / net / vsc9953.c
1 /*
2  *  Copyright 2014 - 2015 Freescale Semiconductor, Inc.
3  *
4  *  SPDX-License-Identifier:      GPL-2.0+
5  *
6  *  Driver for the Vitesse VSC9953 L2 Switch
7  */
8
9 #include <asm/io.h>
10 #include <asm/fsl_serdes.h>
11 #include <fm_eth.h>
12 #include <fsl_memac.h>
13 #include <bitfield.h>
14 #include <errno.h>
15 #include <malloc.h>
16 #include <vsc9953.h>
17 #include <ethsw.h>
18
19 static struct vsc9953_info vsc9953_l2sw = {
20                 .port[0] = VSC9953_PORT_INFO_INITIALIZER(0),
21                 .port[1] = VSC9953_PORT_INFO_INITIALIZER(1),
22                 .port[2] = VSC9953_PORT_INFO_INITIALIZER(2),
23                 .port[3] = VSC9953_PORT_INFO_INITIALIZER(3),
24                 .port[4] = VSC9953_PORT_INFO_INITIALIZER(4),
25                 .port[5] = VSC9953_PORT_INFO_INITIALIZER(5),
26                 .port[6] = VSC9953_PORT_INFO_INITIALIZER(6),
27                 .port[7] = VSC9953_PORT_INFO_INITIALIZER(7),
28                 .port[8] = VSC9953_PORT_INFO_INITIALIZER(8),
29                 .port[9] = VSC9953_PORT_INFO_INITIALIZER(9),
30 };
31
32 void vsc9953_port_info_set_mdio(int port_no, struct mii_dev *bus)
33 {
34         if (!VSC9953_PORT_CHECK(port_no))
35                 return;
36
37         vsc9953_l2sw.port[port_no].bus = bus;
38 }
39
40 void vsc9953_port_info_set_phy_address(int port_no, int address)
41 {
42         if (!VSC9953_PORT_CHECK(port_no))
43                 return;
44
45         vsc9953_l2sw.port[port_no].phyaddr = address;
46 }
47
48 void vsc9953_port_info_set_phy_int(int port_no, phy_interface_t phy_int)
49 {
50         if (!VSC9953_PORT_CHECK(port_no))
51                 return;
52
53         vsc9953_l2sw.port[port_no].enet_if = phy_int;
54 }
55
56 void vsc9953_port_enable(int port_no)
57 {
58         if (!VSC9953_PORT_CHECK(port_no))
59                 return;
60
61         vsc9953_l2sw.port[port_no].enabled = 1;
62 }
63
64 void vsc9953_port_disable(int port_no)
65 {
66         if (!VSC9953_PORT_CHECK(port_no))
67                 return;
68
69         vsc9953_l2sw.port[port_no].enabled = 0;
70 }
71
72 static void vsc9953_mdio_write(struct vsc9953_mii_mng *phyregs, int port_addr,
73                 int regnum, int value)
74 {
75         int timeout = 50000;
76
77         out_le32(&phyregs->miimcmd, (0x1 << 31) | ((port_addr & 0x1f) << 25) |
78                         ((regnum & 0x1f) << 20) | ((value & 0xffff) << 4) |
79                         (0x1 << 1));
80         asm("sync");
81
82         while ((in_le32(&phyregs->miimstatus) & 0x8) && --timeout)
83                 udelay(1);
84
85         if (timeout == 0)
86                 debug("Timeout waiting for MDIO write\n");
87 }
88
89 static int vsc9953_mdio_read(struct vsc9953_mii_mng *phyregs, int port_addr,
90                 int regnum)
91 {
92         int value = 0xFFFF;
93         int timeout = 50000;
94
95         while ((in_le32(&phyregs->miimstatus) & MIIMIND_OPR_PEND) && --timeout)
96                 udelay(1);
97         if (timeout == 0) {
98                 debug("Timeout waiting for MDIO operation to finish\n");
99                 return value;
100         }
101
102         /* Put the address of the phy, and the register
103          * number into MIICMD
104          */
105         out_le32(&phyregs->miimcmd, (0x1 << 31) | ((port_addr & 0x1f) << 25) |
106                         ((regnum & 0x1f) << 20) | ((value & 0xffff) << 4) |
107                         (0x2 << 1));
108
109         timeout = 50000;
110         /* Wait for the the indication that the read is done */
111         while ((in_le32(&phyregs->miimstatus) & 0x8) && --timeout)
112                 udelay(1);
113         if (timeout == 0)
114                 debug("Timeout waiting for MDIO read\n");
115
116         /* Grab the value read from the PHY */
117         value = in_le32(&phyregs->miimdata);
118
119         if ((value & 0x00030000) == 0)
120                 return value & 0x0000ffff;
121
122         return value;
123 }
124
125 static int init_phy(struct eth_device *dev)
126 {
127         struct vsc9953_port_info *l2sw_port = dev->priv;
128         struct phy_device *phydev = NULL;
129
130 #ifdef CONFIG_PHYLIB
131         if (!l2sw_port->bus)
132                 return 0;
133         phydev = phy_connect(l2sw_port->bus, l2sw_port->phyaddr, dev,
134                         l2sw_port->enet_if);
135         if (!phydev) {
136                 printf("Failed to connect\n");
137                 return -1;
138         }
139
140         phydev->supported &= SUPPORTED_10baseT_Half |
141                         SUPPORTED_10baseT_Full |
142                         SUPPORTED_100baseT_Half |
143                         SUPPORTED_100baseT_Full |
144                         SUPPORTED_1000baseT_Full;
145         phydev->advertising = phydev->supported;
146
147         l2sw_port->phydev = phydev;
148
149         phy_config(phydev);
150 #endif
151
152         return 0;
153 }
154
155 static int vsc9953_port_init(int port_no)
156 {
157         struct eth_device *dev;
158
159         /* Internal ports never have a PHY */
160         if (VSC9953_INTERNAL_PORT_CHECK(port_no))
161                 return 0;
162
163         /* alloc eth device */
164         dev = (struct eth_device *)calloc(1, sizeof(struct eth_device));
165         if (!dev)
166                 return -ENOMEM;
167
168         sprintf(dev->name, "SW@PORT%d", port_no);
169         dev->priv = &vsc9953_l2sw.port[port_no];
170         dev->init = NULL;
171         dev->halt = NULL;
172         dev->send = NULL;
173         dev->recv = NULL;
174
175         if (init_phy(dev)) {
176                 free(dev);
177                 return -ENODEV;
178         }
179
180         return 0;
181 }
182
183 static int vsc9953_vlan_table_poll_idle(void)
184 {
185         struct vsc9953_analyzer *l2ana_reg;
186         int timeout;
187
188         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
189                         VSC9953_ANA_OFFSET);
190
191         timeout = 50000;
192         while (((in_le32(&l2ana_reg->ana_tables.vlan_access) &
193                  VSC9953_VLAN_CMD_MASK) != VSC9953_VLAN_CMD_IDLE) && --timeout)
194                 udelay(1);
195
196         return timeout ? 0 : -EBUSY;
197 }
198
199 #ifdef CONFIG_CMD_ETHSW
200 /* Add/remove a port to/from a VLAN */
201 static void vsc9953_vlan_table_membership_set(int vid, u32 port_no, u8 add)
202 {
203         u32 val;
204         struct vsc9953_analyzer *l2ana_reg;
205
206         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
207                         VSC9953_ANA_OFFSET);
208
209         if (vsc9953_vlan_table_poll_idle() < 0) {
210                 debug("VLAN table timeout\n");
211                 return;
212         }
213
214         val = in_le32(&l2ana_reg->ana_tables.vlan_tidx);
215         val = bitfield_replace_by_mask(val, VSC9953_ANA_TBL_VID_MASK, vid);
216         out_le32(&l2ana_reg->ana_tables.vlan_tidx, val);
217
218         clrsetbits_le32(&l2ana_reg->ana_tables.vlan_access,
219                         VSC9953_VLAN_CMD_MASK, VSC9953_VLAN_CMD_READ);
220
221         if (vsc9953_vlan_table_poll_idle() < 0) {
222                 debug("VLAN table timeout\n");
223                 return;
224         }
225
226         val = in_le32(&l2ana_reg->ana_tables.vlan_tidx);
227         val = bitfield_replace_by_mask(val, VSC9953_ANA_TBL_VID_MASK, vid);
228         out_le32(&l2ana_reg->ana_tables.vlan_tidx, val);
229
230         val = in_le32(&l2ana_reg->ana_tables.vlan_access);
231         if (!add) {
232                 val = bitfield_replace_by_mask(val, VSC9953_VLAN_CMD_MASK,
233                                                 VSC9953_VLAN_CMD_WRITE) &
234                       ~(bitfield_replace_by_mask(0, VSC9953_VLAN_PORT_MASK,
235                                                  (1 << port_no)));
236                  ;
237         } else {
238                 val = bitfield_replace_by_mask(val, VSC9953_VLAN_CMD_MASK,
239                                                 VSC9953_VLAN_CMD_WRITE) |
240                       bitfield_replace_by_mask(0, VSC9953_VLAN_PORT_MASK,
241                                                (1 << port_no));
242         }
243         out_le32(&l2ana_reg->ana_tables.vlan_access, val);
244
245         /* wait for VLAN table command to flush */
246         if (vsc9953_vlan_table_poll_idle() < 0) {
247                 debug("VLAN table timeout\n");
248                 return;
249         }
250 }
251
252 /* show VLAN membership for a port */
253 static void vsc9953_vlan_membership_show(int port_no)
254 {
255         u32 val;
256         struct vsc9953_analyzer *l2ana_reg;
257         u32 vid;
258
259         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
260                         VSC9953_ANA_OFFSET);
261
262         printf("Port %d VLAN membership: ", port_no);
263
264         for (vid = 0; vid < VSC9953_MAX_VLAN; vid++) {
265                 if (vsc9953_vlan_table_poll_idle() < 0) {
266                         debug("VLAN table timeout\n");
267                         return;
268                 }
269
270                 val = in_le32(&l2ana_reg->ana_tables.vlan_tidx);
271                 val = bitfield_replace_by_mask(val, VSC9953_ANA_TBL_VID_MASK,
272                                                vid);
273                 out_le32(&l2ana_reg->ana_tables.vlan_tidx, val);
274
275                 clrsetbits_le32(&l2ana_reg->ana_tables.vlan_access,
276                                 VSC9953_VLAN_CMD_MASK, VSC9953_VLAN_CMD_READ);
277
278                 if (vsc9953_vlan_table_poll_idle() < 0) {
279                         debug("VLAN table timeout\n");
280                         return;
281                 }
282
283                 val = in_le32(&l2ana_reg->ana_tables.vlan_access);
284
285                 if (bitfield_extract_by_mask(val, VSC9953_VLAN_PORT_MASK) &
286                     (1 << port_no))
287                         printf("%d ", vid);
288         }
289         printf("\n");
290 }
291 #endif
292
293 /* vlan table set/clear all membership of vid */
294 static void vsc9953_vlan_table_membership_all_set(int vid, int set_member)
295 {
296         uint val;
297         struct vsc9953_analyzer *l2ana_reg;
298
299         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
300                         VSC9953_ANA_OFFSET);
301
302         if (vsc9953_vlan_table_poll_idle() < 0) {
303                 debug("VLAN table timeout\n");
304                 return;
305         }
306
307         /* read current vlan configuration */
308         val = in_le32(&l2ana_reg->ana_tables.vlan_tidx);
309         out_le32(&l2ana_reg->ana_tables.vlan_tidx,
310                  bitfield_replace_by_mask(val, VSC9953_ANA_TBL_VID_MASK, vid));
311
312         clrsetbits_le32(&l2ana_reg->ana_tables.vlan_access,
313                         VSC9953_VLAN_CMD_MASK, VSC9953_VLAN_CMD_READ);
314
315         if (vsc9953_vlan_table_poll_idle() < 0) {
316                 debug("VLAN table timeout\n");
317                 return;
318         }
319
320         val = in_le32(&l2ana_reg->ana_tables.vlan_tidx);
321         out_le32(&l2ana_reg->ana_tables.vlan_tidx,
322                  bitfield_replace_by_mask(val, VSC9953_ANA_TBL_VID_MASK, vid));
323
324         clrsetbits_le32(&l2ana_reg->ana_tables.vlan_access,
325                         VSC9953_VLAN_PORT_MASK | VSC9953_VLAN_CMD_MASK,
326                         VSC9953_VLAN_CMD_WRITE |
327                         (set_member ? VSC9953_VLAN_PORT_MASK : 0));
328 }
329
330 #ifdef CONFIG_CMD_ETHSW
331 /* Get PVID of a VSC9953 port */
332 static int vsc9953_port_vlan_pvid_get(int port_nr, int *pvid)
333 {
334         u32 val;
335         struct vsc9953_analyzer *l2ana_reg;
336
337         /* Administrative down */
338         if (!vsc9953_l2sw.port[port_nr].enabled) {
339                 printf("Port %d is administrative down\n", port_nr);
340                 return -1;
341         }
342
343         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
344                                 VSC9953_ANA_OFFSET);
345
346         /* Get ingress PVID */
347         val = in_le32(&l2ana_reg->port[port_nr].vlan_cfg);
348         *pvid = bitfield_extract_by_mask(val, VSC9953_VLAN_CFG_VID_MASK);
349
350         return 0;
351 }
352 #endif
353
354 /* Set PVID for a VSC9953 port */
355 static void vsc9953_port_vlan_pvid_set(int port_no, int pvid)
356 {
357         uint val;
358         struct vsc9953_analyzer *l2ana_reg;
359         struct vsc9953_rew_reg *l2rew_reg;
360
361         /* Administrative down */
362         if (!vsc9953_l2sw.port[port_no].enabled) {
363                 printf("Port %d is administrative down\n", port_no);
364                 return;
365         }
366
367         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
368                         VSC9953_ANA_OFFSET);
369         l2rew_reg = (struct vsc9953_rew_reg *)(VSC9953_OFFSET +
370                         VSC9953_REW_OFFSET);
371
372         /* Set PVID on ingress */
373         val = in_le32(&l2ana_reg->port[port_no].vlan_cfg);
374         val = bitfield_replace_by_mask(val, VSC9953_VLAN_CFG_VID_MASK, pvid);
375         out_le32(&l2ana_reg->port[port_no].vlan_cfg, val);
376
377         /* Set PVID on egress */
378         val = in_le32(&l2rew_reg->port[port_no].port_vlan_cfg);
379         val = bitfield_replace_by_mask(val, VSC9953_PORT_VLAN_CFG_VID_MASK,
380                                        pvid);
381         out_le32(&l2rew_reg->port[port_no].port_vlan_cfg, val);
382 }
383
384 static void vsc9953_port_all_vlan_pvid_set(int pvid)
385 {
386         int i;
387
388         for (i = 0; i < VSC9953_MAX_PORTS; i++)
389                 vsc9953_port_vlan_pvid_set(i, pvid);
390 }
391
392 /* Enable/disable vlan aware of a VSC9953 port */
393 static void vsc9953_port_vlan_aware_set(int port_no, int enabled)
394 {
395         struct vsc9953_analyzer *l2ana_reg;
396
397         /* Administrative down */
398         if (!vsc9953_l2sw.port[port_no].enabled) {
399                 printf("Port %d is administrative down\n", port_no);
400                 return;
401         }
402
403         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
404                         VSC9953_ANA_OFFSET);
405
406         if (enabled)
407                 setbits_le32(&l2ana_reg->port[port_no].vlan_cfg,
408                              VSC9953_VLAN_CFG_AWARE_ENA);
409         else
410                 clrbits_le32(&l2ana_reg->port[port_no].vlan_cfg,
411                              VSC9953_VLAN_CFG_AWARE_ENA);
412 }
413
414 /* Set all VSC9953 ports' vlan aware  */
415 static void vsc9953_port_all_vlan_aware_set(int enabled)
416 {
417         int i;
418
419         for (i = 0; i < VSC9953_MAX_PORTS; i++)
420                 vsc9953_port_vlan_aware_set(i, enabled);
421 }
422
423 /* Enable/disable vlan pop count of a VSC9953 port */
424 static void vsc9953_port_vlan_popcnt_set(int port_no, int popcnt)
425 {
426         uint val;
427         struct vsc9953_analyzer *l2ana_reg;
428
429         /* Administrative down */
430         if (!vsc9953_l2sw.port[port_no].enabled) {
431                 printf("Port %d is administrative down\n", port_no);
432                 return;
433         }
434
435         if (popcnt > 3 || popcnt < 0) {
436                 printf("Invalid pop count value: %d\n", port_no);
437                 return;
438         }
439
440         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
441                         VSC9953_ANA_OFFSET);
442
443         val = in_le32(&l2ana_reg->port[port_no].vlan_cfg);
444         val = bitfield_replace_by_mask(val, VSC9953_VLAN_CFG_POP_CNT_MASK,
445                                        popcnt);
446         out_le32(&l2ana_reg->port[port_no].vlan_cfg, val);
447 }
448
449 /* Set all VSC9953 ports' pop count  */
450 static void vsc9953_port_all_vlan_poncnt_set(int popcnt)
451 {
452         int i;
453
454         for (i = 0; i < VSC9953_MAX_PORTS; i++)
455                 vsc9953_port_vlan_popcnt_set(i, popcnt);
456 }
457
458 /* Enable/disable learning for frames dropped due to ingress filtering */
459 static void vsc9953_vlan_ingr_fltr_learn_drop(int enable)
460 {
461         struct vsc9953_analyzer *l2ana_reg;
462
463         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
464                         VSC9953_ANA_OFFSET);
465
466         if (enable)
467                 setbits_le32(&l2ana_reg->ana.adv_learn, VSC9953_VLAN_CHK);
468         else
469                 clrbits_le32(&l2ana_reg->ana.adv_learn, VSC9953_VLAN_CHK);
470 }
471
472 enum aggr_code_mode {
473         AGGR_CODE_RAND = 0,
474         AGGR_CODE_ALL,  /* S/D MAC, IPv4 S/D IP, IPv6 Flow Label, S/D PORT */
475 };
476
477 /* Set aggregation code generation mode */
478 static int vsc9953_aggr_code_set(enum aggr_code_mode ac)
479 {
480         int rc;
481         struct vsc9953_analyzer *l2ana_reg;
482
483         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
484                                                 VSC9953_ANA_OFFSET);
485
486         switch (ac) {
487         case AGGR_CODE_RAND:
488                 clrsetbits_le32(&l2ana_reg->common.aggr_cfg,
489                                 VSC9953_AC_DMAC_ENA | VSC9953_AC_SMAC_ENA |
490                                 VSC9953_AC_IP6_LBL_ENA |
491                                 VSC9953_AC_IP6_TCPUDP_ENA |
492                                 VSC9953_AC_IP4_SIPDIP_ENA |
493                                 VSC9953_AC_IP4_TCPUDP_ENA, VSC9953_AC_RND_ENA);
494                 rc = 0;
495                 break;
496         case AGGR_CODE_ALL:
497                 clrsetbits_le32(&l2ana_reg->common.aggr_cfg, VSC9953_AC_RND_ENA,
498                                 VSC9953_AC_DMAC_ENA | VSC9953_AC_SMAC_ENA |
499                                 VSC9953_AC_IP6_LBL_ENA |
500                                 VSC9953_AC_IP6_TCPUDP_ENA |
501                                 VSC9953_AC_IP4_SIPDIP_ENA |
502                                 VSC9953_AC_IP4_TCPUDP_ENA);
503                 rc = 0;
504                 break;
505         default:
506                 /* unknown mode for aggregation code */
507                 rc = -EINVAL;
508         }
509
510         return rc;
511 }
512
513 /* Egress untag modes of a VSC9953 port */
514 enum egress_untag_mode {
515         EGRESS_UNTAG_ALL = 0,
516         EGRESS_UNTAG_PVID_AND_ZERO,
517         EGRESS_UNTAG_ZERO,
518         EGRESS_UNTAG_NONE,
519 };
520
521 #ifdef CONFIG_CMD_ETHSW
522 /* Get egress tagging configuration for a VSC9953 port */
523 static int vsc9953_port_vlan_egr_untag_get(int port_no,
524                                            enum egress_untag_mode *mode)
525 {
526         u32 val;
527         struct vsc9953_rew_reg *l2rew_reg;
528
529         /* Administrative down */
530         if (!vsc9953_l2sw.port[port_no].enabled) {
531                 printf("Port %d is administrative down\n", port_no);
532                 return -1;
533         }
534
535         l2rew_reg = (struct vsc9953_rew_reg *)(VSC9953_OFFSET +
536                         VSC9953_REW_OFFSET);
537
538         val = in_le32(&l2rew_reg->port[port_no].port_tag_cfg);
539
540         switch (val & VSC9953_TAG_CFG_MASK) {
541         case VSC9953_TAG_CFG_NONE:
542                 *mode = EGRESS_UNTAG_ALL;
543                 return 0;
544         case VSC9953_TAG_CFG_ALL_BUT_PVID_ZERO:
545                 *mode = EGRESS_UNTAG_PVID_AND_ZERO;
546                 return 0;
547         case VSC9953_TAG_CFG_ALL_BUT_ZERO:
548                 *mode = EGRESS_UNTAG_ZERO;
549                 return 0;
550         case VSC9953_TAG_CFG_ALL:
551                 *mode = EGRESS_UNTAG_NONE;
552                 return 0;
553         default:
554                 printf("Unknown egress tagging configuration for port %d\n",
555                        port_no);
556                 return -1;
557         }
558 }
559
560 /* Show egress tagging configuration for a VSC9953 port */
561 static void vsc9953_port_vlan_egr_untag_show(int port_no)
562 {
563         enum egress_untag_mode mode;
564
565         if (vsc9953_port_vlan_egr_untag_get(port_no, &mode)) {
566                 printf("%7d\t%17s\n", port_no, "-");
567                 return;
568         }
569
570         printf("%7d\t", port_no);
571         switch (mode) {
572         case EGRESS_UNTAG_ALL:
573                 printf("%17s\n", "all");
574                 break;
575         case EGRESS_UNTAG_NONE:
576                 printf("%17s\n", "none");
577                 break;
578         case EGRESS_UNTAG_PVID_AND_ZERO:
579                 printf("%17s\n", "PVID and 0");
580                 break;
581         case EGRESS_UNTAG_ZERO:
582                 printf("%17s\n", "0");
583                 break;
584         default:
585                 printf("%17s\n", "-");
586         }
587 }
588 #endif
589
590 static void vsc9953_port_vlan_egr_untag_set(int port_no,
591                                             enum egress_untag_mode mode)
592 {
593         struct vsc9953_rew_reg *l2rew_reg;
594
595         /* Administrative down */
596         if (!vsc9953_l2sw.port[port_no].enabled) {
597                 printf("Port %d is administrative down\n", port_no);
598                 return;
599         }
600
601         l2rew_reg = (struct vsc9953_rew_reg *)(VSC9953_OFFSET +
602                         VSC9953_REW_OFFSET);
603
604         switch (mode) {
605         case EGRESS_UNTAG_ALL:
606                 clrsetbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
607                                 VSC9953_TAG_CFG_MASK, VSC9953_TAG_CFG_NONE);
608                 break;
609         case EGRESS_UNTAG_PVID_AND_ZERO:
610                 clrsetbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
611                                 VSC9953_TAG_CFG_MASK,
612                                 VSC9953_TAG_CFG_ALL_BUT_PVID_ZERO);
613                 break;
614         case EGRESS_UNTAG_ZERO:
615                 clrsetbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
616                                 VSC9953_TAG_CFG_MASK,
617                                 VSC9953_TAG_CFG_ALL_BUT_ZERO);
618                 break;
619         case EGRESS_UNTAG_NONE:
620                 clrsetbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
621                                 VSC9953_TAG_CFG_MASK, VSC9953_TAG_CFG_ALL);
622                 break;
623         default:
624                 printf("Unknown untag mode for port %d\n", port_no);
625         }
626 }
627
628 static void vsc9953_port_all_vlan_egress_untagged_set(
629                 enum egress_untag_mode mode)
630 {
631         int i;
632
633         for (i = 0; i < VSC9953_MAX_PORTS; i++)
634                 vsc9953_port_vlan_egr_untag_set(i, mode);
635 }
636
637 static int vsc9953_autoage_time_set(int age_period)
638 {
639         u32 autoage;
640         struct vsc9953_analyzer *l2ana_reg;
641
642         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
643                                                 VSC9953_ANA_OFFSET);
644
645         if (age_period < 0 || age_period > VSC9953_AUTOAGE_PERIOD_MASK)
646                 return -EINVAL;
647
648         autoage = bitfield_replace_by_mask(in_le32(&l2ana_reg->ana.auto_age),
649                                            VSC9953_AUTOAGE_PERIOD_MASK,
650                                            age_period);
651         out_le32(&l2ana_reg->ana.auto_age, autoage);
652
653         return 0;
654 }
655
656 #ifdef CONFIG_CMD_ETHSW
657
658 /* Enable/disable status of a VSC9953 port */
659 static void vsc9953_port_status_set(int port_no, u8 enabled)
660 {
661         struct vsc9953_qsys_reg *l2qsys_reg;
662
663         /* Administrative down */
664         if (!vsc9953_l2sw.port[port_no].enabled)
665                 return;
666
667         l2qsys_reg = (struct vsc9953_qsys_reg *)(VSC9953_OFFSET +
668                         VSC9953_QSYS_OFFSET);
669
670         if (enabled)
671                 setbits_le32(&l2qsys_reg->sys.switch_port_mode[port_no],
672                              VSC9953_PORT_ENA);
673         else
674                 clrbits_le32(&l2qsys_reg->sys.switch_port_mode[port_no],
675                              VSC9953_PORT_ENA);
676 }
677
678 /* Start autonegotiation for a VSC9953 PHY */
679 static void vsc9953_phy_autoneg(int port_no)
680 {
681         if (!vsc9953_l2sw.port[port_no].phydev)
682                 return;
683
684         if (vsc9953_l2sw.port[port_no].phydev->drv->startup(
685                         vsc9953_l2sw.port[port_no].phydev))
686                 printf("Failed to start PHY for port %d\n", port_no);
687 }
688
689 /* Print a VSC9953 port's configuration */
690 static void vsc9953_port_config_show(int port_no)
691 {
692         int speed;
693         int duplex;
694         int link;
695         u8 enabled;
696         u32 val;
697         struct vsc9953_qsys_reg *l2qsys_reg;
698
699         l2qsys_reg = (struct vsc9953_qsys_reg *)(VSC9953_OFFSET +
700                         VSC9953_QSYS_OFFSET);
701
702         val = in_le32(&l2qsys_reg->sys.switch_port_mode[port_no]);
703         enabled = vsc9953_l2sw.port[port_no].enabled &&
704                   (val & VSC9953_PORT_ENA);
705
706         /* internal ports (8 and 9) are fixed */
707         if (VSC9953_INTERNAL_PORT_CHECK(port_no)) {
708                 link = 1;
709                 speed = SPEED_2500;
710                 duplex = DUPLEX_FULL;
711         } else {
712                 if (vsc9953_l2sw.port[port_no].phydev) {
713                         link = vsc9953_l2sw.port[port_no].phydev->link;
714                         speed = vsc9953_l2sw.port[port_no].phydev->speed;
715                         duplex = vsc9953_l2sw.port[port_no].phydev->duplex;
716                 } else {
717                         link = -1;
718                         speed = -1;
719                         duplex = -1;
720                 }
721         }
722
723         printf("%8d ", port_no);
724         printf("%8s ", enabled == 1 ? "enabled" : "disabled");
725         printf("%8s ", link == 1 ? "up" : "down");
726
727         switch (speed) {
728         case SPEED_10:
729                 printf("%8d ", 10);
730                 break;
731         case SPEED_100:
732                 printf("%8d ", 100);
733                 break;
734         case SPEED_1000:
735                 printf("%8d ", 1000);
736                 break;
737         case SPEED_2500:
738                 printf("%8d ", 2500);
739                 break;
740         case SPEED_10000:
741                 printf("%8d ", 10000);
742                 break;
743         default:
744                 printf("%8s ", "-");
745         }
746
747         printf("%8s\n", duplex == DUPLEX_FULL ? "full" : "half");
748 }
749
750 /* Show VSC9953 ports' statistics */
751 static void vsc9953_port_statistics_show(int port_no)
752 {
753         u32 rx_val;
754         u32 tx_val;
755         struct vsc9953_system_reg *l2sys_reg;
756
757         /* Administrative down */
758         if (!vsc9953_l2sw.port[port_no].enabled) {
759                 printf("Port %d is administrative down\n", port_no);
760                 return;
761         }
762
763         l2sys_reg = (struct vsc9953_system_reg *)(VSC9953_OFFSET +
764                         VSC9953_SYS_OFFSET);
765
766         printf("Statistics for L2 Switch port %d:\n", port_no);
767
768         /* Set counter view for our port */
769         out_le32(&l2sys_reg->sys.stat_cfg, port_no);
770
771 #define VSC9953_STATS_PRINTF "%-15s %10u"
772
773         /* Get number of Rx and Tx frames */
774         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_short) +
775                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_frag) +
776                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_jabber) +
777                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_long) +
778                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_64) +
779                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_65_127) +
780                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_128_255) +
781                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_256_511) +
782                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_512_1023) +
783                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_1024_1526) +
784                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_jumbo);
785         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_64) +
786                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_65_127) +
787                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_128_255) +
788                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_256_511) +
789                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_512_1023) +
790                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_1024_1526) +
791                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_jumbo);
792         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
793                "Rx frames:", rx_val, "Tx frames:", tx_val);
794
795         /* Get number of Rx and Tx bytes */
796         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_oct);
797         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_oct);
798         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
799                "Rx bytes:", rx_val, "Tx bytes:", tx_val);
800
801         /* Get number of Rx frames received ok and Tx frames sent ok */
802         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_0) +
803                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_1) +
804                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_2) +
805                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_3) +
806                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_4) +
807                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_5) +
808                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_6) +
809                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_7) +
810                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_0) +
811                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_1) +
812                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_2) +
813                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_3) +
814                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_4) +
815                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_5) +
816                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_6) +
817                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_7);
818         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_64) +
819                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_65_127) +
820                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_128_255) +
821                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_256_511) +
822                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_512_1023) +
823                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_1024_1526) +
824                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_jumbo);
825         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
826                "Rx frames ok:", rx_val, "Tx frames ok:", tx_val);
827
828         /* Get number of Rx and Tx unicast frames */
829         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_uc);
830         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_uc);
831         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
832                "Rx unicast:", rx_val, "Tx unicast:", tx_val);
833
834         /* Get number of Rx and Tx broadcast frames */
835         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_bc);
836         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_bc);
837         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
838                "Rx broadcast:", rx_val, "Tx broadcast:", tx_val);
839
840         /* Get number of Rx and Tx frames of 64B */
841         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_64);
842         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_64);
843         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
844                "Rx 64B:", rx_val, "Tx 64B:", tx_val);
845
846         /* Get number of Rx and Tx frames with sizes between 65B and 127B */
847         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_65_127);
848         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_65_127);
849         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
850                "Rx 65B-127B:", rx_val, "Tx 65B-127B:", tx_val);
851
852         /* Get number of Rx and Tx frames with sizes between 128B and 255B */
853         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_128_255);
854         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_128_255);
855         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
856                "Rx 128B-255B:", rx_val, "Tx 128B-255B:", tx_val);
857
858         /* Get number of Rx and Tx frames with sizes between 256B and 511B */
859         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_256_511);
860         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_256_511);
861         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
862                "Rx 256B-511B:", rx_val, "Tx 256B-511B:", tx_val);
863
864         /* Get number of Rx and Tx frames with sizes between 512B and 1023B */
865         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_512_1023);
866         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_512_1023);
867         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
868                "Rx 512B-1023B:", rx_val, "Tx 512B-1023B:", tx_val);
869
870         /* Get number of Rx and Tx frames with sizes between 1024B and 1526B */
871         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_1024_1526);
872         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_1024_1526);
873         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
874                "Rx 1024B-1526B:", rx_val, "Tx 1024B-1526B:", tx_val);
875
876         /* Get number of Rx and Tx jumbo frames */
877         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_jumbo);
878         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_jumbo);
879         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
880                "Rx jumbo:", rx_val, "Tx jumbo:", tx_val);
881
882         /* Get number of Rx and Tx dropped frames */
883         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_cat_drop) +
884                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_tail) +
885                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_0) +
886                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_1) +
887                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_2) +
888                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_3) +
889                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_4) +
890                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_5) +
891                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_6) +
892                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_7) +
893                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_0) +
894                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_1) +
895                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_2) +
896                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_3) +
897                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_4) +
898                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_5) +
899                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_6) +
900                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_7);
901         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_drop) +
902                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_aged);
903         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
904                "Rx drops:", rx_val, "Tx drops:", tx_val);
905
906         /*
907          * Get number of Rx frames with CRC or alignment errors
908          * and number of detected Tx collisions
909          */
910         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_crc);
911         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_col);
912         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
913                "Rx CRC&align:", rx_val, "Tx coll:", tx_val);
914
915         /*
916          * Get number of Rx undersized frames and
917          * number of Tx aged frames
918          */
919         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_short);
920         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_aged);
921         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
922                "Rx undersize:", rx_val, "Tx aged:", tx_val);
923
924         /* Get number of Rx oversized frames */
925         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_long);
926         printf(VSC9953_STATS_PRINTF"\n", "Rx oversized:", rx_val);
927
928         /* Get number of Rx fragmented frames */
929         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_frag);
930         printf(VSC9953_STATS_PRINTF"\n", "Rx fragments:", rx_val);
931
932         /* Get number of Rx jabber errors */
933         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_jabber);
934         printf(VSC9953_STATS_PRINTF"\n", "Rx jabbers:", rx_val);
935
936         /*
937          * Get number of Rx frames filtered due to classification rules or
938          * no destination ports
939          */
940         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_cat_drop) +
941                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_local);
942         printf(VSC9953_STATS_PRINTF"\n", "Rx filtered:", rx_val);
943
944         printf("\n");
945 }
946
947 /* Clear statistics for a VSC9953 port */
948 static void vsc9953_port_statistics_clear(int port_no)
949 {
950         struct vsc9953_system_reg *l2sys_reg;
951
952         /* Administrative down */
953         if (!vsc9953_l2sw.port[port_no].enabled) {
954                 printf("Port %d is administrative down\n", port_no);
955                 return;
956         }
957
958         l2sys_reg = (struct vsc9953_system_reg *)(VSC9953_OFFSET +
959                         VSC9953_SYS_OFFSET);
960
961         /* Clear all counter groups for our ports */
962         out_le32(&l2sys_reg->sys.stat_cfg, port_no |
963                  VSC9953_STAT_CLEAR_RX | VSC9953_STAT_CLEAR_TX |
964                  VSC9953_STAT_CLEAR_DR);
965 }
966
967 enum port_learn_mode {
968         PORT_LEARN_NONE,
969         PORT_LEARN_AUTO
970 };
971
972 /* Set learning configuration for a VSC9953 port */
973 static void vsc9953_port_learn_mode_set(int port_no, enum port_learn_mode mode)
974 {
975         struct vsc9953_analyzer *l2ana_reg;
976
977         /* Administrative down */
978         if (!vsc9953_l2sw.port[port_no].enabled) {
979                 printf("Port %d is administrative down\n", port_no);
980                 return;
981         }
982
983         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
984                         VSC9953_ANA_OFFSET);
985
986         switch (mode) {
987         case PORT_LEARN_NONE:
988                 clrbits_le32(&l2ana_reg->port[port_no].port_cfg,
989                              VSC9953_PORT_CFG_LEARN_DROP |
990                              VSC9953_PORT_CFG_LEARN_CPU |
991                              VSC9953_PORT_CFG_LEARN_AUTO |
992                              VSC9953_PORT_CFG_LEARN_ENA);
993                 break;
994         case PORT_LEARN_AUTO:
995                 clrsetbits_le32(&l2ana_reg->port[port_no].port_cfg,
996                                 VSC9953_PORT_CFG_LEARN_DROP |
997                                 VSC9953_PORT_CFG_LEARN_CPU,
998                                 VSC9953_PORT_CFG_LEARN_ENA |
999                                 VSC9953_PORT_CFG_LEARN_AUTO);
1000                 break;
1001         default:
1002                 printf("Unknown learn mode for port %d\n", port_no);
1003         }
1004 }
1005
1006 /* Get learning configuration for a VSC9953 port */
1007 static int vsc9953_port_learn_mode_get(int port_no, enum port_learn_mode *mode)
1008 {
1009         u32 val;
1010         struct vsc9953_analyzer *l2ana_reg;
1011
1012         /* Administrative down */
1013         if (!vsc9953_l2sw.port[port_no].enabled) {
1014                 printf("Port %d is administrative down\n", port_no);
1015                 return -1;
1016         }
1017
1018         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1019                         VSC9953_ANA_OFFSET);
1020
1021         /* For now we only support HW learning (auto) and no learning */
1022         val = in_le32(&l2ana_reg->port[port_no].port_cfg);
1023         if ((val & (VSC9953_PORT_CFG_LEARN_ENA |
1024                     VSC9953_PORT_CFG_LEARN_AUTO)) ==
1025             (VSC9953_PORT_CFG_LEARN_ENA | VSC9953_PORT_CFG_LEARN_AUTO))
1026                 *mode = PORT_LEARN_AUTO;
1027         else
1028                 *mode = PORT_LEARN_NONE;
1029
1030         return 0;
1031 }
1032
1033 /* wait for FDB to become available */
1034 static int vsc9953_mac_table_poll_idle(void)
1035 {
1036         struct vsc9953_analyzer *l2ana_reg;
1037         u32 timeout;
1038
1039         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1040                         VSC9953_ANA_OFFSET);
1041
1042         timeout = 50000;
1043         while (((in_le32(&l2ana_reg->ana_tables.mac_access) &
1044                          VSC9953_MAC_CMD_MASK) !=
1045                 VSC9953_MAC_CMD_IDLE) && --timeout)
1046                 udelay(1);
1047
1048         return timeout ? 0 : -EBUSY;
1049 }
1050
1051 /* enum describing available commands for the MAC table */
1052 enum mac_table_cmd {
1053         MAC_TABLE_READ,
1054         MAC_TABLE_LOOKUP,
1055         MAC_TABLE_WRITE,
1056         MAC_TABLE_LEARN,
1057         MAC_TABLE_FORGET,
1058         MAC_TABLE_GET_NEXT,
1059         MAC_TABLE_AGE,
1060 };
1061
1062 /* Issues a command to the FDB table */
1063 static int vsc9953_mac_table_cmd(enum mac_table_cmd cmd)
1064 {
1065         struct vsc9953_analyzer *l2ana_reg;
1066
1067         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1068                         VSC9953_ANA_OFFSET);
1069
1070         switch (cmd) {
1071         case MAC_TABLE_READ:
1072                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
1073                                 VSC9953_MAC_CMD_MASK | VSC9953_MAC_CMD_VALID,
1074                                 VSC9953_MAC_CMD_READ);
1075                 break;
1076         case MAC_TABLE_LOOKUP:
1077                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
1078                                 VSC9953_MAC_CMD_MASK, VSC9953_MAC_CMD_READ |
1079                                 VSC9953_MAC_CMD_VALID);
1080                 break;
1081         case MAC_TABLE_WRITE:
1082                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
1083                                 VSC9953_MAC_CMD_MASK |
1084                                 VSC9953_MAC_ENTRYTYPE_MASK,
1085                                 VSC9953_MAC_CMD_WRITE |
1086                                 VSC9953_MAC_ENTRYTYPE_LOCKED);
1087                 break;
1088         case MAC_TABLE_LEARN:
1089                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
1090                                 VSC9953_MAC_CMD_MASK |
1091                                 VSC9953_MAC_ENTRYTYPE_MASK,
1092                                 VSC9953_MAC_CMD_LEARN |
1093                                 VSC9953_MAC_ENTRYTYPE_LOCKED |
1094                                 VSC9953_MAC_CMD_VALID);
1095                 break;
1096         case MAC_TABLE_FORGET:
1097                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
1098                                 VSC9953_MAC_CMD_MASK |
1099                                 VSC9953_MAC_ENTRYTYPE_MASK,
1100                                 VSC9953_MAC_CMD_FORGET);
1101                 break;
1102         case MAC_TABLE_GET_NEXT:
1103                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
1104                                 VSC9953_MAC_CMD_MASK |
1105                                 VSC9953_MAC_ENTRYTYPE_MASK,
1106                                 VSC9953_MAC_CMD_NEXT);
1107                 break;
1108         case MAC_TABLE_AGE:
1109                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
1110                                 VSC9953_MAC_CMD_MASK |
1111                                 VSC9953_MAC_ENTRYTYPE_MASK,
1112                                 VSC9953_MAC_CMD_AGE);
1113                 break;
1114         default:
1115                 printf("Unknown MAC table command\n");
1116         }
1117
1118         if (vsc9953_mac_table_poll_idle() < 0) {
1119                 debug("MAC table timeout\n");
1120                 return -1;
1121         }
1122
1123         return 0;
1124 }
1125
1126 /* show the FDB entries that correspond to a port and a VLAN */
1127 static void vsc9953_mac_table_show(int port_no, int vid)
1128 {
1129         int rc[VSC9953_MAX_PORTS];
1130         enum port_learn_mode mode[VSC9953_MAX_PORTS];
1131         int i;
1132         u32 val;
1133         u32 vlan;
1134         u32 mach;
1135         u32 macl;
1136         u32 dest_indx;
1137         struct vsc9953_analyzer *l2ana_reg;
1138
1139         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1140                         VSC9953_ANA_OFFSET);
1141
1142         /* disable auto learning */
1143         if (port_no == ETHSW_CMD_PORT_ALL) {
1144                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1145                         rc[i] = vsc9953_port_learn_mode_get(i, &mode[i]);
1146                         if (!rc[i] && mode[i] != PORT_LEARN_NONE)
1147                                 vsc9953_port_learn_mode_set(i, PORT_LEARN_NONE);
1148                 }
1149         } else {
1150                 rc[port_no] = vsc9953_port_learn_mode_get(port_no,
1151                                                           &mode[port_no]);
1152                 if (!rc[port_no] && mode[port_no] != PORT_LEARN_NONE)
1153                         vsc9953_port_learn_mode_set(port_no, PORT_LEARN_NONE);
1154         }
1155
1156         /* write port and vid to get selected FDB entries */
1157         val = in_le32(&l2ana_reg->ana.anag_efil);
1158         if (port_no != ETHSW_CMD_PORT_ALL) {
1159                 val = bitfield_replace_by_mask(val, VSC9953_AGE_PORT_MASK,
1160                                                port_no) | VSC9953_AGE_PORT_EN;
1161         }
1162         if (vid != ETHSW_CMD_VLAN_ALL) {
1163                 val = bitfield_replace_by_mask(val, VSC9953_AGE_VID_MASK,
1164                                                vid) | VSC9953_AGE_VID_EN;
1165         }
1166         out_le32(&l2ana_reg->ana.anag_efil, val);
1167
1168         /* set MAC and VLAN to 0 to look from beginning */
1169         clrbits_le32(&l2ana_reg->ana_tables.mach_data,
1170                      VSC9953_MAC_VID_MASK | VSC9953_MAC_MACH_MASK);
1171         out_le32(&l2ana_reg->ana_tables.macl_data, 0);
1172
1173         /* get entries */
1174         printf("%10s %17s %5s %4s\n", "EntryType", "MAC", "PORT", "VID");
1175         do {
1176                 if (vsc9953_mac_table_cmd(MAC_TABLE_GET_NEXT) < 0) {
1177                         debug("GET NEXT MAC table command failed\n");
1178                         break;
1179                 }
1180
1181                 val = in_le32(&l2ana_reg->ana_tables.mac_access);
1182
1183                 /* get out when an invalid entry is found */
1184                 if (!(val & VSC9953_MAC_CMD_VALID))
1185                         break;
1186
1187                 switch (val & VSC9953_MAC_ENTRYTYPE_MASK) {
1188                 case VSC9953_MAC_ENTRYTYPE_NORMAL:
1189                         printf("%10s ", "Dynamic");
1190                         break;
1191                 case VSC9953_MAC_ENTRYTYPE_LOCKED:
1192                         printf("%10s ", "Static");
1193                         break;
1194                 case VSC9953_MAC_ENTRYTYPE_IPV4MCAST:
1195                         printf("%10s ", "IPv4 Mcast");
1196                         break;
1197                 case VSC9953_MAC_ENTRYTYPE_IPV6MCAST:
1198                         printf("%10s ", "IPv6 Mcast");
1199                         break;
1200                 default:
1201                         printf("%10s ", "Unknown");
1202                 }
1203
1204                 dest_indx = bitfield_extract_by_mask(val,
1205                                                      VSC9953_MAC_DESTIDX_MASK);
1206
1207                 val = in_le32(&l2ana_reg->ana_tables.mach_data);
1208                 vlan = bitfield_extract_by_mask(val, VSC9953_MAC_VID_MASK);
1209                 mach = bitfield_extract_by_mask(val, VSC9953_MAC_MACH_MASK);
1210                 macl = in_le32(&l2ana_reg->ana_tables.macl_data);
1211
1212                 printf("%02x:%02x:%02x:%02x:%02x:%02x ", (mach >> 8) & 0xff,
1213                        mach & 0xff, (macl >> 24) & 0xff, (macl >> 16) & 0xff,
1214                        (macl >> 8) & 0xff, macl & 0xff);
1215                 printf("%5d ", dest_indx);
1216                 printf("%4d\n", vlan);
1217         } while (1);
1218
1219         /* set learning mode to previous value */
1220         if (port_no == ETHSW_CMD_PORT_ALL) {
1221                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1222                         if (!rc[i] && mode[i] != PORT_LEARN_NONE)
1223                                 vsc9953_port_learn_mode_set(i, mode[i]);
1224                 }
1225         } else {
1226                 /* If administrative down, skip */
1227                 if (!rc[port_no] && mode[port_no] != PORT_LEARN_NONE)
1228                         vsc9953_port_learn_mode_set(port_no, mode[port_no]);
1229         }
1230
1231         /* reset FDB port and VLAN FDB selection */
1232         clrbits_le32(&l2ana_reg->ana.anag_efil, VSC9953_AGE_PORT_EN |
1233                      VSC9953_AGE_PORT_MASK | VSC9953_AGE_VID_EN |
1234                      VSC9953_AGE_VID_MASK);
1235 }
1236
1237 /* Add a static FDB entry */
1238 static int vsc9953_mac_table_add(u8 port_no, uchar mac[6], int vid)
1239 {
1240         u32 val;
1241         struct vsc9953_analyzer *l2ana_reg;
1242
1243         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1244                         VSC9953_ANA_OFFSET);
1245
1246         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1247         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1248               (mac[0] << 8) | (mac[1] << 0);
1249         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1250
1251         out_le32(&l2ana_reg->ana_tables.macl_data,
1252                  (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) |
1253                  (mac[5] << 0));
1254
1255         /* set on which port is the MAC address added */
1256         val = in_le32(&l2ana_reg->ana_tables.mac_access);
1257         val = bitfield_replace_by_mask(val, VSC9953_MAC_DESTIDX_MASK, port_no);
1258         out_le32(&l2ana_reg->ana_tables.mac_access, val);
1259
1260         if (vsc9953_mac_table_cmd(MAC_TABLE_LEARN) < 0)
1261                 return -1;
1262
1263         /* check if the MAC address was indeed added */
1264         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1265         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1266               (mac[0] << 8) | (mac[1] << 0);
1267         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1268
1269         out_le32(&l2ana_reg->ana_tables.macl_data,
1270                  (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) |
1271                  (mac[5] << 0));
1272
1273         if (vsc9953_mac_table_cmd(MAC_TABLE_READ) < 0)
1274                 return -1;
1275
1276         val = in_le32(&l2ana_reg->ana_tables.mac_access);
1277
1278         if ((port_no != bitfield_extract_by_mask(val,
1279                                                  VSC9953_MAC_DESTIDX_MASK))) {
1280                 printf("Failed to add MAC address\n");
1281                 return -1;
1282         }
1283         return 0;
1284 }
1285
1286 /* Delete a FDB entry */
1287 static int vsc9953_mac_table_del(uchar mac[6], u16 vid)
1288 {
1289         u32 val;
1290         struct vsc9953_analyzer *l2ana_reg;
1291
1292         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1293                         VSC9953_ANA_OFFSET);
1294
1295         /* check first if MAC entry is present */
1296         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1297         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1298               (mac[0] << 8) | (mac[1] << 0);
1299         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1300
1301         out_le32(&l2ana_reg->ana_tables.macl_data,
1302                  (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) |
1303                  (mac[5] << 0));
1304
1305         if (vsc9953_mac_table_cmd(MAC_TABLE_LOOKUP) < 0) {
1306                 debug("Lookup in the MAC table failed\n");
1307                 return -1;
1308         }
1309
1310         if (!(in_le32(&l2ana_reg->ana_tables.mac_access) &
1311               VSC9953_MAC_CMD_VALID)) {
1312                 printf("The MAC address: %02x:%02x:%02x:%02x:%02x:%02x ",
1313                        mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
1314                 printf("VLAN: %d does not exist.\n", vid);
1315                 return -1;
1316         }
1317
1318         /* FDB entry found, proceed to delete */
1319         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1320         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1321               (mac[0] << 8) | (mac[1] << 0);
1322         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1323
1324         out_le32(&l2ana_reg->ana_tables.macl_data, (mac[2] << 24) |
1325                  (mac[3] << 16) | (mac[4] << 8) | (mac[5] << 0));
1326
1327         if (vsc9953_mac_table_cmd(MAC_TABLE_FORGET) < 0)
1328                 return -1;
1329
1330         /* check if the MAC entry is still in FDB */
1331         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1332         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1333               (mac[0] << 8) | (mac[1] << 0);
1334         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1335
1336         out_le32(&l2ana_reg->ana_tables.macl_data, (mac[2] << 24) |
1337                  (mac[3] << 16) | (mac[4] << 8) | (mac[5] << 0));
1338
1339         if (vsc9953_mac_table_cmd(MAC_TABLE_LOOKUP) < 0) {
1340                 debug("Lookup in the MAC table failed\n");
1341                 return -1;
1342         }
1343         if (in_le32(&l2ana_reg->ana_tables.mac_access) &
1344             VSC9953_MAC_CMD_VALID) {
1345                 printf("Failed to delete MAC address\n");
1346                 return -1;
1347         }
1348
1349         return 0;
1350 }
1351
1352 /* age the unlocked entries in FDB */
1353 static void vsc9953_mac_table_age(int port_no, int vid)
1354 {
1355         int rc[VSC9953_MAX_PORTS];
1356         enum port_learn_mode mode[VSC9953_MAX_PORTS];
1357         u32 val;
1358         int i;
1359         struct vsc9953_analyzer *l2ana_reg;
1360
1361         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1362                         VSC9953_ANA_OFFSET);
1363
1364         /* set port and VID for selective aging */
1365         val = in_le32(&l2ana_reg->ana.anag_efil);
1366         if (port_no != ETHSW_CMD_PORT_ALL) {
1367                 /* disable auto learning */
1368                 rc[port_no] = vsc9953_port_learn_mode_get(port_no,
1369                                                           &mode[port_no]);
1370                 if (!rc[port_no] && mode[port_no] != PORT_LEARN_NONE)
1371                         vsc9953_port_learn_mode_set(port_no, PORT_LEARN_NONE);
1372
1373                 val = bitfield_replace_by_mask(val, VSC9953_AGE_PORT_MASK,
1374                                                port_no) | VSC9953_AGE_PORT_EN;
1375         } else {
1376                 /* disable auto learning on all ports */
1377                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1378                         rc[i] = vsc9953_port_learn_mode_get(i, &mode[i]);
1379                         if (!rc[i] && mode[i] != PORT_LEARN_NONE)
1380                                 vsc9953_port_learn_mode_set(i, PORT_LEARN_NONE);
1381                 }
1382         }
1383
1384         if (vid != ETHSW_CMD_VLAN_ALL) {
1385                 val = bitfield_replace_by_mask(val, VSC9953_AGE_VID_MASK, vid) |
1386                       VSC9953_AGE_VID_EN;
1387         }
1388         out_le32(&l2ana_reg->ana.anag_efil, val);
1389
1390         /* age the dynamic FDB entries */
1391         vsc9953_mac_table_cmd(MAC_TABLE_AGE);
1392
1393         /* clear previously set port and VID */
1394         clrbits_le32(&l2ana_reg->ana.anag_efil, VSC9953_AGE_PORT_EN |
1395                      VSC9953_AGE_PORT_MASK | VSC9953_AGE_VID_EN |
1396                      VSC9953_AGE_VID_MASK);
1397
1398         if (port_no != ETHSW_CMD_PORT_ALL) {
1399                 if (!rc[port_no] && mode[port_no] != PORT_LEARN_NONE)
1400                         vsc9953_port_learn_mode_set(port_no, mode[port_no]);
1401         } else {
1402                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1403                         if (!rc[i] && mode[i] != PORT_LEARN_NONE)
1404                                 vsc9953_port_learn_mode_set(i, mode[i]);
1405                 }
1406         }
1407 }
1408
1409 /* Delete all the dynamic FDB entries */
1410 static void vsc9953_mac_table_flush(int port, int vid)
1411 {
1412         vsc9953_mac_table_age(port, vid);
1413         vsc9953_mac_table_age(port, vid);
1414 }
1415
1416 enum egress_vlan_tag {
1417         EGR_TAG_CLASS = 0,
1418         EGR_TAG_PVID,
1419 };
1420
1421 /* Set egress tag mode for a VSC9953 port */
1422 static void vsc9953_port_vlan_egress_tag_set(int port_no,
1423                                              enum egress_vlan_tag mode)
1424 {
1425         struct vsc9953_rew_reg *l2rew_reg;
1426
1427         l2rew_reg = (struct vsc9953_rew_reg *)(VSC9953_OFFSET +
1428                         VSC9953_REW_OFFSET);
1429
1430         switch (mode) {
1431         case EGR_TAG_CLASS:
1432                 clrbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
1433                              VSC9953_TAG_VID_PVID);
1434                 break;
1435         case EGR_TAG_PVID:
1436                 setbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
1437                              VSC9953_TAG_VID_PVID);
1438                 break;
1439         default:
1440                 printf("Unknown egress VLAN tag mode for port %d\n", port_no);
1441         }
1442 }
1443
1444 /* Get egress tag mode for a VSC9953 port */
1445 static void vsc9953_port_vlan_egress_tag_get(int port_no,
1446                                              enum egress_vlan_tag *mode)
1447 {
1448         u32 val;
1449         struct vsc9953_rew_reg *l2rew_reg;
1450
1451         l2rew_reg = (struct vsc9953_rew_reg *)(VSC9953_OFFSET +
1452                         VSC9953_REW_OFFSET);
1453
1454         val = in_le32(&l2rew_reg->port[port_no].port_tag_cfg);
1455         if (val & VSC9953_TAG_VID_PVID)
1456                 *mode = EGR_TAG_PVID;
1457         else
1458                 *mode = EGR_TAG_CLASS;
1459 }
1460
1461 /* VSC9953 VLAN learning modes */
1462 enum vlan_learning_mode {
1463         SHARED_VLAN_LEARNING,
1464         PRIVATE_VLAN_LEARNING,
1465 };
1466
1467 /* Set VLAN learning mode for VSC9953 */
1468 static void vsc9953_vlan_learning_set(enum vlan_learning_mode lrn_mode)
1469 {
1470         struct vsc9953_analyzer *l2ana_reg;
1471
1472         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1473                         VSC9953_ANA_OFFSET);
1474
1475         switch (lrn_mode) {
1476         case SHARED_VLAN_LEARNING:
1477                 setbits_le32(&l2ana_reg->ana.agen_ctrl, VSC9953_FID_MASK_ALL);
1478                 break;
1479         case PRIVATE_VLAN_LEARNING:
1480                 clrbits_le32(&l2ana_reg->ana.agen_ctrl, VSC9953_FID_MASK_ALL);
1481                 break;
1482         default:
1483                 printf("Unknown VLAN learn mode\n");
1484         }
1485 }
1486
1487 /* Get VLAN learning mode for VSC9953 */
1488 static int vsc9953_vlan_learning_get(enum vlan_learning_mode *lrn_mode)
1489 {
1490         u32 val;
1491         struct vsc9953_analyzer *l2ana_reg;
1492
1493         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1494                         VSC9953_ANA_OFFSET);
1495
1496         val = in_le32(&l2ana_reg->ana.agen_ctrl);
1497
1498         if (!(val & VSC9953_FID_MASK_ALL)) {
1499                 *lrn_mode = PRIVATE_VLAN_LEARNING;
1500         } else if ((val & VSC9953_FID_MASK_ALL) == VSC9953_FID_MASK_ALL) {
1501                 *lrn_mode = SHARED_VLAN_LEARNING;
1502         } else {
1503                 printf("Unknown VLAN learning mode\n");
1504                 return -EINVAL;
1505         }
1506
1507         return 0;
1508 }
1509
1510 /* Enable/disable VLAN ingress filtering on a VSC9953 port */
1511 static void vsc9953_port_ingress_filtering_set(int port_no, int enabled)
1512 {
1513         struct vsc9953_analyzer *l2ana_reg;
1514
1515         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1516                         VSC9953_ANA_OFFSET);
1517
1518         if (enabled)
1519                 setbits_le32(&l2ana_reg->ana.vlan_mask, 1 << port_no);
1520         else
1521                 clrbits_le32(&l2ana_reg->ana.vlan_mask, 1 << port_no);
1522 }
1523
1524 /* Return VLAN ingress filtering on a VSC9953 port */
1525 static int vsc9953_port_ingress_filtering_get(int port_no)
1526 {
1527         u32 val;
1528         struct vsc9953_analyzer *l2ana_reg;
1529
1530         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1531                         VSC9953_ANA_OFFSET);
1532
1533         val = in_le32(&l2ana_reg->ana.vlan_mask);
1534         return !!(val & (1 << port_no));
1535 }
1536
1537 /* Get the aggregation group of a port */
1538 static int vsc9953_port_aggr_grp_get(int port_no, int *aggr_grp)
1539 {
1540         u32 val;
1541         struct vsc9953_analyzer *l2ana_reg;
1542
1543         if (!VSC9953_PORT_CHECK(port_no))
1544                 return -EINVAL;
1545
1546         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1547                                                 VSC9953_ANA_OFFSET);
1548
1549         val = in_le32(&l2ana_reg->port[port_no].port_cfg);
1550         *aggr_grp = bitfield_extract_by_mask(val,
1551                                              VSC9953_PORT_CFG_PORTID_MASK);
1552
1553         return 0;
1554 }
1555
1556 static void vsc9953_aggr_grp_members_get(int aggr_grp,
1557                                          u8 aggr_membr[VSC9953_MAX_PORTS])
1558 {
1559         int port_no;
1560         int aggr_membr_grp;
1561
1562         for (port_no = 0; port_no < VSC9953_MAX_PORTS; port_no++) {
1563                 aggr_membr[port_no] = 0;
1564
1565                 if (vsc9953_port_aggr_grp_get(port_no, &aggr_membr_grp))
1566                         continue;
1567
1568                 if (aggr_grp == aggr_membr_grp)
1569                         aggr_membr[port_no] = 1;
1570         }
1571 }
1572
1573 static void vsc9953_update_dest_members_masks(int port_no, u32 membr_bitfld_old,
1574                                               u32 membr_bitfld_new)
1575 {
1576         int i;
1577         u32 pgid;
1578         struct vsc9953_analyzer *l2ana_reg;
1579
1580         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1581                                                 VSC9953_ANA_OFFSET);
1582
1583         /*
1584          * NOTE: Only the unicast destination masks are updated, since
1585          * we do not support for now Layer-2 multicast entries
1586          */
1587         for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1588                 if (i == port_no) {
1589                         clrsetbits_le32(&l2ana_reg->port_id_tbl.port_grp_id[i],
1590                                         VSC9953_PGID_PORT_MASK,
1591                                         membr_bitfld_new);
1592                         continue;
1593                 }
1594
1595                 pgid = in_le32(&l2ana_reg->port_id_tbl.port_grp_id[i]);
1596                 if ((u32)(1 << i) & membr_bitfld_old & VSC9953_PGID_PORT_MASK)
1597                         pgid &= ~((u32)(1 << port_no));
1598                 if ((u32)(1 << i) & membr_bitfld_new & VSC9953_PGID_PORT_MASK)
1599                         pgid |= ((u32)(1 << port_no));
1600
1601                 out_le32(&l2ana_reg->port_id_tbl.port_grp_id[i], pgid);
1602         }
1603 }
1604
1605 static void vsc9953_update_source_members_masks(int port_no,
1606                                                 u32 membr_bitfld_old,
1607                                                 u32 membr_bitfld_new)
1608 {
1609         int i;
1610         int index;
1611         u32 pgid;
1612         struct vsc9953_analyzer *l2ana_reg;
1613
1614         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1615                                                 VSC9953_ANA_OFFSET);
1616
1617         for (i = 0; i < VSC9953_MAX_PORTS + 1; i++) {
1618                 index = PGID_SRC_START + i;
1619                 pgid = in_le32(&l2ana_reg->port_id_tbl.port_grp_id[index]);
1620                 if (i == port_no) {
1621                         pgid = (pgid | VSC9953_PGID_PORT_MASK) &
1622                                ~membr_bitfld_new;
1623                         out_le32(&l2ana_reg->port_id_tbl.port_grp_id[index],
1624                                  pgid);
1625                         continue;
1626                 }
1627
1628                 if ((u32)(1 << i) & membr_bitfld_old & VSC9953_PGID_PORT_MASK)
1629                         pgid |= (u32)(1 << port_no);
1630
1631                 if ((u32)(1 << i) & membr_bitfld_new & VSC9953_PGID_PORT_MASK)
1632                         pgid &= ~(u32)(1 << port_no);
1633                 out_le32(&l2ana_reg->port_id_tbl.port_grp_id[index], pgid);
1634         }
1635 }
1636
1637 static u32 vsc9953_aggr_mask_get_next(u32 aggr_mask, u32 member_bitfield)
1638 {
1639         if (!member_bitfield)
1640                 return 0;
1641
1642         if (!(aggr_mask & VSC9953_PGID_PORT_MASK))
1643                 aggr_mask = 1;
1644         else
1645                 aggr_mask <<= 1;
1646
1647         while (!(aggr_mask & member_bitfield)) {
1648                 aggr_mask <<= 1;
1649                 if (!(aggr_mask & VSC9953_PGID_PORT_MASK))
1650                         aggr_mask = 1;
1651         }
1652
1653         return aggr_mask;
1654 }
1655
1656 static void vsc9953_update_aggr_members_masks(int port_no, u32 membr_bitfld_old,
1657                                               u32 membr_bitfld_new)
1658 {
1659         int i;
1660         u32 pgid;
1661         u32 aggr_mask_old = 0;
1662         u32 aggr_mask_new = 0;
1663         struct vsc9953_analyzer *l2ana_reg;
1664
1665         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1666                                                 VSC9953_ANA_OFFSET);
1667
1668         /* Update all the PGID aggregation masks */
1669         for (i = PGID_AGGR_START; i < PGID_SRC_START; i++) {
1670                 pgid = in_le32(&l2ana_reg->port_id_tbl.port_grp_id[i]);
1671
1672                 aggr_mask_old = vsc9953_aggr_mask_get_next(aggr_mask_old,
1673                                                            membr_bitfld_old);
1674                 pgid = (pgid & ~membr_bitfld_old) | aggr_mask_old;
1675
1676                 aggr_mask_new = vsc9953_aggr_mask_get_next(aggr_mask_new,
1677                                                            membr_bitfld_new);
1678                 pgid = (pgid & ~membr_bitfld_new) | aggr_mask_new;
1679
1680                 out_le32(&l2ana_reg->port_id_tbl.port_grp_id[i], pgid);
1681         }
1682 }
1683
1684 static u32 vsc9953_aggr_membr_bitfield_get(u8 member[VSC9953_MAX_PORTS])
1685 {
1686         int i;
1687         u32 member_bitfield = 0;
1688
1689         for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1690                 if (member[i])
1691                         member_bitfield |= 1 << i;
1692         }
1693         member_bitfield &= VSC9953_PGID_PORT_MASK;
1694
1695         return member_bitfield;
1696 }
1697
1698 static void vsc9953_update_members_masks(int port_no,
1699                                          u8 member_old[VSC9953_MAX_PORTS],
1700                                          u8 member_new[VSC9953_MAX_PORTS])
1701 {
1702         u32 membr_bitfld_old = vsc9953_aggr_membr_bitfield_get(member_old);
1703         u32 membr_bitfld_new = vsc9953_aggr_membr_bitfield_get(member_new);
1704
1705         vsc9953_update_dest_members_masks(port_no, membr_bitfld_old,
1706                                           membr_bitfld_new);
1707         vsc9953_update_source_members_masks(port_no, membr_bitfld_old,
1708                                             membr_bitfld_new);
1709         vsc9953_update_aggr_members_masks(port_no, membr_bitfld_old,
1710                                           membr_bitfld_new);
1711 }
1712
1713 /* Set the aggregation group of a port */
1714 static int vsc9953_port_aggr_grp_set(int port_no, int aggr_grp)
1715 {
1716         u8 aggr_membr_old[VSC9953_MAX_PORTS];
1717         u8 aggr_membr_new[VSC9953_MAX_PORTS];
1718         int rc;
1719         int aggr_grp_old;
1720         u32 val;
1721         struct vsc9953_analyzer *l2ana_reg;
1722
1723         if (!VSC9953_PORT_CHECK(port_no) || !VSC9953_PORT_CHECK(aggr_grp))
1724                 return -EINVAL;
1725
1726         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1727                                                 VSC9953_ANA_OFFSET);
1728
1729         rc = vsc9953_port_aggr_grp_get(port_no, &aggr_grp_old);
1730         if (rc)
1731                 return rc;
1732
1733         /* get all the members of the old aggregation group */
1734         vsc9953_aggr_grp_members_get(aggr_grp_old, aggr_membr_old);
1735
1736         /* get all the members of the same aggregation group */
1737         vsc9953_aggr_grp_members_get(aggr_grp, aggr_membr_new);
1738
1739         /* add current port as member to the new aggregation group */
1740         aggr_membr_old[port_no] = 0;
1741         aggr_membr_new[port_no] = 1;
1742
1743         /* update masks */
1744         vsc9953_update_members_masks(port_no, aggr_membr_old, aggr_membr_new);
1745
1746         /* Change logical port number */
1747         val = in_le32(&l2ana_reg->port[port_no].port_cfg);
1748         val = bitfield_replace_by_mask(val,
1749                                        VSC9953_PORT_CFG_PORTID_MASK, aggr_grp);
1750         out_le32(&l2ana_reg->port[port_no].port_cfg, val);
1751
1752         return 0;
1753 }
1754
1755 static int vsc9953_port_status_key_func(struct ethsw_command_def *parsed_cmd)
1756 {
1757         int i;
1758         u8 enabled;
1759
1760         /* Last keyword should tell us if we should enable/disable the port */
1761         if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
1762             ethsw_id_enable)
1763                 enabled = 1;
1764         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
1765                  ethsw_id_disable)
1766                 enabled = 0;
1767         else
1768                 return CMD_RET_USAGE;
1769
1770         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1771                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1772                         printf("Invalid port number: %d\n", parsed_cmd->port);
1773                         return CMD_RET_FAILURE;
1774                 }
1775                 vsc9953_port_status_set(parsed_cmd->port, enabled);
1776         } else {
1777                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1778                         vsc9953_port_status_set(i, enabled);
1779         }
1780
1781         return CMD_RET_SUCCESS;
1782 }
1783
1784 static int vsc9953_port_config_key_func(struct ethsw_command_def *parsed_cmd)
1785 {
1786         int i;
1787
1788         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1789                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1790                         printf("Invalid port number: %d\n", parsed_cmd->port);
1791                         return CMD_RET_FAILURE;
1792                 }
1793                 vsc9953_phy_autoneg(parsed_cmd->port);
1794                 printf("%8s %8s %8s %8s %8s\n",
1795                        "Port", "Status", "Link", "Speed",
1796                        "Duplex");
1797                 vsc9953_port_config_show(parsed_cmd->port);
1798
1799         } else {
1800                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1801                         vsc9953_phy_autoneg(i);
1802                 printf("%8s %8s %8s %8s %8s\n",
1803                        "Port", "Status", "Link", "Speed", "Duplex");
1804                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1805                         vsc9953_port_config_show(i);
1806         }
1807
1808         return CMD_RET_SUCCESS;
1809 }
1810
1811 static int vsc9953_port_stats_key_func(struct ethsw_command_def *parsed_cmd)
1812 {
1813         int i;
1814
1815         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1816                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1817                         printf("Invalid port number: %d\n", parsed_cmd->port);
1818                         return CMD_RET_FAILURE;
1819                 }
1820                 vsc9953_port_statistics_show(parsed_cmd->port);
1821         } else {
1822                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1823                         vsc9953_port_statistics_show(i);
1824         }
1825
1826         return CMD_RET_SUCCESS;
1827 }
1828
1829 static int vsc9953_port_stats_clear_key_func(struct ethsw_command_def
1830                                              *parsed_cmd)
1831 {
1832         int i;
1833
1834         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1835                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1836                         printf("Invalid port number: %d\n", parsed_cmd->port);
1837                         return CMD_RET_FAILURE;
1838                 }
1839                 vsc9953_port_statistics_clear(parsed_cmd->port);
1840         } else {
1841                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1842                         vsc9953_port_statistics_clear(i);
1843         }
1844
1845         return CMD_RET_SUCCESS;
1846 }
1847
1848 static int vsc9953_learn_show_key_func(struct ethsw_command_def *parsed_cmd)
1849 {
1850         int i;
1851         enum port_learn_mode mode;
1852
1853         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1854                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1855                         printf("Invalid port number: %d\n", parsed_cmd->port);
1856                         return CMD_RET_FAILURE;
1857                 }
1858                 if (vsc9953_port_learn_mode_get(parsed_cmd->port, &mode))
1859                         return CMD_RET_FAILURE;
1860                 printf("%7s %11s\n", "Port", "Learn mode");
1861                 switch (mode) {
1862                 case PORT_LEARN_NONE:
1863                         printf("%7d %11s\n", parsed_cmd->port, "disable");
1864                         break;
1865                 case PORT_LEARN_AUTO:
1866                         printf("%7d %11s\n", parsed_cmd->port, "auto");
1867                         break;
1868                 default:
1869                         printf("%7d %11s\n", parsed_cmd->port, "-");
1870                 }
1871         } else {
1872                 printf("%7s %11s\n", "Port", "Learn mode");
1873                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1874                         if (vsc9953_port_learn_mode_get(i, &mode))
1875                                 continue;
1876                         switch (mode) {
1877                         case PORT_LEARN_NONE:
1878                                 printf("%7d %11s\n", i, "disable");
1879                                 break;
1880                         case PORT_LEARN_AUTO:
1881                                 printf("%7d %11s\n", i, "auto");
1882                                 break;
1883                         default:
1884                                 printf("%7d %11s\n", i, "-");
1885                         }
1886                 }
1887         }
1888
1889         return CMD_RET_SUCCESS;
1890 }
1891
1892 static int vsc9953_learn_set_key_func(struct ethsw_command_def *parsed_cmd)
1893 {
1894         int i;
1895         enum port_learn_mode mode;
1896
1897         /* Last keyword should tell us the learn mode */
1898         if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
1899             ethsw_id_auto)
1900                 mode = PORT_LEARN_AUTO;
1901         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
1902                  ethsw_id_disable)
1903                 mode = PORT_LEARN_NONE;
1904         else
1905                 return CMD_RET_USAGE;
1906
1907         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1908                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1909                         printf("Invalid port number: %d\n", parsed_cmd->port);
1910                         return CMD_RET_FAILURE;
1911                 }
1912                 vsc9953_port_learn_mode_set(parsed_cmd->port, mode);
1913         } else {
1914                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1915                         vsc9953_port_learn_mode_set(i, mode);
1916         }
1917
1918         return CMD_RET_SUCCESS;
1919 }
1920
1921 static int vsc9953_fdb_show_key_func(struct ethsw_command_def *parsed_cmd)
1922 {
1923         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL &&
1924             !VSC9953_PORT_CHECK(parsed_cmd->port)) {
1925                 printf("Invalid port number: %d\n", parsed_cmd->port);
1926                 return CMD_RET_FAILURE;
1927         }
1928
1929         if (parsed_cmd->vid != ETHSW_CMD_VLAN_ALL &&
1930             !VSC9953_VLAN_CHECK(parsed_cmd->vid)) {
1931                 printf("Invalid VID number: %d\n", parsed_cmd->vid);
1932                 return CMD_RET_FAILURE;
1933         }
1934
1935         vsc9953_mac_table_show(parsed_cmd->port, parsed_cmd->vid);
1936
1937         return CMD_RET_SUCCESS;
1938 }
1939
1940 static int vsc9953_fdb_flush_key_func(struct ethsw_command_def *parsed_cmd)
1941 {
1942         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL &&
1943             !VSC9953_PORT_CHECK(parsed_cmd->port)) {
1944                 printf("Invalid port number: %d\n", parsed_cmd->port);
1945                 return CMD_RET_FAILURE;
1946         }
1947
1948         if (parsed_cmd->vid != ETHSW_CMD_VLAN_ALL &&
1949             !VSC9953_VLAN_CHECK(parsed_cmd->vid)) {
1950                 printf("Invalid VID number: %d\n", parsed_cmd->vid);
1951                 return CMD_RET_FAILURE;
1952         }
1953
1954         vsc9953_mac_table_flush(parsed_cmd->port, parsed_cmd->vid);
1955
1956         return CMD_RET_SUCCESS;
1957 }
1958
1959 static int vsc9953_fdb_entry_add_key_func(struct ethsw_command_def *parsed_cmd)
1960 {
1961         int vid;
1962
1963         /* a port number must be present */
1964         if (parsed_cmd->port == ETHSW_CMD_PORT_ALL) {
1965                 printf("Please specify a port\n");
1966                 return CMD_RET_FAILURE;
1967         }
1968
1969         if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1970                 printf("Invalid port number: %d\n", parsed_cmd->port);
1971                 return CMD_RET_FAILURE;
1972         }
1973
1974         /* Use VLAN 1 if VID is not set */
1975         vid = (parsed_cmd->vid == ETHSW_CMD_VLAN_ALL ? 1 : parsed_cmd->vid);
1976
1977         if (!VSC9953_VLAN_CHECK(vid)) {
1978                 printf("Invalid VID number: %d\n", vid);
1979                 return CMD_RET_FAILURE;
1980         }
1981
1982         if (vsc9953_mac_table_add(parsed_cmd->port, parsed_cmd->ethaddr, vid))
1983                 return CMD_RET_FAILURE;
1984
1985         return CMD_RET_SUCCESS;
1986 }
1987
1988 static int vsc9953_fdb_entry_del_key_func(struct ethsw_command_def *parsed_cmd)
1989 {
1990         int vid;
1991
1992         /* Use VLAN 1 if VID is not set */
1993         vid = (parsed_cmd->vid == ETHSW_CMD_VLAN_ALL ? 1 : parsed_cmd->vid);
1994
1995         if (!VSC9953_VLAN_CHECK(vid)) {
1996                 printf("Invalid VID number: %d\n", vid);
1997                 return CMD_RET_FAILURE;
1998         }
1999
2000         if (vsc9953_mac_table_del(parsed_cmd->ethaddr, vid))
2001                 return CMD_RET_FAILURE;
2002
2003         return CMD_RET_SUCCESS;
2004 }
2005
2006 static int vsc9953_pvid_show_key_func(struct ethsw_command_def *parsed_cmd)
2007 {
2008         int i;
2009         int pvid;
2010
2011         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2012                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2013                         printf("Invalid port number: %d\n", parsed_cmd->port);
2014                         return CMD_RET_FAILURE;
2015                 }
2016
2017                 if (vsc9953_port_vlan_pvid_get(parsed_cmd->port, &pvid))
2018                         return CMD_RET_FAILURE;
2019                 printf("%7s %7s\n", "Port", "PVID");
2020                 printf("%7d %7d\n", parsed_cmd->port, pvid);
2021         } else {
2022                 printf("%7s %7s\n", "Port", "PVID");
2023                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
2024                         if (vsc9953_port_vlan_pvid_get(i, &pvid))
2025                                 continue;
2026                         printf("%7d %7d\n", i, pvid);
2027                 }
2028         }
2029
2030         return CMD_RET_SUCCESS;
2031 }
2032
2033 static int vsc9953_pvid_set_key_func(struct ethsw_command_def *parsed_cmd)
2034 {
2035         /* PVID number should be set in parsed_cmd->vid */
2036         if (parsed_cmd->vid == ETHSW_CMD_VLAN_ALL) {
2037                 printf("Please set a pvid value\n");
2038                 return CMD_RET_FAILURE;
2039         }
2040
2041         if (!VSC9953_VLAN_CHECK(parsed_cmd->vid)) {
2042                 printf("Invalid VID number: %d\n", parsed_cmd->vid);
2043                 return CMD_RET_FAILURE;
2044         }
2045
2046         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2047                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2048                         printf("Invalid port number: %d\n", parsed_cmd->port);
2049                         return CMD_RET_FAILURE;
2050                 }
2051                 vsc9953_port_vlan_pvid_set(parsed_cmd->port, parsed_cmd->vid);
2052         } else {
2053                 vsc9953_port_all_vlan_pvid_set(parsed_cmd->vid);
2054         }
2055
2056         return CMD_RET_SUCCESS;
2057 }
2058
2059 static int vsc9953_vlan_show_key_func(struct ethsw_command_def *parsed_cmd)
2060 {
2061         int i;
2062
2063         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2064                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2065                         printf("Invalid port number: %d\n", parsed_cmd->port);
2066                         return CMD_RET_FAILURE;
2067                 }
2068                 vsc9953_vlan_membership_show(parsed_cmd->port);
2069         } else {
2070                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
2071                         vsc9953_vlan_membership_show(i);
2072         }
2073
2074         return CMD_RET_SUCCESS;
2075 }
2076
2077 static int vsc9953_vlan_set_key_func(struct ethsw_command_def *parsed_cmd)
2078 {
2079         int i;
2080         int add;
2081
2082         /* VLAN should be set in parsed_cmd->vid */
2083         if (parsed_cmd->vid == ETHSW_CMD_VLAN_ALL) {
2084                 printf("Please set a vlan value\n");
2085                 return CMD_RET_FAILURE;
2086         }
2087
2088         if (!VSC9953_VLAN_CHECK(parsed_cmd->vid)) {
2089                 printf("Invalid VID number: %d\n", parsed_cmd->vid);
2090                 return CMD_RET_FAILURE;
2091         }
2092
2093         /* keywords add/delete should be the last but one in array */
2094         if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 2] ==
2095             ethsw_id_add)
2096                 add = 1;
2097         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 2] ==
2098                  ethsw_id_del)
2099                 add = 0;
2100         else
2101                 return CMD_RET_USAGE;
2102
2103         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2104                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2105                         printf("Invalid port number: %d\n", parsed_cmd->port);
2106                         return CMD_RET_FAILURE;
2107                 }
2108                 vsc9953_vlan_table_membership_set(parsed_cmd->vid,
2109                                                   parsed_cmd->port, add);
2110         } else {
2111                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
2112                         vsc9953_vlan_table_membership_set(parsed_cmd->vid, i,
2113                                                           add);
2114         }
2115
2116         return CMD_RET_SUCCESS;
2117 }
2118 static int vsc9953_port_untag_show_key_func(
2119                 struct ethsw_command_def *parsed_cmd)
2120 {
2121         int i;
2122
2123         printf("%7s\t%17s\n", "Port", "Untag");
2124         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2125                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2126                         printf("Invalid port number: %d\n", parsed_cmd->port);
2127                         return CMD_RET_FAILURE;
2128                 }
2129                 vsc9953_port_vlan_egr_untag_show(parsed_cmd->port);
2130         } else {
2131                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
2132                         vsc9953_port_vlan_egr_untag_show(i);
2133         }
2134
2135         return CMD_RET_SUCCESS;
2136 }
2137
2138 static int vsc9953_port_untag_set_key_func(struct ethsw_command_def *parsed_cmd)
2139 {
2140         int i;
2141         enum egress_untag_mode mode;
2142
2143         /* keywords for the untagged mode are the last in the array */
2144         if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
2145             ethsw_id_all)
2146                 mode = EGRESS_UNTAG_ALL;
2147         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
2148                  ethsw_id_none)
2149                 mode = EGRESS_UNTAG_NONE;
2150         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
2151                  ethsw_id_pvid)
2152                 mode = EGRESS_UNTAG_PVID_AND_ZERO;
2153         else
2154                 return CMD_RET_USAGE;
2155
2156         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2157                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2158                         printf("Invalid port number: %d\n", parsed_cmd->port);
2159                         return CMD_RET_FAILURE;
2160                 }
2161                 vsc9953_port_vlan_egr_untag_set(parsed_cmd->port, mode);
2162         } else {
2163                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
2164                         vsc9953_port_vlan_egr_untag_set(i, mode);
2165         }
2166
2167         return CMD_RET_SUCCESS;
2168 }
2169
2170 static int vsc9953_egr_vlan_tag_show_key_func(
2171                 struct ethsw_command_def *parsed_cmd)
2172 {
2173         int i;
2174         enum egress_vlan_tag mode;
2175
2176         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2177                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2178                         printf("Invalid port number: %d\n", parsed_cmd->port);
2179                         return CMD_RET_FAILURE;
2180                 }
2181                 vsc9953_port_vlan_egress_tag_get(parsed_cmd->port, &mode);
2182                 printf("%7s\t%12s\n", "Port", "Egress VID");
2183                 printf("%7d\t", parsed_cmd->port);
2184                 switch (mode) {
2185                 case EGR_TAG_CLASS:
2186                         printf("%12s\n", "classified");
2187                         break;
2188                 case EGR_TAG_PVID:
2189                         printf("%12s\n", "pvid");
2190                         break;
2191                 default:
2192                         printf("%12s\n", "-");
2193                 }
2194         } else {
2195                 printf("%7s\t%12s\n", "Port", "Egress VID");
2196                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
2197                         vsc9953_port_vlan_egress_tag_get(i, &mode);
2198                         switch (mode) {
2199                         case EGR_TAG_CLASS:
2200                                 printf("%7d\t%12s\n", i, "classified");
2201                                 break;
2202                         case EGR_TAG_PVID:
2203                                 printf("%7d\t%12s\n", i, "pvid");
2204                                 break;
2205                         default:
2206                                 printf("%7d\t%12s\n", i, "-");
2207                         }
2208                 }
2209         }
2210
2211         return CMD_RET_SUCCESS;
2212 }
2213
2214 static int vsc9953_egr_vlan_tag_set_key_func(
2215                 struct ethsw_command_def *parsed_cmd)
2216 {
2217         int i;
2218         enum egress_vlan_tag mode;
2219
2220         /* keywords for the egress vlan tag mode are the last in the array */
2221         if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
2222             ethsw_id_pvid)
2223                 mode = EGR_TAG_PVID;
2224         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
2225                  ethsw_id_classified)
2226                 mode = EGR_TAG_CLASS;
2227         else
2228                 return CMD_RET_USAGE;
2229
2230         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2231                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2232                         printf("Invalid port number: %d\n", parsed_cmd->port);
2233                         return CMD_RET_FAILURE;
2234                 }
2235                 vsc9953_port_vlan_egress_tag_set(parsed_cmd->port, mode);
2236         } else {
2237                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
2238                         vsc9953_port_vlan_egress_tag_set(i, mode);
2239         }
2240
2241         return CMD_RET_SUCCESS;
2242 }
2243
2244 static int vsc9953_vlan_learn_show_key_func(
2245                 struct ethsw_command_def *parsed_cmd)
2246 {
2247         int rc;
2248         enum vlan_learning_mode mode;
2249
2250         rc = vsc9953_vlan_learning_get(&mode);
2251         if (rc)
2252                 return CMD_RET_FAILURE;
2253
2254         switch (mode) {
2255         case SHARED_VLAN_LEARNING:
2256                 printf("VLAN learning mode: shared\n");
2257                 break;
2258         case PRIVATE_VLAN_LEARNING:
2259                 printf("VLAN learning mode: private\n");
2260                 break;
2261         default:
2262                 printf("Unknown VLAN learning mode\n");
2263                 rc = CMD_RET_FAILURE;
2264         }
2265
2266         return CMD_RET_SUCCESS;
2267 }
2268
2269 static int vsc9953_vlan_learn_set_key_func(struct ethsw_command_def *parsed_cmd)
2270 {
2271         enum vlan_learning_mode mode;
2272
2273         /* keywords for shared/private are the last in the array */
2274         if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
2275             ethsw_id_shared)
2276                 mode = SHARED_VLAN_LEARNING;
2277         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
2278                  ethsw_id_private)
2279                 mode = PRIVATE_VLAN_LEARNING;
2280         else
2281                 return CMD_RET_USAGE;
2282
2283         vsc9953_vlan_learning_set(mode);
2284
2285         return CMD_RET_SUCCESS;
2286 }
2287
2288 static int vsc9953_ingr_fltr_show_key_func(struct ethsw_command_def *parsed_cmd)
2289 {
2290         int i;
2291         int enabled;
2292
2293         printf("%7s\t%18s\n", "Port", "Ingress filtering");
2294         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2295                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2296                         printf("Invalid port number: %d\n", parsed_cmd->port);
2297                         return CMD_RET_FAILURE;
2298                 }
2299                 enabled = vsc9953_port_ingress_filtering_get(parsed_cmd->port);
2300                 printf("%7d\t%18s\n", parsed_cmd->port, enabled ? "enable" :
2301                                                                   "disable");
2302         } else {
2303                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
2304                         enabled = vsc9953_port_ingress_filtering_get(i);
2305                         printf("%7d\t%18s\n", parsed_cmd->port, enabled ?
2306                                                                 "enable" :
2307                                                                 "disable");
2308                 }
2309         }
2310
2311         return CMD_RET_SUCCESS;
2312 }
2313
2314 static int vsc9953_ingr_fltr_set_key_func(struct ethsw_command_def *parsed_cmd)
2315 {
2316         int i;
2317         int enable;
2318
2319         /* keywords for enabling/disabling ingress filtering
2320          * are the last in the array
2321          */
2322         if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
2323             ethsw_id_enable)
2324                 enable = 1;
2325         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
2326                  ethsw_id_disable)
2327                 enable = 0;
2328         else
2329                 return CMD_RET_USAGE;
2330
2331         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2332                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2333                         printf("Invalid port number: %d\n", parsed_cmd->port);
2334                         return CMD_RET_FAILURE;
2335                 }
2336                 vsc9953_port_ingress_filtering_set(parsed_cmd->port, enable);
2337         } else {
2338                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
2339                         vsc9953_port_ingress_filtering_set(i, enable);
2340         }
2341
2342         return CMD_RET_SUCCESS;
2343 }
2344
2345 static int vsc9953_port_aggr_show_key_func(struct ethsw_command_def *parsed_cmd)
2346 {
2347         int i;
2348         int aggr_grp;
2349
2350         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2351                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2352                         printf("Invalid port number: %d\n", parsed_cmd->port);
2353                         return CMD_RET_FAILURE;
2354                 }
2355
2356                 if (vsc9953_port_aggr_grp_get(parsed_cmd->port, &aggr_grp))
2357                         return CMD_RET_FAILURE;
2358                 printf("%7s %10s\n", "Port", "Aggr grp");
2359                 printf("%7d %10d\n", parsed_cmd->port, aggr_grp);
2360         } else {
2361                 printf("%7s %10s\n", "Port", "Aggr grp");
2362                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
2363                         if (vsc9953_port_aggr_grp_get(i, &aggr_grp))
2364                                 continue;
2365                         printf("%7d %10d\n", i, aggr_grp);
2366                 }
2367         }
2368
2369         return CMD_RET_SUCCESS;
2370 }
2371
2372 static int vsc9953_port_aggr_set_key_func(struct ethsw_command_def *parsed_cmd)
2373 {
2374         int i;
2375
2376         /* Aggregation group number should be set in parsed_cmd->aggr_grp */
2377         if (parsed_cmd->aggr_grp == ETHSW_CMD_AGGR_GRP_NONE) {
2378                 printf("Please set an aggregation group value\n");
2379                 return CMD_RET_FAILURE;
2380         }
2381
2382         if (!VSC9953_PORT_CHECK(parsed_cmd->aggr_grp)) {
2383                 printf("Invalid aggregation group number: %d\n",
2384                        parsed_cmd->aggr_grp);
2385                 return CMD_RET_FAILURE;
2386         }
2387
2388         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
2389                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
2390                         printf("Invalid port number: %d\n", parsed_cmd->port);
2391                         return CMD_RET_FAILURE;
2392                 }
2393                 if (vsc9953_port_aggr_grp_set(parsed_cmd->port,
2394                                               parsed_cmd->aggr_grp)) {
2395                         printf("Port %d: failed to set aggr group %d\n",
2396                                parsed_cmd->port, parsed_cmd->aggr_grp);
2397                 }
2398         } else {
2399                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
2400                         if (vsc9953_port_aggr_grp_set(i,
2401                                                       parsed_cmd->aggr_grp)) {
2402                                 printf("Port %d: failed to set aggr group %d\n",
2403                                        i, parsed_cmd->aggr_grp);
2404                         }
2405                 }
2406         }
2407
2408         return CMD_RET_SUCCESS;
2409 }
2410
2411 static struct ethsw_command_func vsc9953_cmd_func = {
2412                 .ethsw_name = "L2 Switch VSC9953",
2413                 .port_enable = &vsc9953_port_status_key_func,
2414                 .port_disable = &vsc9953_port_status_key_func,
2415                 .port_show = &vsc9953_port_config_key_func,
2416                 .port_stats = &vsc9953_port_stats_key_func,
2417                 .port_stats_clear = &vsc9953_port_stats_clear_key_func,
2418                 .port_learn = &vsc9953_learn_set_key_func,
2419                 .port_learn_show = &vsc9953_learn_show_key_func,
2420                 .fdb_show = &vsc9953_fdb_show_key_func,
2421                 .fdb_flush = &vsc9953_fdb_flush_key_func,
2422                 .fdb_entry_add = &vsc9953_fdb_entry_add_key_func,
2423                 .fdb_entry_del = &vsc9953_fdb_entry_del_key_func,
2424                 .pvid_show = &vsc9953_pvid_show_key_func,
2425                 .pvid_set = &vsc9953_pvid_set_key_func,
2426                 .vlan_show = &vsc9953_vlan_show_key_func,
2427                 .vlan_set = &vsc9953_vlan_set_key_func,
2428                 .port_untag_show = &vsc9953_port_untag_show_key_func,
2429                 .port_untag_set = &vsc9953_port_untag_set_key_func,
2430                 .port_egr_vlan_show = &vsc9953_egr_vlan_tag_show_key_func,
2431                 .port_egr_vlan_set = &vsc9953_egr_vlan_tag_set_key_func,
2432                 .vlan_learn_show = &vsc9953_vlan_learn_show_key_func,
2433                 .vlan_learn_set = &vsc9953_vlan_learn_set_key_func,
2434                 .port_ingr_filt_show = &vsc9953_ingr_fltr_show_key_func,
2435                 .port_ingr_filt_set = &vsc9953_ingr_fltr_set_key_func,
2436                 .port_aggr_show = &vsc9953_port_aggr_show_key_func,
2437                 .port_aggr_set = &vsc9953_port_aggr_set_key_func,
2438 };
2439
2440 #endif /* CONFIG_CMD_ETHSW */
2441
2442 /*****************************************************************************
2443 At startup, the default configuration would be:
2444         - HW learning enabled on all ports; (HW default)
2445         - All ports are in VLAN 1;
2446         - All ports are VLAN aware;
2447         - All ports have POP_COUNT 1;
2448         - All ports have PVID 1;
2449         - All ports have TPID 0x8100; (HW default)
2450         - All ports tag frames classified to all VLANs that are not PVID;
2451 *****************************************************************************/
2452 void vsc9953_default_configuration(void)
2453 {
2454         int i;
2455
2456         if (vsc9953_autoage_time_set(VSC9953_DEFAULT_AGE_TIME))
2457                 debug("VSC9953: failed to set AGE time to %d\n",
2458                       VSC9953_DEFAULT_AGE_TIME);
2459
2460         for (i = 0; i < VSC9953_MAX_VLAN; i++)
2461                 vsc9953_vlan_table_membership_all_set(i, 0);
2462         vsc9953_port_all_vlan_aware_set(1);
2463         vsc9953_port_all_vlan_pvid_set(1);
2464         vsc9953_port_all_vlan_poncnt_set(1);
2465         vsc9953_vlan_table_membership_all_set(1, 1);
2466         vsc9953_vlan_ingr_fltr_learn_drop(1);
2467         vsc9953_port_all_vlan_egress_untagged_set(EGRESS_UNTAG_PVID_AND_ZERO);
2468         if (vsc9953_aggr_code_set(AGGR_CODE_ALL))
2469                 debug("VSC9953: failed to set default aggregation code mode\n");
2470 }
2471
2472 void vsc9953_init(bd_t *bis)
2473 {
2474         u32 i;
2475         u32 hdx_cfg = 0;
2476         u32 phy_addr = 0;
2477         int timeout;
2478         struct vsc9953_system_reg *l2sys_reg;
2479         struct vsc9953_qsys_reg *l2qsys_reg;
2480         struct vsc9953_dev_gmii *l2dev_gmii_reg;
2481         struct vsc9953_analyzer *l2ana_reg;
2482         struct vsc9953_devcpu_gcb *l2dev_gcb;
2483
2484         l2dev_gmii_reg = (struct vsc9953_dev_gmii *)(VSC9953_OFFSET +
2485                         VSC9953_DEV_GMII_OFFSET);
2486
2487         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
2488                         VSC9953_ANA_OFFSET);
2489
2490         l2sys_reg = (struct vsc9953_system_reg *)(VSC9953_OFFSET +
2491                         VSC9953_SYS_OFFSET);
2492
2493         l2qsys_reg = (struct vsc9953_qsys_reg *)(VSC9953_OFFSET +
2494                         VSC9953_QSYS_OFFSET);
2495
2496         l2dev_gcb = (struct vsc9953_devcpu_gcb *)(VSC9953_OFFSET +
2497                         VSC9953_DEVCPU_GCB);
2498
2499         out_le32(&l2dev_gcb->chip_regs.soft_rst,
2500                  VSC9953_SOFT_SWC_RST_ENA);
2501         timeout = 50000;
2502         while ((in_le32(&l2dev_gcb->chip_regs.soft_rst) &
2503                         VSC9953_SOFT_SWC_RST_ENA) && --timeout)
2504                 udelay(1); /* busy wait for vsc9953 soft reset */
2505         if (timeout == 0)
2506                 debug("Timeout waiting for VSC9953 to reset\n");
2507
2508         out_le32(&l2sys_reg->sys.reset_cfg, VSC9953_MEM_ENABLE |
2509                  VSC9953_MEM_INIT);
2510
2511         timeout = 50000;
2512         while ((in_le32(&l2sys_reg->sys.reset_cfg) &
2513                 VSC9953_MEM_INIT) && --timeout)
2514                 udelay(1); /* busy wait for vsc9953 memory init */
2515         if (timeout == 0)
2516                 debug("Timeout waiting for VSC9953 memory to initialize\n");
2517
2518         out_le32(&l2sys_reg->sys.reset_cfg, (in_le32(&l2sys_reg->sys.reset_cfg)
2519                         | VSC9953_CORE_ENABLE));
2520
2521         /* VSC9953 Setting to be done once only */
2522         out_le32(&l2qsys_reg->sys.ext_cpu_cfg, 0x00000b00);
2523
2524         for (i = 0; i < VSC9953_MAX_PORTS; i++) {
2525                 if (vsc9953_port_init(i))
2526                         printf("Failed to initialize l2switch port %d\n", i);
2527
2528                 if (!vsc9953_l2sw.port[i].enabled)
2529                         continue;
2530
2531                 /* Enable VSC9953 GMII Ports Port ID 0 - 7 */
2532                 if (VSC9953_INTERNAL_PORT_CHECK(i)) {
2533                         out_le32(&l2ana_reg->pfc[i].pfc_cfg,
2534                                  VSC9953_PFC_FC_QSGMII);
2535                         out_le32(&l2sys_reg->pause_cfg.mac_fc_cfg[i],
2536                                  VSC9953_MAC_FC_CFG_QSGMII);
2537                 } else {
2538                         out_le32(&l2ana_reg->pfc[i].pfc_cfg,
2539                                  VSC9953_PFC_FC);
2540                         out_le32(&l2sys_reg->pause_cfg.mac_fc_cfg[i],
2541                                  VSC9953_MAC_FC_CFG);
2542                 }
2543
2544                 l2dev_gmii_reg = (struct vsc9953_dev_gmii *)
2545                                  (VSC9953_OFFSET + VSC9953_DEV_GMII_OFFSET +
2546                                  T1040_SWITCH_GMII_DEV_OFFSET * i);
2547
2548                 out_le32(&l2dev_gmii_reg->port_mode.clock_cfg,
2549                          VSC9953_CLOCK_CFG);
2550                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_ena_cfg,
2551                          VSC9953_MAC_ENA_CFG);
2552                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_mode_cfg,
2553                          VSC9953_MAC_MODE_CFG);
2554                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_ifg_cfg,
2555                          VSC9953_MAC_IFG_CFG);
2556                 /* mac_hdx_cfg varies with port id*/
2557                 hdx_cfg = VSC9953_MAC_HDX_CFG | (i << 16);
2558                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_hdx_cfg, hdx_cfg);
2559                 out_le32(&l2sys_reg->sys.front_port_mode[i],
2560                          VSC9953_FRONT_PORT_MODE);
2561                 setbits_le32(&l2qsys_reg->sys.switch_port_mode[i],
2562                              VSC9953_PORT_ENA);
2563                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_maxlen_cfg,
2564                          VSC9953_MAC_MAX_LEN);
2565                 out_le32(&l2sys_reg->pause_cfg.pause_cfg[i],
2566                          VSC9953_PAUSE_CFG);
2567                 /* WAIT FOR 2 us*/
2568                 udelay(2);
2569
2570                 /* Initialize Lynx PHY Wrappers */
2571                 phy_addr = 0;
2572                 if (vsc9953_l2sw.port[i].enet_if ==
2573                                 PHY_INTERFACE_MODE_QSGMII)
2574                         phy_addr = (i + 0x4) & 0x1F;
2575                 else if (vsc9953_l2sw.port[i].enet_if ==
2576                                 PHY_INTERFACE_MODE_SGMII)
2577                         phy_addr = (i + 1) & 0x1F;
2578
2579                 if (phy_addr) {
2580                         /* SGMII IF mode + AN enable */
2581                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
2582                                            0x14, PHY_SGMII_IF_MODE_AN |
2583                                            PHY_SGMII_IF_MODE_SGMII);
2584                         /* Dev ability according to SGMII specification */
2585                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
2586                                            0x4, PHY_SGMII_DEV_ABILITY_SGMII);
2587                         /* Adjust link timer for SGMII
2588                          * 1.6 ms in units of 8 ns = 2 * 10^5 = 0x30d40
2589                          */
2590                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
2591                                            0x13, 0x0003);
2592                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
2593                                            0x12, 0x0d40);
2594                         /* Restart AN */
2595                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
2596                                            0x0, PHY_SGMII_CR_DEF_VAL |
2597                                            PHY_SGMII_CR_RESET_AN);
2598
2599                         timeout = 50000;
2600                         while ((vsc9953_mdio_read(&l2dev_gcb->mii_mng[0],
2601                                         phy_addr, 0x01) & 0x0020) && --timeout)
2602                                 udelay(1); /* wait for AN to complete */
2603                         if (timeout == 0)
2604                                 debug("Timeout waiting for AN to complete\n");
2605                 }
2606         }
2607
2608         vsc9953_default_configuration();
2609
2610 #ifdef CONFIG_CMD_ETHSW
2611         if (ethsw_define_functions(&vsc9953_cmd_func) < 0)
2612                 debug("Unable to use \"ethsw\" commands\n");
2613 #endif
2614
2615         printf("VSC9953 L2 switch initialized\n");
2616         return;
2617 }