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