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