release/src-rt/et/sys/etc.c

   1 /*
   2  * Common [OS-independent] portion of
   3  * Broadcom Home Networking Division 10/100 Mbit/s Ethernet
   4  * Device Driver.
   5  *
   6  * Copyright (C) 2010, Broadcom Corporation
   7  * All Rights Reserved.
   8  *
   9  * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Broadcom Corporation;
  10  * the contents of this file may not be disclosed to third parties, copied
  11  * or duplicated in any form, in whole or in part, without the prior
  12  * written permission of Broadcom Corporation.
  13  * $Id: etc.c,v 1.119.2.2 2010-07-01 23:25:01 Exp $
  14  */
  15
  16 #include <typedefs.h>
  17 #include <osl.h>
  18 #include <bcmendian.h>
  19 #include <proto/ethernet.h>
  20 #include <proto/vlan.h>
  21 #include <proto/bcmip.h>
  22 #include <proto/802.1d.h>
  23 #include <bcmenetmib.h>
  24 #include <bcmenetrxh.h>
  25 #include <bcmenetphy.h>
  26 #include <et_dbg.h>
  27 #include <etc.h>
  28 #include <et_export.h>
  29 #include <bcmutils.h>
  30
  31 #ifdef ETROBO
  32 #ifndef _siutils_h_
  33 typedef const struct si_pub  si_t;
  34 #endif
  35 #include <bcmrobo.h>
  36 #endif /* ETROBO */
  37
  38 uint32 et_msg_level =
  39 #ifdef BCMDBG
  40         1;
  41 #else
  42         0;
  43 #endif /* BCMDBG */
  44
  45 /* local prototypes */
  46 static void etc_loopback(etc_info_t *etc, int on);
  47 #ifdef BCMDBG
  48 static void etc_dumpetc(etc_info_t *etc, struct bcmstrbuf *b);
  49 #endif /* BCMDBG */
  50
  51 /* 802.1d priority to traffic class mapping. queues correspond one-to-one
  52  * with traffic classes.
  53  */
  54 uint32 up2tc[NUMPRIO] = {
  55         TC_BE,          /* 0    BE    TC_BE    Best Effort */
  56         TC_BK,          /* 1    BK    TC_BK    Background */
  57         TC_BK,          /* 2    --    TC_BK    Background */
  58         TC_BE,          /* 3    EE    TC_BE    Best Effort */
  59         TC_CL,          /* 4    CL    TC_CL    Controlled Load */
  60         TC_CL,          /* 5    VI    TC_CL    Controlled Load */
  61         TC_VO,          /* 6    VO    TC_VO    Voice */
  62         TC_VO           /* 7    NC    TC_VO    Voice */
  63 };
  64
  65 uint32 priq_selector[] = {
  66         [0x0] = TC_NONE, [0x1] = TC_BK, [0x2] = TC_BE, [0x3] = TC_BE,
  67         [0x4] = TC_CL,   [0x5] = TC_CL, [0x6] = TC_CL, [0x7] = TC_CL,
  68         [0x8] = TC_VO,   [0x9] = TC_VO, [0xa] = TC_VO, [0xb] = TC_VO,
  69         [0xc] = TC_VO,   [0xd] = TC_VO, [0xe] = TC_VO, [0xf] = TC_VO
  70 };
  71
  72 /* find the chip opsvec for this chip */
  73 struct chops*
  74 etc_chipmatch(uint vendor, uint device)
  75 {
  76 #if !defined(_CFE_) || defined(CFG_ETC47XX)
  77         {
  78                 extern struct chops bcm47xx_et_chops;
  79
  80                 if (bcm47xx_et_chops.id(vendor, device))
  81                         return (&bcm47xx_et_chops);
  82         }
  83 #endif
  84
  85 #ifdef CFG_GMAC
  86         {
  87                 extern struct chops bcmgmac_et_chops;
  88
  89                 if (bcmgmac_et_chops.id(vendor, device))
  90                         return (&bcmgmac_et_chops);
  91         }
  92 #endif /* CFG_GMAC */
  93         return (NULL);
  94 }
  95
  96 void*
  97 etc_attach(void *et, uint vendor, uint device, uint unit, void *osh, void *regsva)
  98 {
  99         etc_info_t *etc;
 100
 101         ET_TRACE(("et%d: etc_attach: vendor 0x%x device 0x%x\n", unit, vendor, device));
 102
 103         /* some code depends on packed structures */
 104         ASSERT(sizeof(struct ether_addr) == ETHER_ADDR_LEN);
 105         ASSERT(sizeof(struct ether_header) == ETHER_HDR_LEN);
 106
 107         /* allocate etc_info_t state structure */
 108         if ((etc = (etc_info_t*) MALLOC(osh, sizeof(etc_info_t))) == NULL) {
 109                 ET_ERROR(("et%d: etc_attach: out of memory, malloced %d bytes\n", unit,
 110                           MALLOCED(osh)));
 111                 return (NULL);
 112         }
 113         bzero((char*)etc, sizeof(etc_info_t));
 114
 115         etc->et = et;
 116         etc->unit = unit;
 117         etc->osh = osh;
 118         etc->vendorid = (uint16) vendor;
 119         etc->deviceid = (uint16) device;
 120         etc->forcespeed = ET_AUTO;
 121         etc->linkstate = FALSE;
 122
 123         /* set chip opsvec */
 124         etc->chops = etc_chipmatch(vendor, device);
 125         ASSERT(etc->chops);
 126
 127         /* chip attach */
 128         if ((etc->ch = (*etc->chops->attach)(etc, osh, regsva)) == NULL) {
 129                 ET_ERROR(("et%d: chipattach error\n", unit));
 130                 goto fail;
 131         }
 132
 133         return ((void*)etc);
 134
 135 fail:
 136         etc_detach(etc);
 137         return (NULL);
 138 }
 139
 140 void
 141 etc_detach(etc_info_t *etc)
 142 {
 143         if (etc == NULL)
 144                 return;
 145
 146         /* free chip private state */
 147         if (etc->ch) {
 148                 (*etc->chops->detach)(etc->ch);
 149                 etc->chops = etc->ch = NULL;
 150         }
 151
 152         MFREE(etc->osh, etc, sizeof(etc_info_t));
 153 }
 154
 155 void
 156 etc_reset(etc_info_t *etc)
 157 {
 158         ET_TRACE(("et%d: etc_reset\n", etc->unit));
 159
 160         etc->reset++;
 161
 162         /* reset the chip */
 163         (*etc->chops->reset)(etc->ch);
 164
 165         /* free any posted tx packets */
 166         (*etc->chops->txreclaim)(etc->ch, TRUE);
 167
 168 #ifdef DMA
 169         /* free any posted rx packets */
 170         (*etc->chops->rxreclaim)(etc->ch);
 171 #endif /* DMA */
 172 }
 173
 174 void
 175 etc_init(etc_info_t *etc, uint options)
 176 {
 177         ET_TRACE(("et%d: etc_init\n", etc->unit));
 178
 179         ASSERT(etc->pioactive == NULL);
 180         ASSERT(!ETHER_ISNULLADDR(&etc->cur_etheraddr));
 181         ASSERT(!ETHER_ISMULTI(&etc->cur_etheraddr));
 182
 183         /* init the chip */
 184         (*etc->chops->init)(etc->ch, options);
 185 }
 186
 187 /* mark interface up */
 188 void
 189 etc_up(etc_info_t *etc)
 190 {
 191         etc->up = TRUE;
 192
 193         et_init(etc->et, ET_INIT_DEF_OPTIONS);
 194 }
 195
 196 /* mark interface down */
 197 uint
 198 etc_down(etc_info_t *etc, int reset)
 199 {
 200         uint callback;
 201
 202         callback = 0;
 203
 204         ET_FLAG_DOWN(etc);
 205
 206         if (reset)
 207                 et_reset(etc->et);
 208
 209         /* suppress link state changes during power management mode changes */
 210         if (etc->linkstate) {
 211                 etc->linkstate = FALSE;
 212                 if (!etc->pm_modechange)
 213                         et_link_down(etc->et);
 214         }
 215
 216         return (callback);
 217 }
 218
 219 /* common iovar handler. return 0=ok, -1=error */
 220 int
 221 etc_iovar(etc_info_t *etc, uint cmd, uint set, void *arg)
 222 {
 223         int error;
 224 #if defined(ETROBO) && !defined(_CFE_)
 225         int i;
 226         uint *vecarg;
 227         robo_info_t *robo = etc->robo;
 228 #endif /* ETROBO && _CFE_ */
 229
 230         error = 0;
 231         ET_TRACE(("et%d: etc_iovar: cmd 0x%x\n", etc->unit, cmd));
 232
 233         switch (cmd) {
 234 #if defined(ETROBO) && !defined(_CFE_)
 235                 case IOV_ET_POWER_SAVE_MODE:
 236                         vecarg = (uint *)arg;
 237                         if (set)
 238                                 error = robo_power_save_mode_set(robo, vecarg[1], vecarg[0]);
 239                         else {
 240                                 /* get power save mode of all the phys */
 241                                 if (vecarg[0] == MAX_NO_PHYS) {
 242                                         for (i = 0; i < MAX_NO_PHYS; i++)
 243                                                 vecarg[i] = robo_power_save_mode_get(robo, i);
 244                                         break;
 245                                 }
 246
 247                                 /* get power save mode of the phy */
 248                                 error = robo_power_save_mode_get(robo, vecarg[0]);
 249                                 if (error != -1) {
 250                                         vecarg[1] = error;
 251                                         error = 0;
 252                                 }
 253                         }
 254                         break;
 255 #endif /* ETROBO && !_CFE_ */
 256 #ifdef BCMDBG
 257                 case IOV_ET_CLEAR_DUMP:
 258                         if (set) {
 259                                 uint size = ((char *)(&etc->rxbadlen) - (char *)(&etc->txframe));
 260
 261                                 bzero((char *)&etc->txframe, size + sizeof(etc->rxbadlen));
 262                                 (*etc->chops->dumpmib)(etc->ch, NULL, TRUE);
 263                                 error = 0;
 264                         }
 265                         break;
 266 #endif /* BCMDBG */
 267
 268                 default:
 269                         error = -1;
 270         }
 271
 272         return (error);
 273 }
 274
 275 /* common ioctl handler.  return: 0=ok, -1=error */
 276 int
 277 etc_ioctl(etc_info_t *etc, int cmd, void *arg)
 278 {
 279         int error;
 280         int val;
 281         int *vec = (int*)arg;
 282
 283         error = 0;
 284
 285         val = arg ? *(int*)arg : 0;
 286
 287         ET_TRACE(("et%d: etc_ioctl: cmd 0x%x\n", etc->unit, cmd));
 288
 289         switch (cmd) {
 290         case ETCUP:
 291                 et_up(etc->et);
 292                 break;
 293
 294         case ETCDOWN:
 295                 et_down(etc->et, TRUE);
 296                 break;
 297
 298         case ETCLOOP:
 299                 etc_loopback(etc, val);
 300                 break;
 301
 302         case ETCDUMP:
 303                 if (et_msg_level & 0x10000)
 304                         bcmdumplog((char *)arg, 4096);
 305 #ifdef BCMDBG
 306                 else
 307                 {
 308                         struct bcmstrbuf b;
 309                         bcm_binit(&b, (char*)arg, 4096);
 310                         et_dump(etc->et, &b);
 311                 }
 312 #endif /* BCMDBG */
 313                 break;
 314
 315         case ETCSETMSGLEVEL:
 316                 et_msg_level = val;
 317                 break;
 318
 319         case ETCPROMISC:
 320                 etc_promisc(etc, val);
 321                 break;
 322
 323         case ETCQOS:
 324                 etc_qos(etc, val);
 325                 break;
 326
 327         case ETCSPEED:
 328                 if (val == ET_1000FULL) {
 329                         etc->speed = 1000;
 330                         etc->duplex = 1;
 331                 } else if (val == ET_1000HALF) {
 332                         etc->speed = 1000;
 333                         etc->duplex = 0;
 334                 } else if (val == ET_100FULL) {
 335                         etc->speed = 100;
 336                         etc->duplex = 1;
 337                 } else if (val == ET_100HALF) {
 338                         etc->speed = 100;
 339                         etc->duplex = 0;
 340                 } else if (val == ET_10FULL) {
 341                         etc->speed = 10;
 342                         etc->duplex = 1;
 343                 } else if (val == ET_10HALF) {
 344                         etc->speed = 10;
 345                         etc->duplex = 0;
 346                 } else if (val == ET_AUTO)
 347                         ;
 348                 else
 349                         goto err;
 350
 351                 etc->forcespeed = val;
 352
 353                 /* explicitly reset the phy */
 354                 (*etc->chops->phyreset)(etc->ch, etc->phyaddr);
 355
 356                 /* request restart autonegotiation if we're reverting to adv mode */
 357                 if (etc->forcespeed == ET_AUTO) {
 358                         etc->advertise = (ADV_100FULL | ADV_100HALF |
 359                                           ADV_10FULL | ADV_10HALF);
 360                         etc->advertise2 = ADV_1000FULL;
 361                         etc->needautoneg = TRUE;
 362                 } else {
 363                         etc->advertise = etc->advertise2 = 0;
 364                         etc->needautoneg = FALSE;
 365                 }
 366
 367                 et_init(etc->et, ET_INIT_DEF_OPTIONS);
 368                 break;
 369
 370         case ETCPHYRD:
 371                 if (vec) {
 372                         vec[1] = (*etc->chops->phyrd)(etc->ch, etc->phyaddr, vec[0]);
 373                         ET_TRACE(("etc_ioctl: ETCPHYRD of reg 0x%x => 0x%x\n", vec[0], vec[1]));
 374                 }
 375                 break;
 376
 377         case ETCPHYRD2:
 378                 if (vec) {
 379                         uint phyaddr, reg;
 380                         phyaddr = vec[0] >> 16;
 381                         if (phyaddr < MAXEPHY) {
 382                                 reg = vec[0] & 0xffff;
 383                                 vec[1] = (*etc->chops->phyrd)(etc->ch, phyaddr, reg);
 384                                 ET_TRACE(("etc_ioctl: ETCPHYRD2 of phy 0x%x, reg 0x%x => 0x%x\n",
 385                                           phyaddr, reg, vec[1]));
 386                         }
 387                 }
 388                 break;
 389
 390         case ETCPHYWR:
 391                 if (vec) {
 392                         ET_TRACE(("etc_ioctl: ETCPHYWR to reg 0x%x <= 0x%x\n", vec[0], vec[1]));
 393                         (*etc->chops->phywr)(etc->ch, etc->phyaddr, vec[0], (uint16)vec[1]);
 394 #ifdef ETROBO
 395                         /* Invalidate current robo page */
 396                         if (etc->robo && etc->phyaddr == EPHY_NOREG && vec[0] == 0x10)
 397                                 ((robo_info_t *)etc->robo)->page = -1;
 398 #endif
 399                 }
 400                 break;
 401
 402         case ETCPHYWR2:
 403                 if (vec) {
 404                         uint phyaddr, reg;
 405                         phyaddr = vec[0] >> 16;
 406                         if (phyaddr < MAXEPHY) {
 407                                 reg = vec[0] & 0xffff;
 408                                 (*etc->chops->phywr)(etc->ch, phyaddr, reg, (uint16)vec[1]);
 409 #ifdef ETROBO
 410                                 /* Invalidate current robo page */
 411                                 if (etc->robo && phyaddr == EPHY_NOREG && reg == 0x10)
 412                                         ((robo_info_t *)etc->robo)->page = -1;
 413 #endif
 414                                 ET_TRACE(("etc_ioctl: ETCPHYWR2 to phy 0x%x, reg 0x%x <= 0x%x\n",
 415                                           phyaddr, reg, vec[1]));
 416                         }
 417                 }
 418                 break;
 419
 420 #ifdef ETROBO
 421         case ETCROBORD:
 422                 if (etc->robo && vec) {
 423                         uint page, reg;
 424                         uint16 val;
 425                         robo_info_t *robo = (robo_info_t *)etc->robo;
 426
 427                         page = vec[0] >> 16;
 428                         reg = vec[0] & 0xffff;
 429                         val = -1;
 430                         robo->ops->read_reg(etc->robo, page, reg, &val, 2);
 431                         vec[1] = val;
 432                         ET_TRACE(("etc_ioctl: ETCROBORD of page 0x%x, reg 0x%x => 0x%x\n",
 433                                   page, reg, val));
 434                 }
 435                 break;
 436
 437         case ETCROBOWR:
 438                 if (etc->robo && vec) {
 439                         uint page, reg;
 440                         uint16 val;
 441                         robo_info_t *robo = (robo_info_t *)etc->robo;
 442
 443                         page = vec[0] >> 16;
 444                         reg = vec[0] & 0xffff;
 445                         val = vec[1];
 446                         robo->ops->write_reg(etc->robo, page, vec[0], &val, 2);
 447                         ET_TRACE(("etc_ioctl: ETCROBOWR to page 0x%x, reg 0x%x <= 0x%x\n",
 448                                   page, reg, val));
 449                 }
 450                 break;
 451 #endif /* ETROBO */
 452
 453
 454         default:
 455         err:
 456                 error = -1;
 457         }
 458
 459         return (error);
 460 }
 461
 462 /* called once per second */
 463 void
 464 etc_watchdog(etc_info_t *etc)
 465 {
 466         uint16 status;
 467         uint16 lpa;
 468 #if defined(ETROBO) && !defined(_CFE_)
 469         robo_info_t *robo = (robo_info_t *)etc->robo;
 470         static uint32 sleep_timer = PWRSAVE_SLEEP_TIME, wake_timer;
 471 #endif /* ETROBO && !_CFE_ */
 472
 473         etc->now++;
 474
 475 #if defined(ETROBO) && !defined(_CFE_)
 476         /* Every PWRSAVE_WAKE_TIME sec the phys that are in manual mode
 477          * is taken out of that mode and link status is checked after
 478          * PWRSAVE_SLEEP_TIME sec to see if any of the links is up
 479          * to take that port is taken out of the manual power save mode
 480          */
 481         if (robo) {
 482                 if (ROBO_IS_PWRSAVE_MANUAL(robo)) {
 483                         if (etc->now == sleep_timer) {
 484                                 robo_power_save_toggle(robo, FALSE);
 485                                 wake_timer = sleep_timer + PWRSAVE_WAKE_TIME;
 486                         } else if (etc->now == wake_timer) {
 487                                 robo_power_save_toggle(robo, TRUE);
 488                                 sleep_timer = wake_timer + PWRSAVE_SLEEP_TIME;
 489                         }
 490                 }
 491
 492                 /* Apply the auto configuration from the nvram variable in the beginning */
 493                 if ((etc->now == PWRSAVE_WAKE_TIME) && ROBO_IS_PWRSAVE_AUTO(robo)) {
 494                         robo_power_save_mode_update(robo);
 495                 }
 496         }
 497 #endif /* ETROBO && !_CFE_ */
 498
 499         /* no local phy registers */
 500         if (etc->phyaddr == EPHY_NOREG) {
 501                 etc->linkstate = TRUE;
 502                 etc->duplex = 1;
 503                 /* keep emac txcontrol duplex bit consistent with current phy duplex */
 504                 (*etc->chops->duplexupd)(etc->ch);
 505                 return;
 506         }
 507
 508         status = (*etc->chops->phyrd)(etc->ch, etc->phyaddr, 1);
 509         /* check for bad mdio read */
 510         if (status == 0xffff) {
 511                 ET_ERROR(("et%d: etc_watchdog: bad mdio read: phyaddr %d mdcport %d\n",
 512                         etc->unit, etc->phyaddr, etc->mdcport));
 513                 return;
 514         }
 515
 516         if (etc->forcespeed == ET_AUTO) {
 517                 uint16 adv, adv2 = 0, status2 = 0, estatus;
 518
 519                 adv = (*etc->chops->phyrd)(etc->ch, etc->phyaddr, 4);
 520                 lpa = (*etc->chops->phyrd)(etc->ch, etc->phyaddr, 5);
 521
 522                 /* read extended status register. if we are 1000BASE-T
 523                  * capable then get our advertised capabilities and the
 524                  * link partner capabilities from 1000BASE-T control and
 525                  * status registers.
 526                  */
 527                 estatus = (*etc->chops->phyrd)(etc->ch, etc->phyaddr, 15);
 528                 if ((estatus != 0xffff) && (estatus & EST_1000TFULL)) {
 529                         /* read 1000BASE-T control and status registers */
 530                         adv2 = (*etc->chops->phyrd)(etc->ch, etc->phyaddr, 9);
 531                         status2 = (*etc->chops->phyrd)(etc->ch, etc->phyaddr, 10);
 532                 }
 533
 534                 /* update current speed and duplex */
 535                 if ((adv2 & ADV_1000FULL) && (status2 & LPA_1000FULL)) {
 536                         etc->speed = 1000;
 537                         etc->duplex = 1;
 538                 } else if ((adv2 & ADV_1000HALF) && (status2 & LPA_1000HALF)) {
 539                         etc->speed = 1000;
 540                         etc->duplex = 0;
 541                 } else if ((adv & ADV_100FULL) && (lpa & LPA_100FULL)) {
 542                         etc->speed = 100;
 543                         etc->duplex = 1;
 544                 } else if ((adv & ADV_100HALF) && (lpa & LPA_100HALF)) {
 545                         etc->speed = 100;
 546                         etc->duplex = 0;
 547                 } else if ((adv & ADV_10FULL) && (lpa & LPA_10FULL)) {
 548                         etc->speed = 10;
 549                         etc->duplex = 1;
 550                 } else {
 551                         etc->speed = 10;
 552                         etc->duplex = 0;
 553                 }
 554         }
 555
 556         /* monitor link state */
 557         if (!etc->linkstate && (status & STAT_LINK)) {
 558                 etc->linkstate = TRUE;
 559                 if (etc->pm_modechange)
 560                         etc->pm_modechange = FALSE;
 561                 else
 562                         et_link_up(etc->et);
 563         } else if (etc->linkstate && !(status & STAT_LINK)) {
 564                 etc->linkstate = FALSE;
 565                 if (!etc->pm_modechange)
 566                         et_link_down(etc->et);
 567         }
 568
 569         /* keep emac txcontrol duplex bit consistent with current phy duplex */
 570         (*etc->chops->duplexupd)(etc->ch);
 571
 572         /* check for remote fault error */
 573         if (status & STAT_REMFAULT) {
 574                 ET_ERROR(("et%d: remote fault\n", etc->unit));
 575         }
 576
 577         /* check for jabber error */
 578         if (status & STAT_JAB) {
 579                 ET_ERROR(("et%d: jabber\n", etc->unit));
 580         }
 581
 582         /*
 583          * Read chip mib counters occationally before the 16bit ones can wrap.
 584          * We don't use the high-rate mib counters.
 585          */
 586         if ((etc->now % 30) == 0)
 587                 (*etc->chops->statsupd)(etc->ch);
 588 }
 589
 590 static void
 591 etc_loopback(etc_info_t *etc, int on)
 592 {
 593         ET_TRACE(("et%d: etc_loopback: %d\n", etc->unit, on));
 594
 595         etc->loopbk = (bool) on;
 596         et_init(etc->et, ET_INIT_DEF_OPTIONS);
 597 }
 598
 599 void
 600 etc_promisc(etc_info_t *etc, uint on)
 601 {
 602         ET_TRACE(("et%d: etc_promisc: %d\n", etc->unit, on));
 603
 604         etc->promisc = (bool) on;
 605         et_init(etc->et, ET_INIT_DEF_OPTIONS);
 606 }
 607
 608 void
 609 etc_qos(etc_info_t *etc, uint on)
 610 {
 611         ET_TRACE(("et%d: etc_qos: %d\n", etc->unit, on));
 612
 613         etc->qos = (bool) on;
 614         et_init(etc->et, ET_INIT_DEF_OPTIONS);
 615 }
 616
 617 /* WAR: BCM53115 switch is not retaining the tag while forwarding
 618  * the vlan/priority tagged frames even when tag status preserve
 619  * is enabled. This problem can be only worked around by doing
 620  * double tagging for priority tagged frames. This will trick the
 621  * switch in to just removing the outer tag on the egress. Inner
 622  * tag remains which contains the prio.
 623  */
 624 #ifdef ETROBO
 625 void *
 626 etc_bcm53115_war(etc_info_t *etc, void *p)
 627 {
 628         struct ethervlan_header *evh;
 629         uint16 vlan_tag;
 630         int vlan_prio;
 631         uint8 *data = PKTDATA(etc->osh, p);
 632         uint8 *ip_body = data + sizeof(struct ethervlan_header);
 633
 634         evh = (struct ethervlan_header *)data;
 635         /* No additional TAG added if IPTOS has priority != 0 */
 636         if ((evh->vlan_type != hton16(ETHER_TYPE_8021Q)) ||
 637             (IP_TOS46(ip_body) & IPV4_TOS_PREC_MASK))
 638                 return (p);
 639
 640         vlan_tag = evh->vlan_tag;
 641         vlan_prio = vlan_tag & hton16(VLAN_PRI_MASK << VLAN_PRI_SHIFT);
 642
 643         /* No need to do anything for priority 0 */
 644         if (vlan_prio == 0)
 645                 return (p);
 646
 647         /* If the packet is shared or there is not enough headroom
 648          * then allocate new header buffer and link the original
 649          * buffer to it.
 650          */
 651         if ((PKTHEADROOM(etc->osh, p) < VLAN_TAG_LEN) || PKTSHARED(p)) {
 652                 void *pkt;
 653                 uint16 ether_type;
 654
 655                 if ((pkt = PKTGET(etc->osh, VLAN_TAG_LEN +
 656                                   ETHERVLAN_HDR_LEN, TRUE)) == NULL) {
 657                         ET_ERROR(("et%d: PKTGET of size %d failed during expand head\n",
 658                                   etc->unit, VLAN_TAG_LEN + ETHERVLAN_HDR_LEN));
 659                         return (NULL);
 660                 }
 661
 662                 /* Assign priority of original frame */
 663                 PKTSETPRIO(pkt, ntoh16(vlan_prio) >> VLAN_PRI_SHIFT);
 664
 665                 ether_type = evh->ether_type;
 666
 667                 /* Copy the vlan header to the first buffer */
 668                 memcpy(PKTDATA(etc->osh, pkt), data, ETHERVLAN_HDR_LEN);
 669                 PKTPULL(etc->osh, p, ETHERVLAN_HDR_LEN);
 670
 671                 /* Align the pointer to initialize the inner vlan tag and type
 672                  * fields.
 673                  */
 674                 evh = (struct ethervlan_header *)(PKTDATA(etc->osh, pkt) + VLAN_TAG_LEN);
 675                 evh->vlan_tag = vlan_tag;
 676                 evh->ether_type = ether_type;
 677
 678                 /* Chain the original buffer to new header buffer */
 679                 PKTSETNEXT(etc->osh, pkt, p);
 680
 681                 p = pkt;
 682         } else {
 683                 data = PKTPUSH(etc->osh, p, VLAN_TAG_LEN);
 684                 ETHERVLAN_MOVE_HDR(data, data + VLAN_TAG_LEN);
 685                 evh = (struct ethervlan_header *)(data + VLAN_TAG_LEN);
 686         }
 687
 688         evh->vlan_type = hton16(ETHER_TYPE_8021Q);
 689
 690         /* Clear the vlan id in the inner tag */
 691         evh->vlan_tag &= ~(hton16(VLAN_VID_MASK));
 692
 693         return (p);
 694 }
 695 #endif /* ETROBO */
 696
 697 #ifdef BCMDBG
 698 void
 699 etc_dump(etc_info_t *etc, struct bcmstrbuf *b)
 700 {
 701         etc_dumpetc(etc, b);
 702         (*etc->chops->dump)(etc->ch, b);
 703 }
 704
 705 static void
 706 etc_dumpetc(etc_info_t *etc, struct bcmstrbuf *b)
 707 {
 708         char perm[32], cur[32];
 709         uint i;
 710
 711         bcm_bprintf(b, "etc 0x%x et 0x%x unit %d msglevel %d speed/duplex %d%s\n",
 712                 (ulong)etc, (ulong)etc->et, etc->unit, et_msg_level,
 713                 etc->speed, (etc->duplex ? "full": "half"));
 714         bcm_bprintf(b, "up %d promisc %d loopbk %d forcespeed %d advertise 0x%x "
 715                        "advertise2 0x%x needautoneg %d\n",
 716                        etc->up, etc->promisc, etc->loopbk, etc->forcespeed,
 717                        etc->advertise, etc->advertise2, etc->needautoneg);
 718         bcm_bprintf(b, "piomode %d pioactive 0x%x nmulticast %d allmulti %d qos %d\n",
 719                 etc->piomode, (ulong)etc->pioactive, etc->nmulticast, etc->allmulti, etc->qos);
 720         bcm_bprintf(b, "vendor 0x%x device 0x%x rev %d coreunit %d phyaddr %d mdcport %d\n",
 721                 etc->vendorid, etc->deviceid, etc->chiprev,
 722                 etc->coreunit, etc->phyaddr, etc->mdcport);
 723
 724         bcm_bprintf(b, "perm_etheraddr %s cur_etheraddr %s\n",
 725                 bcm_ether_ntoa(&etc->perm_etheraddr, perm),
 726                 bcm_ether_ntoa(&etc->cur_etheraddr, cur));
 727
 728         if (etc->nmulticast) {
 729                 bcm_bprintf(b, "multicast: ");
 730                 for (i = 0; i < etc->nmulticast; i++)
 731                         bcm_bprintf(b, "%s ", bcm_ether_ntoa(&etc->multicast[i], cur));
 732                 bcm_bprintf(b, "\n");
 733         }
 734
 735         bcm_bprintf(b, "linkstate %d\n", etc->linkstate);
 736         bcm_bprintf(b, "\n");
 737
 738         /* refresh stat counters */
 739         (*etc->chops->statsupd)(etc->ch);
 740
 741         /* summary stat counter line */
 742         /* use sw frame and byte counters -- hw mib counters wrap too quickly */
 743         bcm_bprintf(b, "txframe %d txbyte %d txerror %d rxframe %d rxbyte %d rxerror %d\n",
 744                 etc->txframe, etc->txbyte, etc->txerror,
 745                 etc->rxframe, etc->rxbyte, etc->rxerror);
 746
 747         /* transmit & receive stat counters */
 748         /* hardware mib pkt and octet counters wrap too quickly to be useful */
 749         (*etc->chops->dumpmib)(etc->ch, b, FALSE);
 750
 751         bcm_bprintf(b, "txnobuf %d reset %d dmade %d dmada %d dmape %d\n",
 752                        etc->txnobuf, etc->reset, etc->dmade, etc->dmada, etc->dmape);
 753
 754         /* hardware mib pkt and octet counters wrap too quickly to be useful */
 755         bcm_bprintf(b, "rxnobuf %d rxdmauflo %d rxoflo %d rxbadlen %d "
 756                        "rxgiants %d rxoflodiscards %d\n",
 757                        etc->rxnobuf, etc->rxdmauflo, etc->rxoflo, etc->rxbadlen,
 758                        etc->rxgiants, etc->rxoflodiscards);
 759
 760         bcm_bprintf(b, "\n");
 761 }
 762 #endif /* BCMDBG */
 763
 764 uint
 765 etc_totlen(etc_info_t *etc, void *p)
 766 {
 767         uint total;
 768
 769         total = 0;
 770         for (; p; p = PKTNEXT(etc->osh, p))
 771                 total += PKTLEN(etc->osh, p);
 772         return (total);
 773 }
 774
 775 #ifdef BCMDBG
 776 void
 777 etc_prhdr(char *msg, struct ether_header *eh, uint len, int unit)
 778 {
 779         char da[32], sa[32];
 780
 781         if (msg && (msg[0] != '\0'))
 782                 printf("et%d: %s: ", unit, msg);
 783         else
 784                 printf("et%d: ", unit);
 785
 786         printf("dst %s src %s type 0x%x len %d\n",
 787                 bcm_ether_ntoa((struct ether_addr *)eh->ether_dhost, da),
 788                 bcm_ether_ntoa((struct ether_addr *)eh->ether_shost, sa),
 789                 ntoh16(eh->ether_type),
 790                 len);
 791 }
 792 void
 793 etc_prhex(char *msg, uchar *buf, uint nbytes, int unit)
 794 {
 795         if (msg && (msg[0] != '\0'))
 796                 printf("et%d: %s:\n", unit, msg);
 797         else
 798                 printf("et%d:\n", unit);
 799
 800         prhex(NULL, buf, nbytes);
 801 }
 802 #endif /* BCMDBG */