git.sur5r.net Git - u-boot/blob - drivers/net/vsc9953.c

   1 /*
   2  *  Copyright 2014 Freescale Semiconductor, Inc.
   3  *
   4  *  SPDX-License-Identifier:      GPL-2.0+
   5  *
   6  *  Driver for the Vitesse VSC9953 L2 Switch
   7  */
   8
   9 #include <asm/io.h>
  10 #include <asm/fsl_serdes.h>
  11 #include <fm_eth.h>
  12 #include <fsl_memac.h>
  13 #include <bitfield.h>
  14 #include <errno.h>
  15 #include <malloc.h>
  16 #include <vsc9953.h>
  17 #include <ethsw.h>
  18
  19 static struct vsc9953_info vsc9953_l2sw = {
  20                 .port[0] = VSC9953_PORT_INFO_INITIALIZER(0),
  21                 .port[1] = VSC9953_PORT_INFO_INITIALIZER(1),
  22                 .port[2] = VSC9953_PORT_INFO_INITIALIZER(2),
  23                 .port[3] = VSC9953_PORT_INFO_INITIALIZER(3),
  24                 .port[4] = VSC9953_PORT_INFO_INITIALIZER(4),
  25                 .port[5] = VSC9953_PORT_INFO_INITIALIZER(5),
  26                 .port[6] = VSC9953_PORT_INFO_INITIALIZER(6),
  27                 .port[7] = VSC9953_PORT_INFO_INITIALIZER(7),
  28                 .port[8] = VSC9953_PORT_INFO_INITIALIZER(8),
  29                 .port[9] = VSC9953_PORT_INFO_INITIALIZER(9),
  30 };
  31
  32 void vsc9953_port_info_set_mdio(int port_no, struct mii_dev *bus)
  33 {
  34         if (!VSC9953_PORT_CHECK(port_no))
  35                 return;
  36
  37         vsc9953_l2sw.port[port_no].bus = bus;
  38 }
  39
  40 void vsc9953_port_info_set_phy_address(int port_no, int address)
  41 {
  42         if (!VSC9953_PORT_CHECK(port_no))
  43                 return;
  44
  45         vsc9953_l2sw.port[port_no].phyaddr = address;
  46 }
  47
  48 void vsc9953_port_info_set_phy_int(int port_no, phy_interface_t phy_int)
  49 {
  50         if (!VSC9953_PORT_CHECK(port_no))
  51                 return;
  52
  53         vsc9953_l2sw.port[port_no].enet_if = phy_int;
  54 }
  55
  56 void vsc9953_port_enable(int port_no)
  57 {
  58         if (!VSC9953_PORT_CHECK(port_no))
  59                 return;
  60
  61         vsc9953_l2sw.port[port_no].enabled = 1;
  62 }
  63
  64 void vsc9953_port_disable(int port_no)
  65 {
  66         if (!VSC9953_PORT_CHECK(port_no))
  67                 return;
  68
  69         vsc9953_l2sw.port[port_no].enabled = 0;
  70 }
  71
  72 static void vsc9953_mdio_write(struct vsc9953_mii_mng *phyregs, int port_addr,
  73                 int regnum, int value)
  74 {
  75         int timeout = 50000;
  76
  77         out_le32(&phyregs->miimcmd, (0x1 << 31) | ((port_addr & 0x1f) << 25) |
  78                         ((regnum & 0x1f) << 20) | ((value & 0xffff) << 4) |
  79                         (0x1 << 1));
  80         asm("sync");
  81
  82         while ((in_le32(&phyregs->miimstatus) & 0x8) && --timeout)
  83                 udelay(1);
  84
  85         if (timeout == 0)
  86                 debug("Timeout waiting for MDIO write\n");
  87 }
  88
  89 static int vsc9953_mdio_read(struct vsc9953_mii_mng *phyregs, int port_addr,
  90                 int regnum)
  91 {
  92         int value = 0xFFFF;
  93         int timeout = 50000;
  94
  95         while ((in_le32(&phyregs->miimstatus) & MIIMIND_OPR_PEND) && --timeout)
  96                 udelay(1);
  97         if (timeout == 0) {
  98                 debug("Timeout waiting for MDIO operation to finish\n");
  99                 return value;
 100         }
 101
 102         /* Put the address of the phy, and the register
 103          * number into MIICMD
 104          */
 105         out_le32(&phyregs->miimcmd, (0x1 << 31) | ((port_addr & 0x1f) << 25) |
 106                         ((regnum & 0x1f) << 20) | ((value & 0xffff) << 4) |
 107                         (0x2 << 1));
 108
 109         timeout = 50000;
 110         /* Wait for the the indication that the read is done */
 111         while ((in_le32(&phyregs->miimstatus) & 0x8) && --timeout)
 112                 udelay(1);
 113         if (timeout == 0)
 114                 debug("Timeout waiting for MDIO read\n");
 115
 116         /* Grab the value read from the PHY */
 117         value = in_le32(&phyregs->miimdata);
 118
 119         if ((value & 0x00030000) == 0)
 120                 return value & 0x0000ffff;
 121
 122         return value;
 123 }
 124
 125 static int init_phy(struct eth_device *dev)
 126 {
 127         struct vsc9953_port_info *l2sw_port = dev->priv;
 128         struct phy_device *phydev = NULL;
 129
 130 #ifdef CONFIG_PHYLIB
 131         if (!l2sw_port->bus)
 132                 return 0;
 133         phydev = phy_connect(l2sw_port->bus, l2sw_port->phyaddr, dev,
 134                         l2sw_port->enet_if);
 135         if (!phydev) {
 136                 printf("Failed to connect\n");
 137                 return -1;
 138         }
 139
 140         phydev->supported &= SUPPORTED_10baseT_Half |
 141                         SUPPORTED_10baseT_Full |
 142                         SUPPORTED_100baseT_Half |
 143                         SUPPORTED_100baseT_Full |
 144                         SUPPORTED_1000baseT_Full;
 145         phydev->advertising = phydev->supported;
 146
 147         l2sw_port->phydev = phydev;
 148
 149         phy_config(phydev);
 150 #endif
 151
 152         return 0;
 153 }
 154
 155 static int vsc9953_port_init(int port_no)
 156 {
 157         struct eth_device *dev;
 158
 159         /* Internal ports never have a PHY */
 160         if (VSC9953_INTERNAL_PORT_CHECK(port_no))
 161                 return 0;
 162
 163         /* alloc eth device */
 164         dev = (struct eth_device *)calloc(1, sizeof(struct eth_device));
 165         if (!dev)
 166                 return -ENOMEM;
 167
 168         sprintf(dev->name, "SW@PORT%d", port_no);
 169         dev->priv = &vsc9953_l2sw.port[port_no];
 170         dev->init = NULL;
 171         dev->halt = NULL;
 172         dev->send = NULL;
 173         dev->recv = NULL;
 174
 175         if (init_phy(dev)) {
 176                 free(dev);
 177                 return -ENODEV;
 178         }
 179
 180         return 0;
 181 }
 182
 183 static int vsc9953_vlan_table_poll_idle(void)
 184 {
 185         struct vsc9953_analyzer *l2ana_reg;
 186         int timeout;
 187
 188         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 189                         VSC9953_ANA_OFFSET);
 190
 191         timeout = 50000;
 192         while (((in_le32(&l2ana_reg->ana_tables.vlan_access) &
 193                  VSC9953_VLAN_CMD_MASK) != VSC9953_VLAN_CMD_IDLE) && --timeout)
 194                 udelay(1);
 195
 196         return timeout ? 0 : -EBUSY;
 197 }
 198
 199 /* vlan table set/clear all membership of vid */
 200 static void vsc9953_vlan_table_membership_all_set(int vid, int set_member)
 201 {
 202         uint 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         /* read current vlan configuration */
 214         val = in_le32(&l2ana_reg->ana_tables.vlan_tidx);
 215         out_le32(&l2ana_reg->ana_tables.vlan_tidx,
 216                  bitfield_replace_by_mask(val, VSC9953_ANA_TBL_VID_MASK, vid));
 217
 218         clrsetbits_le32(&l2ana_reg->ana_tables.vlan_access,
 219                         VSC9953_VLAN_CMD_MASK, VSC9953_VLAN_CMD_READ);
 220
 221         if (vsc9953_vlan_table_poll_idle() < 0) {
 222                 debug("VLAN table timeout\n");
 223                 return;
 224         }
 225
 226         val = in_le32(&l2ana_reg->ana_tables.vlan_tidx);
 227         out_le32(&l2ana_reg->ana_tables.vlan_tidx,
 228                  bitfield_replace_by_mask(val, VSC9953_ANA_TBL_VID_MASK, vid));
 229
 230         clrsetbits_le32(&l2ana_reg->ana_tables.vlan_access,
 231                         VSC9953_VLAN_PORT_MASK | VSC9953_VLAN_CMD_MASK,
 232                         VSC9953_VLAN_CMD_WRITE |
 233                         (set_member ? VSC9953_VLAN_PORT_MASK : 0));
 234 }
 235
 236 /* Set PVID for a VSC9953 port */
 237 static void vsc9953_port_vlan_pvid_set(int port_no, int pvid)
 238 {
 239         uint val;
 240         struct vsc9953_analyzer *l2ana_reg;
 241         struct vsc9953_rew_reg *l2rew_reg;
 242
 243         /* Administrative down */
 244         if (!vsc9953_l2sw.port[port_no].enabled) {
 245                 printf("Port %d is administrative down\n", port_no);
 246                 return;
 247         }
 248
 249         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 250                         VSC9953_ANA_OFFSET);
 251         l2rew_reg = (struct vsc9953_rew_reg *)(VSC9953_OFFSET +
 252                         VSC9953_REW_OFFSET);
 253
 254         /* Set PVID on ingress */
 255         val = in_le32(&l2ana_reg->port[port_no].vlan_cfg);
 256         val = bitfield_replace_by_mask(val, VSC9953_VLAN_CFG_VID_MASK, pvid);
 257         out_le32(&l2ana_reg->port[port_no].vlan_cfg, val);
 258
 259         /* Set PVID on egress */
 260         val = in_le32(&l2rew_reg->port[port_no].port_vlan_cfg);
 261         val = bitfield_replace_by_mask(val, VSC9953_PORT_VLAN_CFG_VID_MASK,
 262                                        pvid);
 263         out_le32(&l2rew_reg->port[port_no].port_vlan_cfg, val);
 264 }
 265
 266 static void vsc9953_port_all_vlan_pvid_set(int pvid)
 267 {
 268         int i;
 269
 270         for (i = 0; i < VSC9953_MAX_PORTS; i++)
 271                 vsc9953_port_vlan_pvid_set(i, pvid);
 272 }
 273
 274 /* Enable/disable vlan aware of a VSC9953 port */
 275 static void vsc9953_port_vlan_aware_set(int port_no, int enabled)
 276 {
 277         struct vsc9953_analyzer *l2ana_reg;
 278
 279         /* Administrative down */
 280         if (!vsc9953_l2sw.port[port_no].enabled) {
 281                 printf("Port %d is administrative down\n", port_no);
 282                 return;
 283         }
 284
 285         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 286                         VSC9953_ANA_OFFSET);
 287
 288         if (enabled)
 289                 setbits_le32(&l2ana_reg->port[port_no].vlan_cfg,
 290                              VSC9953_VLAN_CFG_AWARE_ENA);
 291         else
 292                 clrbits_le32(&l2ana_reg->port[port_no].vlan_cfg,
 293                              VSC9953_VLAN_CFG_AWARE_ENA);
 294 }
 295
 296 /* Set all VSC9953 ports' vlan aware  */
 297 static void vsc9953_port_all_vlan_aware_set(int enabled)
 298 {
 299         int i;
 300
 301         for (i = 0; i < VSC9953_MAX_PORTS; i++)
 302                 vsc9953_port_vlan_aware_set(i, enabled);
 303 }
 304
 305 /* Enable/disable vlan pop count of a VSC9953 port */
 306 static void vsc9953_port_vlan_popcnt_set(int port_no, int popcnt)
 307 {
 308         uint val;
 309         struct vsc9953_analyzer *l2ana_reg;
 310
 311         /* Administrative down */
 312         if (!vsc9953_l2sw.port[port_no].enabled) {
 313                 printf("Port %d is administrative down\n", port_no);
 314                 return;
 315         }
 316
 317         if (popcnt > 3 || popcnt < 0) {
 318                 printf("Invalid pop count value: %d\n", port_no);
 319                 return;
 320         }
 321
 322         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 323                         VSC9953_ANA_OFFSET);
 324
 325         val = in_le32(&l2ana_reg->port[port_no].vlan_cfg);
 326         val = bitfield_replace_by_mask(val, VSC9953_VLAN_CFG_POP_CNT_MASK,
 327                                        popcnt);
 328         out_le32(&l2ana_reg->port[port_no].vlan_cfg, val);
 329 }
 330
 331 /* Set all VSC9953 ports' pop count  */
 332 static void vsc9953_port_all_vlan_poncnt_set(int popcnt)
 333 {
 334         int i;
 335
 336         for (i = 0; i < VSC9953_MAX_PORTS; i++)
 337                 vsc9953_port_vlan_popcnt_set(i, popcnt);
 338 }
 339
 340 /* Enable/disable learning for frames dropped due to ingress filtering */
 341 static void vsc9953_vlan_ingr_fltr_learn_drop(int enable)
 342 {
 343         struct vsc9953_analyzer *l2ana_reg;
 344
 345         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 346                         VSC9953_ANA_OFFSET);
 347
 348         if (enable)
 349                 setbits_le32(&l2ana_reg->ana.adv_learn, VSC9953_VLAN_CHK);
 350         else
 351                 clrbits_le32(&l2ana_reg->ana.adv_learn, VSC9953_VLAN_CHK);
 352 }
 353
 354 /* Egress untag modes of a VSC9953 port */
 355 enum egress_untag_mode {
 356         EGRESS_UNTAG_ALL = 0,
 357         EGRESS_UNTAG_PVID_AND_ZERO,
 358         EGRESS_UNTAG_ZERO,
 359         EGRESS_UNTAG_NONE,
 360 };
 361
 362 static void vsc9953_port_vlan_egr_untag_set(int port_no,
 363                                             enum egress_untag_mode mode)
 364 {
 365         struct vsc9953_rew_reg *l2rew_reg;
 366
 367         /* Administrative down */
 368         if (!vsc9953_l2sw.port[port_no].enabled) {
 369                 printf("Port %d is administrative down\n", port_no);
 370                 return;
 371         }
 372
 373         l2rew_reg = (struct vsc9953_rew_reg *)(VSC9953_OFFSET +
 374                         VSC9953_REW_OFFSET);
 375
 376         switch (mode) {
 377         case EGRESS_UNTAG_ALL:
 378                 clrsetbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
 379                                 VSC9953_TAG_CFG_MASK, VSC9953_TAG_CFG_NONE);
 380                 break;
 381         case EGRESS_UNTAG_PVID_AND_ZERO:
 382                 clrsetbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
 383                                 VSC9953_TAG_CFG_MASK,
 384                                 VSC9953_TAG_CFG_ALL_BUT_PVID_ZERO);
 385                 break;
 386         case EGRESS_UNTAG_ZERO:
 387                 clrsetbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
 388                                 VSC9953_TAG_CFG_MASK,
 389                                 VSC9953_TAG_CFG_ALL_BUT_ZERO);
 390                 break;
 391         case EGRESS_UNTAG_NONE:
 392                 clrsetbits_le32(&l2rew_reg->port[port_no].port_tag_cfg,
 393                                 VSC9953_TAG_CFG_MASK, VSC9953_TAG_CFG_ALL);
 394                 break;
 395         default:
 396                 printf("Unknown untag mode for port %d\n", port_no);
 397         }
 398 }
 399
 400 static void vsc9953_port_all_vlan_egress_untagged_set(
 401                 enum egress_untag_mode mode)
 402 {
 403         int i;
 404
 405         for (i = 0; i < VSC9953_MAX_PORTS; i++)
 406                 vsc9953_port_vlan_egr_untag_set(i, mode);
 407 }
 408
 409 #ifdef CONFIG_CMD_ETHSW
 410
 411 /* Enable/disable status of a VSC9953 port */
 412 static void vsc9953_port_status_set(int port_no, u8 enabled)
 413 {
 414         struct vsc9953_qsys_reg *l2qsys_reg;
 415
 416         /* Administrative down */
 417         if (!vsc9953_l2sw.port[port_no].enabled)
 418                 return;
 419
 420         l2qsys_reg = (struct vsc9953_qsys_reg *)(VSC9953_OFFSET +
 421                         VSC9953_QSYS_OFFSET);
 422
 423         if (enabled)
 424                 setbits_le32(&l2qsys_reg->sys.switch_port_mode[port_no],
 425                              VSC9953_PORT_ENA);
 426         else
 427                 clrbits_le32(&l2qsys_reg->sys.switch_port_mode[port_no],
 428                              VSC9953_PORT_ENA);
 429 }
 430
 431 /* Start autonegotiation for a VSC9953 PHY */
 432 static void vsc9953_phy_autoneg(int port_no)
 433 {
 434         if (!vsc9953_l2sw.port[port_no].phydev)
 435                 return;
 436
 437         if (vsc9953_l2sw.port[port_no].phydev->drv->startup(
 438                         vsc9953_l2sw.port[port_no].phydev))
 439                 printf("Failed to start PHY for port %d\n", port_no);
 440 }
 441
 442 /* Print a VSC9953 port's configuration */
 443 static void vsc9953_port_config_show(int port_no)
 444 {
 445         int speed;
 446         int duplex;
 447         int link;
 448         u8 enabled;
 449         u32 val;
 450         struct vsc9953_qsys_reg *l2qsys_reg;
 451
 452         l2qsys_reg = (struct vsc9953_qsys_reg *)(VSC9953_OFFSET +
 453                         VSC9953_QSYS_OFFSET);
 454
 455         val = in_le32(&l2qsys_reg->sys.switch_port_mode[port_no]);
 456         enabled = vsc9953_l2sw.port[port_no].enabled &&
 457                   (val & VSC9953_PORT_ENA);
 458
 459         /* internal ports (8 and 9) are fixed */
 460         if (VSC9953_INTERNAL_PORT_CHECK(port_no)) {
 461                 link = 1;
 462                 speed = SPEED_2500;
 463                 duplex = DUPLEX_FULL;
 464         } else {
 465                 if (vsc9953_l2sw.port[port_no].phydev) {
 466                         link = vsc9953_l2sw.port[port_no].phydev->link;
 467                         speed = vsc9953_l2sw.port[port_no].phydev->speed;
 468                         duplex = vsc9953_l2sw.port[port_no].phydev->duplex;
 469                 } else {
 470                         link = -1;
 471                         speed = -1;
 472                         duplex = -1;
 473                 }
 474         }
 475
 476         printf("%8d ", port_no);
 477         printf("%8s ", enabled == 1 ? "enabled" : "disabled");
 478         printf("%8s ", link == 1 ? "up" : "down");
 479
 480         switch (speed) {
 481         case SPEED_10:
 482                 printf("%8d ", 10);
 483                 break;
 484         case SPEED_100:
 485                 printf("%8d ", 100);
 486                 break;
 487         case SPEED_1000:
 488                 printf("%8d ", 1000);
 489                 break;
 490         case SPEED_2500:
 491                 printf("%8d ", 2500);
 492                 break;
 493         case SPEED_10000:
 494                 printf("%8d ", 10000);
 495                 break;
 496         default:
 497                 printf("%8s ", "-");
 498         }
 499
 500         printf("%8s\n", duplex == DUPLEX_FULL ? "full" : "half");
 501 }
 502
 503 /* Show VSC9953 ports' statistics */
 504 static void vsc9953_port_statistics_show(int port_no)
 505 {
 506         u32 rx_val;
 507         u32 tx_val;
 508         struct vsc9953_system_reg *l2sys_reg;
 509
 510         /* Administrative down */
 511         if (!vsc9953_l2sw.port[port_no].enabled) {
 512                 printf("Port %d is administrative down\n", port_no);
 513                 return;
 514         }
 515
 516         l2sys_reg = (struct vsc9953_system_reg *)(VSC9953_OFFSET +
 517                         VSC9953_SYS_OFFSET);
 518
 519         printf("Statistics for L2 Switch port %d:\n", port_no);
 520
 521         /* Set counter view for our port */
 522         out_le32(&l2sys_reg->sys.stat_cfg, port_no);
 523
 524 #define VSC9953_STATS_PRINTF "%-15s %10u"
 525
 526         /* Get number of Rx and Tx frames */
 527         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_short) +
 528                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_frag) +
 529                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_jabber) +
 530                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_long) +
 531                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_64) +
 532                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_65_127) +
 533                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_128_255) +
 534                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_256_511) +
 535                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_512_1023) +
 536                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_1024_1526) +
 537                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_jumbo);
 538         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_64) +
 539                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_65_127) +
 540                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_128_255) +
 541                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_256_511) +
 542                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_512_1023) +
 543                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_1024_1526) +
 544                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_jumbo);
 545         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 546                "Rx frames:", rx_val, "Tx frames:", tx_val);
 547
 548         /* Get number of Rx and Tx bytes */
 549         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_oct);
 550         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_oct);
 551         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 552                "Rx bytes:", rx_val, "Tx bytes:", tx_val);
 553
 554         /* Get number of Rx frames received ok and Tx frames sent ok */
 555         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_0) +
 556                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_1) +
 557                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_2) +
 558                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_3) +
 559                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_4) +
 560                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_5) +
 561                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_6) +
 562                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_yellow_prio_7) +
 563                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_0) +
 564                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_1) +
 565                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_2) +
 566                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_3) +
 567                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_4) +
 568                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_5) +
 569                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_6) +
 570                  in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_green_prio_7);
 571         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_64) +
 572                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_65_127) +
 573                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_128_255) +
 574                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_256_511) +
 575                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_512_1023) +
 576                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_1024_1526) +
 577                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_jumbo);
 578         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 579                "Rx frames ok:", rx_val, "Tx frames ok:", tx_val);
 580
 581         /* Get number of Rx and Tx unicast frames */
 582         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_uc);
 583         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_uc);
 584         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 585                "Rx unicast:", rx_val, "Tx unicast:", tx_val);
 586
 587         /* Get number of Rx and Tx broadcast frames */
 588         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_bc);
 589         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_bc);
 590         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 591                "Rx broadcast:", rx_val, "Tx broadcast:", tx_val);
 592
 593         /* Get number of Rx and Tx frames of 64B */
 594         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_64);
 595         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_64);
 596         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 597                "Rx 64B:", rx_val, "Tx 64B:", tx_val);
 598
 599         /* Get number of Rx and Tx frames with sizes between 65B and 127B */
 600         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_65_127);
 601         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_65_127);
 602         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 603                "Rx 65B-127B:", rx_val, "Tx 65B-127B:", tx_val);
 604
 605         /* Get number of Rx and Tx frames with sizes between 128B and 255B */
 606         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_128_255);
 607         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_128_255);
 608         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 609                "Rx 128B-255B:", rx_val, "Tx 128B-255B:", tx_val);
 610
 611         /* Get number of Rx and Tx frames with sizes between 256B and 511B */
 612         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_256_511);
 613         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_256_511);
 614         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 615                "Rx 256B-511B:", rx_val, "Tx 256B-511B:", tx_val);
 616
 617         /* Get number of Rx and Tx frames with sizes between 512B and 1023B */
 618         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_512_1023);
 619         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_512_1023);
 620         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 621                "Rx 512B-1023B:", rx_val, "Tx 512B-1023B:", tx_val);
 622
 623         /* Get number of Rx and Tx frames with sizes between 1024B and 1526B */
 624         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_1024_1526);
 625         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_1024_1526);
 626         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 627                "Rx 1024B-1526B:", rx_val, "Tx 1024B-1526B:", tx_val);
 628
 629         /* Get number of Rx and Tx jumbo frames */
 630         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_sz_jumbo);
 631         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_sz_jumbo);
 632         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 633                "Rx jumbo:", rx_val, "Tx jumbo:", tx_val);
 634
 635         /* Get number of Rx and Tx dropped frames */
 636         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_cat_drop) +
 637                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_tail) +
 638                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_0) +
 639                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_1) +
 640                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_2) +
 641                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_3) +
 642                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_4) +
 643                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_5) +
 644                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_6) +
 645                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_yellow_prio_7) +
 646                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_0) +
 647                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_1) +
 648                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_2) +
 649                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_3) +
 650                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_4) +
 651                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_5) +
 652                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_6) +
 653                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_green_prio_7);
 654         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_drop) +
 655                  in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_aged);
 656         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 657                "Rx drops:", rx_val, "Tx drops:", tx_val);
 658
 659         /*
 660          * Get number of Rx frames with CRC or alignment errors
 661          * and number of detected Tx collisions
 662          */
 663         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_crc);
 664         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_col);
 665         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 666                "Rx CRC&align:", rx_val, "Tx coll:", tx_val);
 667
 668         /*
 669          * Get number of Rx undersized frames and
 670          * number of Tx aged frames
 671          */
 672         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_short);
 673         tx_val = in_le32(&l2sys_reg->stat.tx_cntrs.c_tx_aged);
 674         printf(VSC9953_STATS_PRINTF"\t\t"VSC9953_STATS_PRINTF"\n",
 675                "Rx undersize:", rx_val, "Tx aged:", tx_val);
 676
 677         /* Get number of Rx oversized frames */
 678         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_long);
 679         printf(VSC9953_STATS_PRINTF"\n", "Rx oversized:", rx_val);
 680
 681         /* Get number of Rx fragmented frames */
 682         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_frag);
 683         printf(VSC9953_STATS_PRINTF"\n", "Rx fragments:", rx_val);
 684
 685         /* Get number of Rx jabber errors */
 686         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_jabber);
 687         printf(VSC9953_STATS_PRINTF"\n", "Rx jabbers:", rx_val);
 688
 689         /*
 690          * Get number of Rx frames filtered due to classification rules or
 691          * no destination ports
 692          */
 693         rx_val = in_le32(&l2sys_reg->stat.rx_cntrs.c_rx_cat_drop) +
 694                  in_le32(&l2sys_reg->stat.drop_cntrs.c_dr_local);
 695         printf(VSC9953_STATS_PRINTF"\n", "Rx filtered:", rx_val);
 696
 697         printf("\n");
 698 }
 699
 700 /* Clear statistics for a VSC9953 port */
 701 static void vsc9953_port_statistics_clear(int port_no)
 702 {
 703         struct vsc9953_system_reg *l2sys_reg;
 704
 705         /* Administrative down */
 706         if (!vsc9953_l2sw.port[port_no].enabled) {
 707                 printf("Port %d is administrative down\n", port_no);
 708                 return;
 709         }
 710
 711         l2sys_reg = (struct vsc9953_system_reg *)(VSC9953_OFFSET +
 712                         VSC9953_SYS_OFFSET);
 713
 714         /* Clear all counter groups for our ports */
 715         out_le32(&l2sys_reg->sys.stat_cfg, port_no |
 716                  VSC9953_STAT_CLEAR_RX | VSC9953_STAT_CLEAR_TX |
 717                  VSC9953_STAT_CLEAR_DR);
 718 }
 719
 720 enum port_learn_mode {
 721         PORT_LEARN_NONE,
 722         PORT_LEARN_AUTO
 723 };
 724
 725 /* Set learning configuration for a VSC9953 port */
 726 static void vsc9953_port_learn_mode_set(int port_no, enum port_learn_mode mode)
 727 {
 728         struct vsc9953_analyzer *l2ana_reg;
 729
 730         /* Administrative down */
 731         if (!vsc9953_l2sw.port[port_no].enabled) {
 732                 printf("Port %d is administrative down\n", port_no);
 733                 return;
 734         }
 735
 736         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 737                         VSC9953_ANA_OFFSET);
 738
 739         switch (mode) {
 740         case PORT_LEARN_NONE:
 741                 clrbits_le32(&l2ana_reg->port[port_no].port_cfg,
 742                              VSC9953_PORT_CFG_LEARN_DROP |
 743                              VSC9953_PORT_CFG_LEARN_CPU |
 744                              VSC9953_PORT_CFG_LEARN_AUTO |
 745                              VSC9953_PORT_CFG_LEARN_ENA);
 746                 break;
 747         case PORT_LEARN_AUTO:
 748                 clrsetbits_le32(&l2ana_reg->port[port_no].port_cfg,
 749                                 VSC9953_PORT_CFG_LEARN_DROP |
 750                                 VSC9953_PORT_CFG_LEARN_CPU,
 751                                 VSC9953_PORT_CFG_LEARN_ENA |
 752                                 VSC9953_PORT_CFG_LEARN_AUTO);
 753                 break;
 754         default:
 755                 printf("Unknown learn mode for port %d\n", port_no);
 756         }
 757 }
 758
 759 /* Get learning configuration for a VSC9953 port */
 760 static int vsc9953_port_learn_mode_get(int port_no, enum port_learn_mode *mode)
 761 {
 762         u32 val;
 763         struct vsc9953_analyzer *l2ana_reg;
 764
 765         /* Administrative down */
 766         if (!vsc9953_l2sw.port[port_no].enabled) {
 767                 printf("Port %d is administrative down\n", port_no);
 768                 return -1;
 769         }
 770
 771         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 772                         VSC9953_ANA_OFFSET);
 773
 774         /* For now we only support HW learning (auto) and no learning */
 775         val = in_le32(&l2ana_reg->port[port_no].port_cfg);
 776         if ((val & (VSC9953_PORT_CFG_LEARN_ENA |
 777                     VSC9953_PORT_CFG_LEARN_AUTO)) ==
 778             (VSC9953_PORT_CFG_LEARN_ENA | VSC9953_PORT_CFG_LEARN_AUTO))
 779                 *mode = PORT_LEARN_AUTO;
 780         else
 781                 *mode = PORT_LEARN_NONE;
 782
 783         return 0;
 784 }
 785
 786 /* wait for FDB to become available */
 787 static int vsc9953_mac_table_poll_idle(void)
 788 {
 789         struct vsc9953_analyzer *l2ana_reg;
 790         u32 timeout;
 791
 792         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 793                         VSC9953_ANA_OFFSET);
 794
 795         timeout = 50000;
 796         while (((in_le32(&l2ana_reg->ana_tables.mac_access) &
 797                          VSC9953_MAC_CMD_MASK) !=
 798                 VSC9953_MAC_CMD_IDLE) && --timeout)
 799                 udelay(1);
 800
 801         return timeout ? 0 : -EBUSY;
 802 }
 803
 804 /* enum describing available commands for the MAC table */
 805 enum mac_table_cmd {
 806         MAC_TABLE_READ,
 807         MAC_TABLE_LOOKUP,
 808         MAC_TABLE_WRITE,
 809         MAC_TABLE_LEARN,
 810         MAC_TABLE_FORGET,
 811         MAC_TABLE_GET_NEXT,
 812         MAC_TABLE_AGE,
 813 };
 814
 815 /* Issues a command to the FDB table */
 816 static int vsc9953_mac_table_cmd(enum mac_table_cmd cmd)
 817 {
 818         struct vsc9953_analyzer *l2ana_reg;
 819
 820         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 821                         VSC9953_ANA_OFFSET);
 822
 823         switch (cmd) {
 824         case MAC_TABLE_READ:
 825                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
 826                                 VSC9953_MAC_CMD_MASK | VSC9953_MAC_CMD_VALID,
 827                                 VSC9953_MAC_CMD_READ);
 828                 break;
 829         case MAC_TABLE_LOOKUP:
 830                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
 831                                 VSC9953_MAC_CMD_MASK, VSC9953_MAC_CMD_READ |
 832                                 VSC9953_MAC_CMD_VALID);
 833                 break;
 834         case MAC_TABLE_WRITE:
 835                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
 836                                 VSC9953_MAC_CMD_MASK |
 837                                 VSC9953_MAC_ENTRYTYPE_MASK,
 838                                 VSC9953_MAC_CMD_WRITE |
 839                                 VSC9953_MAC_ENTRYTYPE_LOCKED);
 840                 break;
 841         case MAC_TABLE_LEARN:
 842                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
 843                                 VSC9953_MAC_CMD_MASK |
 844                                 VSC9953_MAC_ENTRYTYPE_MASK,
 845                                 VSC9953_MAC_CMD_LEARN |
 846                                 VSC9953_MAC_ENTRYTYPE_LOCKED |
 847                                 VSC9953_MAC_CMD_VALID);
 848                 break;
 849         case MAC_TABLE_FORGET:
 850                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
 851                                 VSC9953_MAC_CMD_MASK |
 852                                 VSC9953_MAC_ENTRYTYPE_MASK,
 853                                 VSC9953_MAC_CMD_FORGET);
 854                 break;
 855         case MAC_TABLE_GET_NEXT:
 856                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
 857                                 VSC9953_MAC_CMD_MASK |
 858                                 VSC9953_MAC_ENTRYTYPE_MASK,
 859                                 VSC9953_MAC_CMD_NEXT);
 860                 break;
 861         case MAC_TABLE_AGE:
 862                 clrsetbits_le32(&l2ana_reg->ana_tables.mac_access,
 863                                 VSC9953_MAC_CMD_MASK |
 864                                 VSC9953_MAC_ENTRYTYPE_MASK,
 865                                 VSC9953_MAC_CMD_AGE);
 866                 break;
 867         default:
 868                 printf("Unknown MAC table command\n");
 869         }
 870
 871         if (vsc9953_mac_table_poll_idle() < 0) {
 872                 debug("MAC table timeout\n");
 873                 return -1;
 874         }
 875
 876         return 0;
 877 }
 878
 879 /* show the FDB entries that correspond to a port and a VLAN */
 880 static void vsc9953_mac_table_show(int port_no, int vid)
 881 {
 882         int rc[VSC9953_MAX_PORTS];
 883         enum port_learn_mode mode[VSC9953_MAX_PORTS];
 884         int i;
 885         u32 val;
 886         u32 vlan;
 887         u32 mach;
 888         u32 macl;
 889         u32 dest_indx;
 890         struct vsc9953_analyzer *l2ana_reg;
 891
 892         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 893                         VSC9953_ANA_OFFSET);
 894
 895         /* disable auto learning */
 896         if (port_no == ETHSW_CMD_PORT_ALL) {
 897                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
 898                         rc[i] = vsc9953_port_learn_mode_get(i, &mode[i]);
 899                         if (!rc[i] && mode[i] != PORT_LEARN_NONE)
 900                                 vsc9953_port_learn_mode_set(i, PORT_LEARN_NONE);
 901                 }
 902         } else {
 903                 rc[port_no] = vsc9953_port_learn_mode_get(port_no,
 904                                                           &mode[port_no]);
 905                 if (!rc[port_no] && mode[port_no] != PORT_LEARN_NONE)
 906                         vsc9953_port_learn_mode_set(port_no, PORT_LEARN_NONE);
 907         }
 908
 909         /* write port and vid to get selected FDB entries */
 910         val = in_le32(&l2ana_reg->ana.anag_efil);
 911         if (port_no != ETHSW_CMD_PORT_ALL) {
 912                 val = bitfield_replace_by_mask(val, VSC9953_AGE_PORT_MASK,
 913                                                port_no) | VSC9953_AGE_PORT_EN;
 914         }
 915         if (vid != ETHSW_CMD_VLAN_ALL) {
 916                 val = bitfield_replace_by_mask(val, VSC9953_AGE_VID_MASK,
 917                                                vid) | VSC9953_AGE_VID_EN;
 918         }
 919         out_le32(&l2ana_reg->ana.anag_efil, val);
 920
 921         /* set MAC and VLAN to 0 to look from beginning */
 922         clrbits_le32(&l2ana_reg->ana_tables.mach_data,
 923                      VSC9953_MAC_VID_MASK | VSC9953_MAC_MACH_MASK);
 924         out_le32(&l2ana_reg->ana_tables.macl_data, 0);
 925
 926         /* get entries */
 927         printf("%10s %17s %5s %4s\n", "EntryType", "MAC", "PORT", "VID");
 928         do {
 929                 if (vsc9953_mac_table_cmd(MAC_TABLE_GET_NEXT) < 0) {
 930                         debug("GET NEXT MAC table command failed\n");
 931                         break;
 932                 }
 933
 934                 val = in_le32(&l2ana_reg->ana_tables.mac_access);
 935
 936                 /* get out when an invalid entry is found */
 937                 if (!(val & VSC9953_MAC_CMD_VALID))
 938                         break;
 939
 940                 switch (val & VSC9953_MAC_ENTRYTYPE_MASK) {
 941                 case VSC9953_MAC_ENTRYTYPE_NORMAL:
 942                         printf("%10s ", "Dynamic");
 943                         break;
 944                 case VSC9953_MAC_ENTRYTYPE_LOCKED:
 945                         printf("%10s ", "Static");
 946                         break;
 947                 case VSC9953_MAC_ENTRYTYPE_IPV4MCAST:
 948                         printf("%10s ", "IPv4 Mcast");
 949                         break;
 950                 case VSC9953_MAC_ENTRYTYPE_IPV6MCAST:
 951                         printf("%10s ", "IPv6 Mcast");
 952                         break;
 953                 default:
 954                         printf("%10s ", "Unknown");
 955                 }
 956
 957                 dest_indx = bitfield_extract_by_mask(val,
 958                                                      VSC9953_MAC_DESTIDX_MASK);
 959
 960                 val = in_le32(&l2ana_reg->ana_tables.mach_data);
 961                 vlan = bitfield_extract_by_mask(val, VSC9953_MAC_VID_MASK);
 962                 mach = bitfield_extract_by_mask(val, VSC9953_MAC_MACH_MASK);
 963                 macl = in_le32(&l2ana_reg->ana_tables.macl_data);
 964
 965                 printf("%02x:%02x:%02x:%02x:%02x:%02x ", (mach >> 8) & 0xff,
 966                        mach & 0xff, (macl >> 24) & 0xff, (macl >> 16) & 0xff,
 967                        (macl >> 8) & 0xff, macl & 0xff);
 968                 printf("%5d ", dest_indx);
 969                 printf("%4d\n", vlan);
 970         } while (1);
 971
 972         /* set learning mode to previous value */
 973         if (port_no == ETHSW_CMD_PORT_ALL) {
 974                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
 975                         if (!rc[i] && mode[i] != PORT_LEARN_NONE)
 976                                 vsc9953_port_learn_mode_set(i, mode[i]);
 977                 }
 978         } else {
 979                 /* If administrative down, skip */
 980                 if (!rc[port_no] && mode[port_no] != PORT_LEARN_NONE)
 981                         vsc9953_port_learn_mode_set(port_no, mode[port_no]);
 982         }
 983
 984         /* reset FDB port and VLAN FDB selection */
 985         clrbits_le32(&l2ana_reg->ana.anag_efil, VSC9953_AGE_PORT_EN |
 986                      VSC9953_AGE_PORT_MASK | VSC9953_AGE_VID_EN |
 987                      VSC9953_AGE_VID_MASK);
 988 }
 989
 990 /* Add a static FDB entry */
 991 static int vsc9953_mac_table_add(u8 port_no, uchar mac[6], int vid)
 992 {
 993         u32 val;
 994         struct vsc9953_analyzer *l2ana_reg;
 995
 996         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
 997                         VSC9953_ANA_OFFSET);
 998
 999         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1000         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1001               (mac[0] << 8) | (mac[1] << 0);
