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