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