1002         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1003
1004         out_le32(&l2ana_reg->ana_tables.macl_data,
1005                  (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) |
1006                  (mac[5] << 0));
1007
1008         /* set on which port is the MAC address added */
1009         val = in_le32(&l2ana_reg->ana_tables.mac_access);
1010         val = bitfield_replace_by_mask(val, VSC9953_MAC_DESTIDX_MASK, port_no);
1011         out_le32(&l2ana_reg->ana_tables.mac_access, val);
1012
1013         if (vsc9953_mac_table_cmd(MAC_TABLE_LEARN) < 0)
1014                 return -1;
1015
1016         /* check if the MAC address was indeed added */
1017         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1018         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1019               (mac[0] << 8) | (mac[1] << 0);
1020         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1021
1022         out_le32(&l2ana_reg->ana_tables.macl_data,
1023                  (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) |
1024                  (mac[5] << 0));
1025
1026         if (vsc9953_mac_table_cmd(MAC_TABLE_READ) < 0)
1027                 return -1;
1028
1029         val = in_le32(&l2ana_reg->ana_tables.mac_access);
1030
1031         if ((port_no != bitfield_extract_by_mask(val,
1032                                                  VSC9953_MAC_DESTIDX_MASK))) {
1033                 printf("Failed to add MAC address\n");
1034                 return -1;
1035         }
1036         return 0;
1037 }
1038
1039 /* Delete a FDB entry */
1040 static int vsc9953_mac_table_del(uchar mac[6], u16 vid)
1041 {
1042         u32 val;
1043         struct vsc9953_analyzer *l2ana_reg;
1044
1045         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1046                         VSC9953_ANA_OFFSET);
1047
1048         /* check first if MAC entry is present */
1049         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1050         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1051               (mac[0] << 8) | (mac[1] << 0);
1052         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1053
1054         out_le32(&l2ana_reg->ana_tables.macl_data,
1055                  (mac[2] << 24) | (mac[3] << 16) | (mac[4] << 8) |
1056                  (mac[5] << 0));
1057
1058         if (vsc9953_mac_table_cmd(MAC_TABLE_LOOKUP) < 0) {
1059                 debug("Lookup in the MAC table failed\n");
1060                 return -1;
1061         }
1062
1063         if (!(in_le32(&l2ana_reg->ana_tables.mac_access) &
1064               VSC9953_MAC_CMD_VALID)) {
1065                 printf("The MAC address: %02x:%02x:%02x:%02x:%02x:%02x ",
1066                        mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
1067                 printf("VLAN: %d does not exist.\n", vid);
1068                 return -1;
1069         }
1070
1071         /* FDB entry found, proceed to delete */
1072         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1073         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1074               (mac[0] << 8) | (mac[1] << 0);
1075         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1076
1077         out_le32(&l2ana_reg->ana_tables.macl_data, (mac[2] << 24) |
1078                  (mac[3] << 16) | (mac[4] << 8) | (mac[5] << 0));
1079
1080         if (vsc9953_mac_table_cmd(MAC_TABLE_FORGET) < 0)
1081                 return -1;
1082
1083         /* check if the MAC entry is still in FDB */
1084         val = in_le32(&l2ana_reg->ana_tables.mach_data);
1085         val = bitfield_replace_by_mask(val, VSC9953_MACHDATA_VID_MASK, vid) |
1086               (mac[0] << 8) | (mac[1] << 0);
1087         out_le32(&l2ana_reg->ana_tables.mach_data, val);
1088
1089         out_le32(&l2ana_reg->ana_tables.macl_data, (mac[2] << 24) |
1090                  (mac[3] << 16) | (mac[4] << 8) | (mac[5] << 0));
1091
1092         if (vsc9953_mac_table_cmd(MAC_TABLE_LOOKUP) < 0) {
1093                 debug("Lookup in the MAC table failed\n");
1094                 return -1;
1095         }
1096         if (in_le32(&l2ana_reg->ana_tables.mac_access) &
1097             VSC9953_MAC_CMD_VALID) {
1098                 printf("Failed to delete MAC address\n");
1099                 return -1;
1100         }
1101
1102         return 0;
1103 }
1104
1105 /* age the unlocked entries in FDB */
1106 static void vsc9953_mac_table_age(int port_no, int vid)
1107 {
1108         int rc[VSC9953_MAX_PORTS];
1109         enum port_learn_mode mode[VSC9953_MAX_PORTS];
1110         u32 val;
1111         int i;
1112         struct vsc9953_analyzer *l2ana_reg;
1113
1114         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1115                         VSC9953_ANA_OFFSET);
1116
1117         /* set port and VID for selective aging */
1118         val = in_le32(&l2ana_reg->ana.anag_efil);
1119         if (port_no != ETHSW_CMD_PORT_ALL) {
1120                 /* disable auto learning */
1121                 rc[port_no] = vsc9953_port_learn_mode_get(port_no,
1122                                                           &mode[port_no]);
1123                 if (!rc[port_no] && mode[port_no] != PORT_LEARN_NONE)
1124                         vsc9953_port_learn_mode_set(port_no, PORT_LEARN_NONE);
1125
1126                 val = bitfield_replace_by_mask(val, VSC9953_AGE_PORT_MASK,
1127                                                port_no) | VSC9953_AGE_PORT_EN;
1128         } else {
1129                 /* disable auto learning on all ports */
1130                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1131                         rc[i] = vsc9953_port_learn_mode_get(i, &mode[i]);
1132                         if (!rc[i] && mode[i] != PORT_LEARN_NONE)
1133                                 vsc9953_port_learn_mode_set(i, PORT_LEARN_NONE);
1134                 }
1135         }
1136
1137         if (vid != ETHSW_CMD_VLAN_ALL) {
1138                 val = bitfield_replace_by_mask(val, VSC9953_AGE_VID_MASK, vid) |
1139                       VSC9953_AGE_VID_EN;
1140         }
1141         out_le32(&l2ana_reg->ana.anag_efil, val);
1142
1143         /* age the dynamic FDB entries */
1144         vsc9953_mac_table_cmd(MAC_TABLE_AGE);
1145
1146         /* clear previously set port and VID */
1147         clrbits_le32(&l2ana_reg->ana.anag_efil, VSC9953_AGE_PORT_EN |
1148                      VSC9953_AGE_PORT_MASK | VSC9953_AGE_VID_EN |
1149                      VSC9953_AGE_VID_MASK);
1150
1151         if (port_no != ETHSW_CMD_PORT_ALL) {
1152                 if (!rc[port_no] && mode[port_no] != PORT_LEARN_NONE)
1153                         vsc9953_port_learn_mode_set(port_no, mode[port_no]);
1154         } else {
1155                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1156                         if (!rc[i] && mode[i] != PORT_LEARN_NONE)
1157                                 vsc9953_port_learn_mode_set(i, mode[i]);
1158                 }
1159         }
1160 }
1161
1162 /* Delete all the dynamic FDB entries */
1163 static void vsc9953_mac_table_flush(int port, int vid)
1164 {
1165         vsc9953_mac_table_age(port, vid);
1166         vsc9953_mac_table_age(port, vid);
1167 }
1168
1169 static int vsc9953_port_status_key_func(struct ethsw_command_def *parsed_cmd)
1170 {
1171         int i;
1172         u8 enabled;
1173
1174         /* Last keyword should tell us if we should enable/disable the port */
1175         if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
1176             ethsw_id_enable)
1177                 enabled = 1;
1178         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
1179                  ethsw_id_disable)
1180                 enabled = 0;
1181         else
1182                 return CMD_RET_USAGE;
1183
1184         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1185                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1186                         printf("Invalid port number: %d\n", parsed_cmd->port);
1187                         return CMD_RET_FAILURE;
1188                 }
1189                 vsc9953_port_status_set(parsed_cmd->port, enabled);
1190         } else {
1191                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1192                         vsc9953_port_status_set(i, enabled);
1193         }
1194
1195         return CMD_RET_SUCCESS;
1196 }
1197
1198 static int vsc9953_port_config_key_func(struct ethsw_command_def *parsed_cmd)
1199 {
1200         int i;
1201
1202         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1203                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1204                         printf("Invalid port number: %d\n", parsed_cmd->port);
1205                         return CMD_RET_FAILURE;
1206                 }
1207                 vsc9953_phy_autoneg(parsed_cmd->port);
1208                 printf("%8s %8s %8s %8s %8s\n",
1209                        "Port", "Status", "Link", "Speed",
1210                        "Duplex");
1211                 vsc9953_port_config_show(parsed_cmd->port);
1212
1213         } else {
1214                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1215                         vsc9953_phy_autoneg(i);
1216                 printf("%8s %8s %8s %8s %8s\n",
1217                        "Port", "Status", "Link", "Speed", "Duplex");
1218                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1219                         vsc9953_port_config_show(i);
1220         }
1221
1222         return CMD_RET_SUCCESS;
1223 }
1224
1225 static int vsc9953_port_stats_key_func(struct ethsw_command_def *parsed_cmd)
1226 {
1227         int i;
1228
1229         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1230                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1231                         printf("Invalid port number: %d\n", parsed_cmd->port);
1232                         return CMD_RET_FAILURE;
1233                 }
1234                 vsc9953_port_statistics_show(parsed_cmd->port);
1235         } else {
1236                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1237                         vsc9953_port_statistics_show(i);
1238         }
1239
1240         return CMD_RET_SUCCESS;
1241 }
1242
1243 static int vsc9953_port_stats_clear_key_func(struct ethsw_command_def
1244                                              *parsed_cmd)
1245 {
1246         int i;
1247
1248         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1249                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1250                         printf("Invalid port number: %d\n", parsed_cmd->port);
1251                         return CMD_RET_FAILURE;
1252                 }
1253                 vsc9953_port_statistics_clear(parsed_cmd->port);
1254         } else {
1255                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1256                         vsc9953_port_statistics_clear(i);
1257         }
1258
1259         return CMD_RET_SUCCESS;
1260 }
1261
1262 static int vsc9953_learn_show_key_func(struct ethsw_command_def *parsed_cmd)
1263 {
1264         int i;
1265         enum port_learn_mode mode;
1266
1267         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1268                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1269                         printf("Invalid port number: %d\n", parsed_cmd->port);
1270                         return CMD_RET_FAILURE;
1271                 }
1272                 if (vsc9953_port_learn_mode_get(parsed_cmd->port, &mode))
1273                         return CMD_RET_FAILURE;
1274                 printf("%7s %11s\n", "Port", "Learn mode");
1275                 switch (mode) {
1276                 case PORT_LEARN_NONE:
1277                         printf("%7d %11s\n", parsed_cmd->port, "disable");
1278                         break;
1279                 case PORT_LEARN_AUTO:
1280                         printf("%7d %11s\n", parsed_cmd->port, "auto");
1281                         break;
1282                 default:
1283                         printf("%7d %11s\n", parsed_cmd->port, "-");
1284                 }
1285         } else {
1286                 printf("%7s %11s\n", "Port", "Learn mode");
1287                 for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1288                         if (vsc9953_port_learn_mode_get(i, &mode))
1289                                 continue;
1290                         switch (mode) {
1291                         case PORT_LEARN_NONE:
1292                                 printf("%7d %11s\n", i, "disable");
1293                                 break;
1294                         case PORT_LEARN_AUTO:
1295                                 printf("%7d %11s\n", i, "auto");
1296                                 break;
1297                         default:
1298                                 printf("%7d %11s\n", i, "-");
1299                         }
1300                 }
1301         }
1302
1303         return CMD_RET_SUCCESS;
1304 }
1305
1306 static int vsc9953_learn_set_key_func(struct ethsw_command_def *parsed_cmd)
1307 {
1308         int i;
1309         enum port_learn_mode mode;
1310
1311         /* Last keyword should tell us the learn mode */
1312         if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
1313             ethsw_id_auto)
1314                 mode = PORT_LEARN_AUTO;
1315         else if (parsed_cmd->cmd_to_keywords[parsed_cmd->cmd_keywords_nr - 1] ==
1316                  ethsw_id_disable)
1317                 mode = PORT_LEARN_NONE;
1318         else
1319                 return CMD_RET_USAGE;
1320
1321         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL) {
1322                 if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1323                         printf("Invalid port number: %d\n", parsed_cmd->port);
1324                         return CMD_RET_FAILURE;
1325                 }
1326                 vsc9953_port_learn_mode_set(parsed_cmd->port, mode);
1327         } else {
1328                 for (i = 0; i < VSC9953_MAX_PORTS; i++)
1329                         vsc9953_port_learn_mode_set(i, mode);
1330         }
1331
1332         return CMD_RET_SUCCESS;
1333 }
1334
1335 static int vsc9953_fdb_show_key_func(struct ethsw_command_def *parsed_cmd)
1336 {
1337         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL &&
1338             !VSC9953_PORT_CHECK(parsed_cmd->port)) {
1339                 printf("Invalid port number: %d\n", parsed_cmd->port);
1340                 return CMD_RET_FAILURE;
1341         }
1342
1343         if (parsed_cmd->vid != ETHSW_CMD_VLAN_ALL &&
1344             !VSC9953_VLAN_CHECK(parsed_cmd->vid)) {
1345                 printf("Invalid VID number: %d\n", parsed_cmd->vid);
1346                 return CMD_RET_FAILURE;
1347         }
1348
1349         vsc9953_mac_table_show(parsed_cmd->port, parsed_cmd->vid);
1350
1351         return CMD_RET_SUCCESS;
1352 }
1353
1354 static int vsc9953_fdb_flush_key_func(struct ethsw_command_def *parsed_cmd)
1355 {
1356         if (parsed_cmd->port != ETHSW_CMD_PORT_ALL &&
1357             !VSC9953_PORT_CHECK(parsed_cmd->port)) {
1358                 printf("Invalid port number: %d\n", parsed_cmd->port);
1359                 return CMD_RET_FAILURE;
1360         }
1361
1362         if (parsed_cmd->vid != ETHSW_CMD_VLAN_ALL &&
1363             !VSC9953_VLAN_CHECK(parsed_cmd->vid)) {
1364                 printf("Invalid VID number: %d\n", parsed_cmd->vid);
1365                 return CMD_RET_FAILURE;
1366         }
1367
1368         vsc9953_mac_table_flush(parsed_cmd->port, parsed_cmd->vid);
1369
1370         return CMD_RET_SUCCESS;
1371 }
1372
1373 static int vsc9953_fdb_entry_add_key_func(struct ethsw_command_def *parsed_cmd)
1374 {
1375         int vid;
1376
1377         /* a port number must be present */
1378         if (parsed_cmd->port == ETHSW_CMD_PORT_ALL) {
1379                 printf("Please specify a port\n");
1380                 return CMD_RET_FAILURE;
1381         }
1382
1383         if (!VSC9953_PORT_CHECK(parsed_cmd->port)) {
1384                 printf("Invalid port number: %d\n", parsed_cmd->port);
1385                 return CMD_RET_FAILURE;
1386         }
1387
1388         /* Use VLAN 1 if VID is not set */
1389         vid = (parsed_cmd->vid == ETHSW_CMD_VLAN_ALL ? 1 : parsed_cmd->vid);
1390
1391         if (!VSC9953_VLAN_CHECK(vid)) {
1392                 printf("Invalid VID number: %d\n", vid);
1393                 return CMD_RET_FAILURE;
1394         }
1395
1396         if (vsc9953_mac_table_add(parsed_cmd->port, parsed_cmd->ethaddr, vid))
1397                 return CMD_RET_FAILURE;
1398
1399         return CMD_RET_SUCCESS;
1400 }
1401
1402 static int vsc9953_fdb_entry_del_key_func(struct ethsw_command_def *parsed_cmd)
1403 {
1404         int vid;
1405
1406         /* Use VLAN 1 if VID is not set */
1407         vid = (parsed_cmd->vid == ETHSW_CMD_VLAN_ALL ? 1 : parsed_cmd->vid);
1408
1409         if (!VSC9953_VLAN_CHECK(vid)) {
1410                 printf("Invalid VID number: %d\n", vid);
1411                 return CMD_RET_FAILURE;
1412         }
1413
1414         if (vsc9953_mac_table_del(parsed_cmd->ethaddr, vid))
1415                 return CMD_RET_FAILURE;
1416
1417         return CMD_RET_SUCCESS;
1418 }
1419
1420 static struct ethsw_command_func vsc9953_cmd_func = {
1421                 .ethsw_name = "L2 Switch VSC9953",
1422                 .port_enable = &vsc9953_port_status_key_func,
1423                 .port_disable = &vsc9953_port_status_key_func,
1424                 .port_show = &vsc9953_port_config_key_func,
1425                 .port_stats = &vsc9953_port_stats_key_func,
1426                 .port_stats_clear = &vsc9953_port_stats_clear_key_func,
1427                 .port_learn = &vsc9953_learn_set_key_func,
1428                 .port_learn_show = &vsc9953_learn_show_key_func,
1429                 .fdb_show = &vsc9953_fdb_show_key_func,
1430                 .fdb_flush = &vsc9953_fdb_flush_key_func,
1431                 .fdb_entry_add = &vsc9953_fdb_entry_add_key_func,
1432                 .fdb_entry_del = &vsc9953_fdb_entry_del_key_func,
1433 };
1434
1435 #endif /* CONFIG_CMD_ETHSW */
1436
1437 /*****************************************************************************
1438 At startup, the default configuration would be:
1439         - HW learning enabled on all ports; (HW default)
1440         - All ports are in VLAN 1;
1441         - All ports are VLAN aware;
1442         - All ports have POP_COUNT 1;
1443         - All ports have PVID 1;
1444         - All ports have TPID 0x8100; (HW default)
1445         - All ports tag frames classified to all VLANs that are not PVID;
1446 *****************************************************************************/
1447 void vsc9953_default_configuration(void)
1448 {
1449         int i;
1450
1451         for (i = 0; i < VSC9953_MAX_VLAN; i++)
1452                 vsc9953_vlan_table_membership_all_set(i, 0);
1453         vsc9953_port_all_vlan_aware_set(1);
1454         vsc9953_port_all_vlan_pvid_set(1);
1455         vsc9953_port_all_vlan_poncnt_set(1);
1456         vsc9953_vlan_table_membership_all_set(1, 1);
1457         vsc9953_vlan_ingr_fltr_learn_drop(1);
1458         vsc9953_port_all_vlan_egress_untagged_set(EGRESS_UNTAG_PVID_AND_ZERO);
1459 }
1460
1461 void vsc9953_init(bd_t *bis)
1462 {
1463         u32 i;
1464         u32 hdx_cfg = 0;
1465         u32 phy_addr = 0;
1466         int timeout;
1467         struct vsc9953_system_reg *l2sys_reg;
1468         struct vsc9953_qsys_reg *l2qsys_reg;
1469         struct vsc9953_dev_gmii *l2dev_gmii_reg;
1470         struct vsc9953_analyzer *l2ana_reg;
1471         struct vsc9953_devcpu_gcb *l2dev_gcb;
1472
1473         l2dev_gmii_reg = (struct vsc9953_dev_gmii *)(VSC9953_OFFSET +
1474                         VSC9953_DEV_GMII_OFFSET);
1475
1476         l2ana_reg = (struct vsc9953_analyzer *)(VSC9953_OFFSET +
1477                         VSC9953_ANA_OFFSET);
1478
1479         l2sys_reg = (struct vsc9953_system_reg *)(VSC9953_OFFSET +
1480                         VSC9953_SYS_OFFSET);
1481
1482         l2qsys_reg = (struct vsc9953_qsys_reg *)(VSC9953_OFFSET +
1483                         VSC9953_QSYS_OFFSET);
1484
1485         l2dev_gcb = (struct vsc9953_devcpu_gcb *)(VSC9953_OFFSET +
1486                         VSC9953_DEVCPU_GCB);
1487
1488         out_le32(&l2dev_gcb->chip_regs.soft_rst,
1489                  VSC9953_SOFT_SWC_RST_ENA);
1490         timeout = 50000;
1491         while ((in_le32(&l2dev_gcb->chip_regs.soft_rst) &
1492                         VSC9953_SOFT_SWC_RST_ENA) && --timeout)
1493                 udelay(1); /* busy wait for vsc9953 soft reset */
1494         if (timeout == 0)
1495                 debug("Timeout waiting for VSC9953 to reset\n");
1496
1497         out_le32(&l2sys_reg->sys.reset_cfg, VSC9953_MEM_ENABLE |
1498                  VSC9953_MEM_INIT);
1499
1500         timeout = 50000;
1501         while ((in_le32(&l2sys_reg->sys.reset_cfg) &
1502                 VSC9953_MEM_INIT) && --timeout)
1503                 udelay(1); /* busy wait for vsc9953 memory init */
1504         if (timeout == 0)
1505                 debug("Timeout waiting for VSC9953 memory to initialize\n");
1506
1507         out_le32(&l2sys_reg->sys.reset_cfg, (in_le32(&l2sys_reg->sys.reset_cfg)
1508                         | VSC9953_CORE_ENABLE));
1509
1510         /* VSC9953 Setting to be done once only */
1511         out_le32(&l2qsys_reg->sys.ext_cpu_cfg, 0x00000b00);
1512
1513         for (i = 0; i < VSC9953_MAX_PORTS; i++) {
1514                 if (vsc9953_port_init(i))
1515                         printf("Failed to initialize l2switch port %d\n", i);
1516
1517                 /* Enable VSC9953 GMII Ports Port ID 0 - 7 */
1518                 if (VSC9953_INTERNAL_PORT_CHECK(i)) {
1519                         out_le32(&l2ana_reg->pfc[i].pfc_cfg,
1520                                  VSC9953_PFC_FC_QSGMII);
1521                         out_le32(&l2sys_reg->pause_cfg.mac_fc_cfg[i],
1522                                  VSC9953_MAC_FC_CFG_QSGMII);
1523                 } else {
1524                         out_le32(&l2ana_reg->pfc[i].pfc_cfg,
1525                                  VSC9953_PFC_FC);
1526                         out_le32(&l2sys_reg->pause_cfg.mac_fc_cfg[i],
1527                                  VSC9953_MAC_FC_CFG);
1528                 }
1529                 out_le32(&l2dev_gmii_reg->port_mode.clock_cfg,
1530                          VSC9953_CLOCK_CFG);
1531                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_ena_cfg,
1532                          VSC9953_MAC_ENA_CFG);
1533                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_mode_cfg,
1534                          VSC9953_MAC_MODE_CFG);
1535                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_ifg_cfg,
1536                          VSC9953_MAC_IFG_CFG);
1537                 /* mac_hdx_cfg varies with port id*/
1538                 hdx_cfg = VSC9953_MAC_HDX_CFG | (i << 16);
1539                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_hdx_cfg, hdx_cfg);
1540                 out_le32(&l2sys_reg->sys.front_port_mode[i],
1541                          VSC9953_FRONT_PORT_MODE);
1542                 setbits_le32(&l2qsys_reg->sys.switch_port_mode[i],
1543                              VSC9953_PORT_ENA);
1544                 out_le32(&l2dev_gmii_reg->mac_cfg_status.mac_maxlen_cfg,
1545                          VSC9953_MAC_MAX_LEN);
1546                 out_le32(&l2sys_reg->pause_cfg.pause_cfg[i],
1547                          VSC9953_PAUSE_CFG);
1548                 /* WAIT FOR 2 us*/
1549                 udelay(2);
1550
1551                 l2dev_gmii_reg = (struct vsc9953_dev_gmii *)(
1552                                 (char *)l2dev_gmii_reg
1553                                 + T1040_SWITCH_GMII_DEV_OFFSET);
1554
1555                 /* Initialize Lynx PHY Wrappers */
1556                 phy_addr = 0;
1557                 if (vsc9953_l2sw.port[i].enet_if ==
1558                                 PHY_INTERFACE_MODE_QSGMII)
1559                         phy_addr = (i + 0x4) & 0x1F;
1560                 else if (vsc9953_l2sw.port[i].enet_if ==
1561                                 PHY_INTERFACE_MODE_SGMII)
1562                         phy_addr = (i + 1) & 0x1F;
1563
1564                 if (phy_addr) {
1565                         /* SGMII IF mode + AN enable */
1566                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
1567                                            0x14, PHY_SGMII_IF_MODE_AN |
1568                                            PHY_SGMII_IF_MODE_SGMII);
1569                         /* Dev ability according to SGMII specification */
1570                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
1571                                            0x4, PHY_SGMII_DEV_ABILITY_SGMII);
1572                         /* Adjust link timer for SGMII
1573                          * 1.6 ms in units of 8 ns = 2 * 10^5 = 0x30d40
1574                          */
1575                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
1576                                            0x13, 0x0003);
1577                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
1578                                            0x12, 0x0d40);
1579                         /* Restart AN */
1580                         vsc9953_mdio_write(&l2dev_gcb->mii_mng[0], phy_addr,
1581                                            0x0, PHY_SGMII_CR_DEF_VAL |
1582                                            PHY_SGMII_CR_RESET_AN);
1583
1584                         timeout = 50000;
1585                         while ((vsc9953_mdio_read(&l2dev_gcb->mii_mng[0],
1586                                         phy_addr, 0x01) & 0x0020) && --timeout)
1587                                 udelay(1); /* wait for AN to complete */
1588                         if (timeout == 0)
1589                                 debug("Timeout waiting for AN to complete\n");
1590                 }
1591         }
1592
1593         vsc9953_default_configuration();
1594
1595 #ifdef CONFIG_CMD_ETHSW
1596         if (ethsw_define_functions(&vsc9953_cmd_func) < 0)
1597                 debug("Unable to use \"ethsw\" commands\n");
1598 #endif
1599
1600         printf("VSC9953 L2 switch initialized\n");
1601         return;
1602 }