package/linux/kernel-patches/027-drivers_net_port_based_qos

   1 diff -Nur linux-mips-cvs/drivers/net/port_based_qos/Atan.c linux-broadcom/drivers/net/port_based_qos/Atan.c
   2 --- linux-mips-cvs/drivers/net/port_based_qos/Atan.c    1970-01-01 01:00:00.000000000 +0100
   3 +++ linux-broadcom/drivers/net/port_based_qos/Atan.c    2005-01-31 13:13:14.000000000 +0100
   4 @@ -0,0 +1,177 @@
   5 +#include "Atan.h"
   6 +#include "c47xx.h"
   7 +
   8 +extern void conf_gpio(int x);
   9 +
  10 +/*----------------------------------------------------------------------------
  11 +Write specified data to eeprom
  12 +entry: *src    = pointer to specified data to write
  13 +               len             = number of short(2 bytes) to be written
  14 +*/
  15 +void write_eeprom( short RegNumber, unsigned short *data, int len )
  16 +{
  17 +    int i2;
  18 +    unsigned short s_addr, s_data;
  19 +    unsigned short *src;
  20 +
  21 +    src = data;
  22 +    SetEEpromToSendState();
  23 +//  the write enable(WEN) instruction must be executed before any device
  24 +//  programming can be done
  25 +
  26 +    s_data = 0x04c0;
  27 +    SendAddrToEEprom(s_data); //00000001 0011000000B
  28 +    SetCSToLowForEEprom();
  29 +
  30 +    s_addr = 0x0500 | (RegNumber & 0x0ff); //00000001 01dddddddd
  31 +    s_data = *src;
  32 +
  33 +    for (i2 = len; i2 > 0 ; i2 --)
  34 +    {
  35 +      SendAddrToEEprom(s_addr); //00000001 01dddddd
  36 +      SendDataToEEprom(s_data); //dddddddd dddddddd
  37 +      SetCSToLowForEEprom();
  38 +      SetCSToLowForEEprom();
  39 +      //WriteWait();
  40 +      s_addr ++;
  41 +      src ++;
  42 +      s_data = *src;
  43 +    }
  44 +//  after all data has been written to EEprom , the write disable(WDS)
  45 +//  instruction must be executed
  46 +    SetCSToHighForEEprom();
  47 +    s_data = 0x0400;
  48 +    SendAddrToEEprom(s_data); //00000001 00000000B
  49 +    SetCSToLowForEEprom();
  50 +    SetCSToLowForEEprom();
  51 +}
  52 +
  53 +void SetEEpromToSendState()
  54 +{
  55 +  conf_gpio(0x0);
  56 +  conf_gpio(0x0);
  57 +  conf_gpio(B_ECS);
  58 +  conf_gpio(B_ECS);
  59 +
  60 +//  ;cs     __--   ,bit 2
  61 +//  ;sck    ____   ,bit 3
  62 +//  ;di     ____   ,bit 5
  63 +//  ;do     ____   ,bit 4
  64 +//  ;
  65 +}
  66 +
  67 +void ResetEEpromToSendState()
  68 +{
  69 +  conf_gpio(0x0);
  70 +  conf_gpio(0x0);
  71 +  conf_gpio(0x0);
  72 +  conf_gpio(0x0);
  73 +
  74 +//  ;cs     ____   ,bit 2
  75 +//  ;sck    ____   ,bit 3
  76 +//  ;di     ____   ,bit 5
  77 +//  ;do     ____   ,bit 4
  78 +//  ;
  79 +}
  80 +
  81 +void SetCSToLowForEEprom()
  82 +{
  83 +  conf_gpio(0x0);
  84 +  conf_gpio(B_ECK);
  85 +  conf_gpio(B_ECK);
  86 +  conf_gpio(0x0);
  87 +
  88 +//  ;cs     ____   ,bit 2
  89 +//  ;sck    _--_   ,bit 3
  90 +//  ;di     ____   ,bit 5
  91 +//  ;do     ____   ,bit 4
  92 +//  ;
  93 +}
  94 +
  95 +void SetCSToHighForEEprom()
  96 +{
  97 +  conf_gpio(B_ECS);
  98 +  conf_gpio(B_ECS|B_ECK);
  99 +  conf_gpio(B_ECS|B_ECK);
 100 +  conf_gpio(B_ECS);
 101 +
 102 +//  ;cs     ----   ,bit 2
 103 +//  ;sck    _--_   ,bit 3
 104 +//  ;di     ____   ,bit 5
 105 +//  ;do     ____   ,bit 4
 106 +//  ;
 107 +}
 108 +
 109 +void send_1ToEEprom()
 110 +{
 111 +  conf_gpio(B_ECS|B_EDI);
 112 +  conf_gpio(B_ECS|B_ECK|B_EDI);
 113 +  conf_gpio(B_ECS|B_ECK|B_EDI);
 114 +  conf_gpio(B_ECS|B_EDI);
 115 +
 116 +//  ;cs     ----   ,bit 2
 117 +//  ;sck    _--_   ,bit 3
 118 +//  ;di     ----   ,bit 5
 119 +//  ;do     ____   ,bit 4
 120 +//  ;
 121 +}
 122 +
 123 +void send_0ToEEprom()
 124 +{
 125 +  conf_gpio(B_ECS);
 126 +  conf_gpio(B_ECS|B_ECK);
 127 +  conf_gpio(B_ECS|B_ECK);
 128 +  conf_gpio(B_ECS);
 129 +
 130 +//  ;cs     ----   ,bit 2
 131 +//  ;sck    _--_   ,bit 3
 132 +//  ;di     ____   ,bit 5
 133 +//  ;do     ____   ,bit 4
 134 +//  ;
 135 +}
 136 +
 137 +#if 0
 138 +void WriteWait()
 139 +{
 140 +  unsigned int status;
 141 +
 142 +  SetCSToLowForEEprom();
 143 +  SetCSToHighForEEprom();
 144 +  do {
 145 +       SetCSToHighForEEprom();
 146 +       //status = ReadGPIOData(EDO);
 147 +       status = gpio & B_EDO;  // read EDO bit
 148 +  }
 149 +  while (!status);   // wait for write - ready
 150 +  SetCSToLowForEEprom();
 151 +}
 152 +#endif
 153 +
 154 +void SendDataToEEprom(short s_data)
 155 +{
 156 +  int data_mask;
 157 +
 158 +  for (data_mask = 0x8000; data_mask != 0; )
 159 +  {
 160 +    if (s_data & data_mask)
 161 +      send_1ToEEprom();
 162 +    else
 163 +      send_0ToEEprom();
 164 +    data_mask = data_mask >> 1;
 165 +  }
 166 +}
 167 +
 168 +void SendAddrToEEprom(short s_data)
 169 +{
 170 +  int data_mask;
 171 +
 172 +  for (data_mask = 0x0400 ;data_mask != 0; )
 173 +  {
 174 +    if (s_data & data_mask)
 175 +      send_1ToEEprom();
 176 +    else
 177 +      send_0ToEEprom();
 178 +    data_mask = data_mask >> 1;
 179 +  }
 180 +}
 181 +
 182 diff -Nur linux-mips-cvs/drivers/net/port_based_qos/Atan.h linux-broadcom/drivers/net/port_based_qos/Atan.h
 183 --- linux-mips-cvs/drivers/net/port_based_qos/Atan.h    1970-01-01 01:00:00.000000000 +0100
 184 +++ linux-broadcom/drivers/net/port_based_qos/Atan.h    2005-01-31 13:13:14.000000000 +0100
 185 @@ -0,0 +1,18 @@
 186 +#ifndef _ATAN_H_
 187 +#define _ATAN_H_
 188 +
 189 +#define SINGLECOLOUR                           0x1
 190 +#define DUALCOLOUR                                     0x2
 191 +
 192 +void write_eeprom(short,unsigned short *,int);
 193 +void SetEEpromToSendState(void);
 194 +void ResetEEpromToSendState(void);
 195 +void SetCSToLowForEEprom(void);
 196 +void SetCSToHighForEEprom(void);
 197 +void send_1ToEEprom(void);
 198 +void send_0ToEEprom(void);
 199 +//void WriteWait(void);
 200 +void SendAddrToEEprom(short);
 201 +void SendDataToEEprom(short);
 202 +
 203 +#endif
 204 diff -Nur linux-mips-cvs/drivers/net/port_based_qos/Makefile linux-broadcom/drivers/net/port_based_qos/Makefile
 205 --- linux-mips-cvs/drivers/net/port_based_qos/Makefile  1970-01-01 01:00:00.000000000 +0100
 206 +++ linux-broadcom/drivers/net/port_based_qos/Makefile  2005-01-31 13:13:14.000000000 +0100
 207 @@ -0,0 +1,27 @@
 208 +#  Copyright 2001, Cybertan Corporation
 209 +#  All Rights Reserved.
 210 +#
 211 +#  This is UNPUBLISHED PROPRIETARY SOURCE CODE of Cybertan Corporation;
 212 +#  the contents of this file may not be disclosed to third parties, copied or
 213 +#  duplicated in any form, in whole or in part, without the prior written
 214 +#  permission of Cybertan Corporation.
 215 +#
 216 +#
 217 +# $Id$
 218 +#
 219 +
 220 +O_TARGET       := port_based_qos_mod.o
 221 +
 222 +PORT_BASED_QOS_MOD_OBJS        := port_based_qos.o Atan.o c47xx.o eeprom.o
 223 +
 224 +export-objs    :=
 225 +obj-y          := $(PORT_BASED_QOS_MOD_OBJS)
 226 +obj-m          := $(O_TARGET)
 227 +
 228 +SRCBASE                := $(TOPDIR)/../..
 229 +EXTRA_CFLAGS   += -I$(SRCBASE)/include -Wall -I$(SRCBASE)/
 230 +
 231 +vpath %.c $(SRCBASE)/shared
 232 +
 233 +include $(TOPDIR)/Rules.make
 234 +
 235 diff -Nur linux-mips-cvs/drivers/net/port_based_qos/c47xx.c linux-broadcom/drivers/net/port_based_qos/c47xx.c
 236 --- linux-mips-cvs/drivers/net/port_based_qos/c47xx.c   1970-01-01 01:00:00.000000000 +0100
 237 +++ linux-broadcom/drivers/net/port_based_qos/c47xx.c   2005-01-31 13:13:14.000000000 +0100
 238 @@ -0,0 +1,85 @@
 239 +#include <typedefs.h>
 240 +#include <sbutils.h>
 241 +#include <bcmdevs.h>
 242 +#include <osl.h>
 243 +#include <bcmnvram.h>
 244 +#include <bcmutils.h>
 245 +
 246 +#include <sbpci.h>
 247 +#include <sbchipc.h>
 248 +#include <sbconfig.h>
 249 +#include <sbextif.h>
 250 +#include <sbmips.h>
 251 +#include "c47xx.h"
 252 +extern uint32 sb_gpioouten(void *sbh, uint32 mask, uint32 val);
 253 +extern uint32 sb_gpioout(void *sbh, uint32 mask, uint32 val);
 254 +extern uint32 sb_gpioin(void *sbh);
 255 +extern uint32 sb_gpiointmask(void *sbh, uint32 mask, uint32 val);
 256 +
 257 +#define OUTENMASK      B_RESET|B_ECS|B_ECK|B_EDI
 258 +#define CFGMASK                B_ECS|B_ECK|B_EDI
 259 +#define BIT(x) (1 << (x))
 260 +#define        ASSERT(exp)             do {} while (0)
 261 +
 262 +void
 263 +conf_gpio(int x)
 264 +{
 265 +       ASSERT(sbh);
 266 +
 267 +       /* Enable all of output pins */
 268 +       sb_gpioouten(sbh, OUTENMASK, OUTENMASK);
 269 +
 270 +       /* We don't want the B_RESET pin changed, unless
 271 +        * it tries to set the B_RESET pin.
 272 +        */
 273 +       if (x & B_RESET)
 274 +               sb_gpioout(sbh, OUTENMASK, x);
 275 +       else
 276 +               sb_gpioout(sbh, CFGMASK, x);
 277 +
 278 +}
 279 +
 280 +void
 281 +gpio_line_set(int x, unsigned int value)
 282 +{
 283 +       ASSERT(sbh);
 284 +
 285 +       if (value == 1)
 286 +               sb_gpioout(sbh, BIT(x), BIT(x));
 287 +       else if (value == 0)
 288 +               sb_gpioout(sbh, BIT(x), 0);
 289 +}
 290 +
 291 +void
 292 +gpio_line_get(int x, int *value)
 293 +{
 294 +       ASSERT(sbh);
 295 +
 296 +       *value = (sb_gpioin(sbh) >> x) & 0x1;
 297 +}
 298 +
 299 +void
 300 +gpio_line_config_in(int x)
 301 +{
 302 +       ASSERT(sbh);
 303 +
 304 +       sb_gpioouten(sbh, BIT(x), 0);
 305 +       sb_gpiointmask(sbh, BIT(x), BIT(x));
 306 +}
 307 +
 308 +void
 309 +gpio_line_config_out(int x)
 310 +{
 311 +       ASSERT(sbh);
 312 +
 313 +       sb_gpiointmask(sbh, BIT(x), 0);
 314 +       sb_gpioouten(sbh, BIT(x), BIT(x));
 315 +}
 316 +
 317 +void
 318 +gpio_line_config_out_all(int x)
 319 +{
 320 +       ASSERT(sbh);
 321 +
 322 +       sb_gpioouten(sbh, OUTENMASK, OUTENMASK);
 323 +}
 324 diff -Nur linux-mips-cvs/drivers/net/port_based_qos/c47xx.h linux-broadcom/drivers/net/port_based_qos/c47xx.h
 325 --- linux-mips-cvs/drivers/net/port_based_qos/c47xx.h   1970-01-01 01:00:00.000000000 +0100
 326 +++ linux-broadcom/drivers/net/port_based_qos/c47xx.h   2005-01-31 13:13:14.000000000 +0100
 327 @@ -0,0 +1,23 @@
 328 +#ifndef _C47XX_H_
 329 +#define _C47XX_H_
 330 +
 331 +extern void    *bcm947xx_sbh;
 332 +#define sbh bcm947xx_sbh
 333 +
 334 +#define GPIO0  0
 335 +#define GPIO1  1
 336 +#define GPIO2  2
 337 +#define GPIO3  3
 338 +#define GPIO4  4
 339 +#define GPIO5  5
 340 +#define GPIO6  6
 341 +#define GPIO7  7
 342 +#define GPIO8  8
 343 +
 344 +#define B_RESET                1<<GPIO0
 345 +#define B_ECS          1<<GPIO2
 346 +#define B_ECK          1<<GPIO3
 347 +#define B_EDO          1<<GPIO4
 348 +#define B_EDI          1<<GPIO5
 349 +
 350 +#endif
 351 diff -Nur linux-mips-cvs/drivers/net/port_based_qos/eeprom.c linux-broadcom/drivers/net/port_based_qos/eeprom.c
 352 --- linux-mips-cvs/drivers/net/port_based_qos/eeprom.c  1970-01-01 01:00:00.000000000 +0100
 353 +++ linux-broadcom/drivers/net/port_based_qos/eeprom.c  2005-01-31 13:13:14.000000000 +0100
 354 @@ -0,0 +1,355 @@
 355 +#include <linux/delay.h>
 356 +
 357 +#define EEDO_PIN       4
 358 +#define EECS_PIN       2
 359 +#define EECK_PIN       3
 360 +#define EEDI_PIN       5
 361 +#define RESET_PIN      0
 362 +
 363 +static void SetCSToLowForEEprom(void);
 364 +//static void SetCSToHighForEEprom(void);
 365 +static void send1ToEEprom(void);
 366 +static void send0ToEEprom(void);
 367 +static void InitSerialInterface(void);
 368 +static void SerialPulse(void);
 369 +static void WriteDataToRegister(unsigned short RegNumber, unsigned short data);
 370 +void ReadDataFromRegister(unsigned short addr, unsigned short *hidata, unsigned short *lodata,int select_count);
 371 +static void WriteDataToEEprom(unsigned short addr, unsigned short data);
 372 +static void WriteCmdToEEprom(unsigned short cmd);
 373 +extern void gpio_line_set(int x, unsigned int value);
 374 +extern void gpio_line_get(int x, int *value);
 375 +extern void gpio_line_config_out(int x);
 376 +extern void gpio_line_config_in(int x);
 377 +extern void gpio_line_config_out_all(void);
 378 +//  ;cs     __--   ,bit 3
 379 +//  ;sck    ____   ,bit 4
 380 +//  ;di     ____   ,bit 5
 381 +//  ;do     ____   ,bit 6
 382 +//  ;
 383 +
 384 +static void SetEEpromToSendState(void)
 385 +{
 386 +       gpio_line_set(EECS_PIN, 0);
 387 +        gpio_line_set(EECK_PIN, 0);
 388 +       gpio_line_set(EEDI_PIN, 1);
 389 +//     gpio_line_set(EEDO_PIN, 1);    /* high impedance */
 390 +
 391 +        mdelay(1);
 392 +        gpio_line_set(EECS_PIN, 1);
 393 +//     gpio_line_set(EEDO_PIN, 1);    /* high impedance */
 394 +}
 395 +
 396 +#if 0
 397 +static void EEpromInit(void)
 398 +{
 399 +       gpio_line_set(EECS_PIN, 0);
 400 +        gpio_line_set(EECK_PIN, 0);
 401 +       gpio_line_set(EEDI_PIN, 1);
 402 +       gpio_line_set(EEDO_PIN, 1);    /* high impedance */
 403 +
 404 +        mdelay(1);
 405 +}
 406 +
 407 +//  ;cs     ____   ,bit 3
 408 +//  ;sck    ____   ,bit 4
 409 +//  ;di     ____   ,bit 5
 410 +//  ;do     ____   ,bit 6
 411 +//  ;
 412 +static void ResetEEpromToSendState(void)
 413 +{
 414 +       gpio_line_set(EECS_PIN, 0);
 415 +       gpio_line_set(EEDI_PIN, 0);
 416 +       //gpio_line_set(EEDO_PIN, 0);
 417 +       gpio_line_set(EECK_PIN, 0);
 418 +}
 419 +#endif /* 0 */
 420 +
 421 +//  ;cs     ____   ,bit 3
 422 +//  ;sck    _--_   ,bit 4
 423 +//  ;di     ____   ,bit 5
 424 +//  ;do     ____   ,bit 6
 425 +//  ;
 426 +static void SetCSToLowForEEprom(void)
 427 +{
 428 +        /* minimum tcs is 1us */
 429 +       gpio_line_set(EECS_PIN, 0);
 430 +        gpio_line_set(EECS_PIN, 0);
 431 +
 432 +       gpio_line_set(EECK_PIN, 0);
 433 +       gpio_line_set(EECK_PIN, 1);
 434 +       gpio_line_set(EECK_PIN, 1);
 435 +       gpio_line_set(EECK_PIN, 1);
 436 +       gpio_line_set(EECK_PIN, 1);
 437 +       gpio_line_set(EECK_PIN, 0);
 438 +
 439 +       gpio_line_set(EECS_PIN, 1);
 440 +
 441 +        udelay(10);
 442 +}
 443 +
 444 +#if 0
 445 +//  ;cs     ----   ,bit 3
 446 +//  ;sck    _--_   ,bit 4
 447 +//  ;di     ____   ,bit 5
 448 +//  ;do     ____   ,bit 6
 449 +//  ;
 450 +static void SetCSToHighForEEprom(void)
 451 +{
 452 +       gpio_line_set(EECS_PIN, 1);
 453 +
 454 +        /* min tskh and tskl is 1us */
 455 +       gpio_line_set(EECK_PIN, 1);
 456 +       udelay(2);
 457 +       gpio_line_set(EECK_PIN, 0);
 458 +}
 459 +#endif /* 0 */
 460 +
 461 +//  ;cs     ----   ,bit 3
 462 +//  ;sck    _--_   ,bit 4
 463 +//  ;di     ----   ,bit 5
 464 +//  ;do     ____   ,bit 6
 465 +//  ;
 466 +static void send1ToEEprom(void)
 467 +{
 468 +//printf("send1ToEEprom(1)...");
 469 +       gpio_line_set(EEDI_PIN, 1);
 470 +
 471 +       gpio_line_set(EECK_PIN, 0);
 472 +        udelay(1);
 473 +       gpio_line_set(EECK_PIN, 1);
 474 +       gpio_line_set(EECK_PIN, 1);
 475 +       gpio_line_set(EECK_PIN, 1);
 476 +        udelay(1);
 477 +       gpio_line_set(EECK_PIN, 0);
 478 +}
 479 +
 480 +//  ;cs     ----   ,bit 3
 481 +//  ;sck    _--_   ,bit 4
 482 +//  ;di     ____   ,bit 5
 483 +//  ;do     ____   ,bit 6
 484 +//  ;
 485 +static void send0ToEEprom(void)
 486 +{
 487 +//printf("send0ToEEprom(0)...");
 488 +       gpio_line_set(EEDI_PIN, 0);
 489 +
 490 +       gpio_line_set(EECK_PIN, 0);
 491 +       udelay(1);
 492 +       gpio_line_set(EECK_PIN, 1);
 493 +       gpio_line_set(EECK_PIN, 1);
 494 +       gpio_line_set(EECK_PIN, 1);
 495 +       udelay(1);
 496 +       gpio_line_set(EECK_PIN, 0);
 497 +}
 498 +
 499 +static void WriteDataToEEprom(unsigned short addr, unsigned short data)
 500 +{
 501 +  unsigned short addr_mask, data_mask;
 502 +
 503 +  SetEEpromToSendState();
 504 +  for (addr_mask = 0x400; addr_mask != 0; )
 505 +  {
 506 +    if (addr & addr_mask)
 507 +      send1ToEEprom();
 508 +    else
 509 +      send0ToEEprom();
 510 +    addr_mask = addr_mask >> 1;
 511 +  }
 512 +  for (data_mask = 0x8000; data_mask != 0; )
 513 +  {
 514 +    if (data & data_mask)
 515 +      send1ToEEprom();
 516 +    else
 517 +      send0ToEEprom();
 518 +    data_mask = data_mask >> 1;
 519 +  }
 520 +  SetCSToLowForEEprom();
 521 +}
 522 +
 523 +static void WriteCmdToEEprom(unsigned short cmd)
 524 +{
 525 +  unsigned short cmd_mask;
 526 +
 527 +  SetEEpromToSendState();
 528 +  for (cmd_mask = 0x0400 ;cmd_mask != 0; )
 529 +  {
 530 +    if (cmd & cmd_mask)
 531 +      send1ToEEprom();
 532 +    else
 533 +      send0ToEEprom();
 534 +    cmd_mask = cmd_mask >> 1;
 535 +  }
 536 +   SetCSToLowForEEprom();
 537 +}
 538 +
 539 +/*
 540 + * Write data to configure registers through EEPROM interface, even we do not have
 541 + * an external EEPROM connectted, ADM6996 got a virtual AT39C66 inside
 542 + */
 543 +static void WriteDataToRegister(unsigned short RegNumber, unsigned short data)
 544 +{
 545 +    unsigned short cmd, addr;
 546 +
 547 +    printk("WriteDataToRegister(RegNumber=0x%x, data=0x%x)\n", RegNumber, data);
 548 +
 549 +//  the write enable(WEN) instruction must be executed before any device
 550 +//  programming can be done
 551 +    cmd = 0x04c0;
 552 +    WriteCmdToEEprom(cmd); //00000001 0011000000B
 553 +
 554 +    addr = 0x0500 | (RegNumber & 0x0ff); //00000001 01dddddddd
 555 +    WriteDataToEEprom(addr, data); //00000001 01dddddd
 556 +
 557 +
 558 +//  after all data has been written to EEprom , the write disable(WDS)
 559 +//  instruction must be executed
 560 +    cmd = 0x0400;
 561 +    WriteCmdToEEprom(cmd); //00000001 00000000B
 562 +}
 563 +
 564 +static void SerialDelay(int count)
 565 +{
 566 +     udelay(count);
 567 +}
 568 +
 569 +static void InitSerialInterface(void)
 570 +{
 571 +        gpio_line_set(EECK_PIN, 0);
 572 +        gpio_line_set(EEDI_PIN, 0);
 573 +}
 574 +
 575 +static void SerialPulse(void)
 576 +{
 577 +        gpio_line_set(EECK_PIN, 0);
 578 +        gpio_line_set(EECK_PIN, 1);
 579 +         SerialDelay(10);
 580 +         gpio_line_set(EECK_PIN, 1);
 581 +         gpio_line_set(EECK_PIN, 0);
 582 +}
 583 +/*
 584 + * Since there is no EEPROM is our board, read from EEPROM need to obey the timing alike
 585 + *  MII interface, EECK = MDC, EEDI = MDIO, please refer to section 4.3 of ADM6996 datasheet
 586 + */
 587 +void ReadDataFromRegister(unsigned short addr, unsigned short *hidata, unsigned short *lodata, int select_count)
 588 +{
 589 +       unsigned short addr_mask, data_mask;
 590 +       int value, i;
 591 +        unsigned char StartBits, Opcode, TAbits;
 592 +
 593 +       gpio_line_config_out_all();
 594 +       mdelay(1);
 595 +       /* initialize serial interface */
 596 +        gpio_line_set(EECS_PIN, 0);
 597 +       InitSerialInterface();
 598 +
 599 +        /* Preamble, 35 bits */
 600 +        gpio_line_set(EECK_PIN, 0);
 601 +        gpio_line_set(EEDI_PIN, 1);
 602 +        for (i = 0; i < 35; i++)
 603 +        {
 604 +            gpio_line_set(EECK_PIN, 1);
 605 +            SerialDelay(10);
 606 +            gpio_line_set(EECK_PIN, 0);
 607 +           SerialDelay(10);
 608 +        }
 609 +
 610 +        /* Start bits, 2-bit(01b) */
 611 +        InitSerialInterface();
 612 +        StartBits = 0x01;
 613 +        for (i = 0; i < 2; i++)
 614 +        {
 615 +             value = (StartBits & 2) ? 1 : 0;
 616 +             gpio_line_set(EEDI_PIN, value);
 617 +             SerialDelay(1);
 618 +             SerialPulse();
 619 +             StartBits <<= 1;
 620 +         }
 621 +
 622 +         /* Opcode, read = 10b */
 623 +         InitSerialInterface();
 624 +         Opcode = 0x02;
 625 +         for (i = 0; i < 2; i++)
 626 +         {
 627 +             value = (Opcode & 0x02) ? 1 : 0;
 628 +             gpio_line_set(EEDI_PIN, value);
 629 +            SerialDelay(1);
 630 +             SerialPulse();
 631 +             Opcode <<= 1;
 632 +         }
 633 +
 634 +         /* 10 bits register address */
 635 +         /* 1-bit Table Select, 2-bit Device Address, 7-bit Register Address  */
 636 +         InitSerialInterface();
 637 +        if (select_count)
 638 +               addr = (addr & 0x7f) | 0x200;
 639 +        else
 640 +               addr = addr & 0x7f ;
 641 +         for (addr_mask = 0x200; addr_mask != 0; addr_mask >>= 1)
 642 +         {
 643 +             value = (addr & addr_mask) ? 1 : 0;
 644 +             gpio_line_set(EEDI_PIN, value);
 645 +            SerialDelay(1);
 646 +             SerialPulse();
 647 +         }
 648 +
 649 +         /* TA, turnaround 2-bit */
 650 +         InitSerialInterface();
 651 +         TAbits = 0x02;
 652 +          gpio_line_config_in(EEDI_PIN);
 653 +         for (i = 0; i < 2; i++)
 654 +         {
 655 +             gpio_line_set(EECK_PIN, 1);
 656 +            SerialDelay(4);
 657 +            gpio_line_get(EEDI_PIN, &value);
 658 +            SerialDelay(4);
 659 +             TAbits <<= 1;
 660 +            gpio_line_set(EECK_PIN, 1);
 661 +         }
 662 +
 663 +
 664 +         /* Latch data from serial management EEDI pin */
 665 +         *hidata = 0;
 666 +         gpio_line_set(EECK_PIN, 0);
 667 +        for (data_mask = 0x8000; data_mask != 0; data_mask >>= 1)
 668 +         {
 669 +             SerialDelay(4);
 670 +             gpio_line_set(EECK_PIN, 1);
 671 +             gpio_line_get(EEDI_PIN, &value);
 672 +             if (value)
 673 +             {
 674 +                 *hidata |= data_mask;
 675 +            }
 676 +             gpio_line_set(EECK_PIN, 0);
 677 +            SerialDelay(4);
 678 +         }
 679 +        *lodata = 0;
 680 +         gpio_line_set(EECK_PIN, 0);
 681 +        for (data_mask = 0x8000; data_mask != 0; data_mask >>= 1)
 682 +         {
 683 +             SerialDelay(4);
 684 +             gpio_line_set(EECK_PIN, 1);
 685 +             gpio_line_get(EEDI_PIN, &value);
 686 +             if (value)
 687 +             {
 688 +                 *lodata |= data_mask;
 689 +            }
 690 +             gpio_line_set(EECK_PIN, 0);
 691 +            SerialDelay(4);
 692 +         }
 693 +
 694 +         SerialDelay(2);
 695 +
 696 +         /* Idle, EECK must send at least one clock at idle time */
 697 +         SerialPulse();
 698 +         gpio_line_set(EECK_PIN, 0);
 699 +         SerialDelay(10);
 700 +         gpio_line_set(EECK_PIN, 1);
 701 +         SerialDelay(10);
 702 +         gpio_line_set(EECK_PIN, 0);
 703 +         SerialPulse();
 704 +
 705 +         gpio_line_config_out(EEDI_PIN);
 706 +        gpio_line_set(EECS_PIN, 1);
 707 +
 708 +   printk("ReadDataFromRegister(addr=0x%x, hidata=0x%x, lodata=0x%x)\n", addr, *hidata, *lodata);
 709 +}
 710 diff -Nur linux-mips-cvs/drivers/net/port_based_qos/port_based_qos.c linux-broadcom/drivers/net/port_based_qos/port_based_qos.c
 711 --- linux-mips-cvs/drivers/net/port_based_qos/port_based_qos.c  1970-01-01 01:00:00.000000000 +0100
 712 +++ linux-broadcom/drivers/net/port_based_qos/port_based_qos.c  2005-01-31 13:13:14.000000000 +0100
 713 @@ -0,0 +1,460 @@
 714 + /*
 715 + * Remaining issues:
 716 + *   + stats support
 717 + *   + multicast support
 718 + *   + media sense
 719 + *   + half/full duplex
 720 + *   - random MAC addr.
 721 + */
 722 +
 723 +#include <linux/config.h>
 724 +#include <linux/module.h>
 725 +#include <linux/kernel.h>
 726 +#include <linux/pci.h>
 727 +#include <linux/init.h>
 728 +#include <linux/ioport.h>
 729 +#include <linux/netdevice.h>
 730 +#include <linux/etherdevice.h>
 731 +#include <linux/ethtool.h>
 732 +#include <linux/mii.h>
 733 +#include <asm/io.h>
 734 +
 735 +#include <linux/sysctl.h>
 736 +#include <cy_conf.h>
 737 +
 738 +#define MODULE_NAME "port_based_qos_mod"
 739 +#define DEVICE_NAME "qos"
 740 +#define MODULE_VERSION "0.0.1"
 741 +
 742 +extern void ReadDataFromRegister(unsigned short addr, unsigned short *hidata, unsigned short *lodata,int select_count);
 743 +
 744 +#ifdef PERFORMANCE_SUPPORT
 745 +static struct ctl_table_header *qos_sysctl_header;
 746 +static unsigned long qos[28];
 747 +
 748 +static ctl_table mytable[] = {
 749 +         { 2000, "qos",
 750 +          qos, sizeof(qos),
 751 +          0644, NULL,
 752 +          proc_dointvec },
 753 +         { 0 }
 754 +};
 755 +
 756 +static unsigned short statis_addr_map[7][6] ={
 757 +       {0x04, 0x06, 0x08, 0x0a, 0x0b, 0x0c},
 758 +       {0x16, 0x18, 0x1a, 0x1c, 0x1d, 0x1e},
 759 +       {0x0d, 0x0f, 0x11, 0x13, 0x14, 0x15},
 760 +       {0x1f, 0x21, 0x23, 0x25, 0x26, 0x27},
 761 +       {0x31, 0x33, 0x35, 0x37, 0x38, 0x39},
 762 +       {0x28, 0x2a, 0x2c, 0x2e, 0x2f, 0x30},
 763 +       {0x01, 0x01, 0x01, 0x01, 0x01, 0x01}
 764 +};
 765 +
 766 +unsigned long get_statistic_from_serial(unsigned short port, unsigned short item)
 767 +{
 768 +    unsigned short hidata, lodata;
 769 +
 770 +    ReadDataFromRegister(statis_addr_map[item][port], &hidata, &lodata, 1);
 771 +       return ((hidata << 16) | lodata);
 772 +}
 773 +#endif
 774 +
 775 +#ifdef HW_QOS_SUPPORT
 776 +struct port_qos_t{
 777 +       int addr;
 778 +       int content_mask;
 779 +       int *content_set;
 780 +};
 781 +
 782 +void WriteDataToRegister_(unsigned short reg_idx, unsigned short content_idx);
 783 +extern void write_eeprom(short,short *,int);
 784 +
 785 +#define BANDWIDTH_1_BIT        2
 786 +#define BANDWIDTH_2_BIT        4
 787 +#define BANDWIDTH_3_BIT        6
 788 +#define BANDWIDTH_4_BIT        7
 789 +
 790 +#define PORT_CONFIG_1  0x3
 791 +#define PORT_CONFIG_2  0x5
 792 +#define PORT_CONFIG_3  0x7
 793 +#define PORT_CONFIG_4  0x8
 794 +#define BANDWIDTH_CTL_123      0x31
 795 +#define BANDWIDTH_CTL_4        0x32
 796 +#define BANDWIDTH_CTL_ENABLE   0x33
 797 +#define DISCARD_MODE   0x10
 798 +#define TOS_PRIO_MAP   0xf
 799 +
 800 +#define PRIORITY_MASK  0xfc7f
 801 +#define PRIORITY_DISABLE_MASK  0xfc7e
 802 +#define FLOW_CTL_MASK  0xfffe
 803 +#define RATE_LIMIT_MASK_1      0xff8f
 804 +#define RATE_LIMIT_MASK_2      0xf8ff
 805 +#define RATE_LIMIT_MASK_3      0x8fff
 806 +#define RATE_LIMIT_MASK_4      0xfff8
 807 +#define BANDWIDTH_CTL_MASK     0xff2b
 808 +#define DISCARD_MASK   0x0fff
 809 +
 810 +#define BANDWIDTH_ENABLE_1     1 << BANDWIDTH_1_BIT//04
 811 +#define BANDWIDTH_ENABLE_2     1 << BANDWIDTH_2_BIT//10
 812 +#define BANDWIDTH_ENABLE_3     1 << BANDWIDTH_3_BIT//40
 813 +#define BANDWIDTH_ENABLE_4     1 << BANDWIDTH_4_BIT//80
 814 +#define BANDWIDTH_CTL_MASK_1   0xffff^BANDWIDTH_ENABLE_1//0xfffb
 815 +#define BANDWIDTH_CTL_MASK_2   0xffff^BANDWIDTH_ENABLE_2//0xffef
 816 +#define BANDWIDTH_CTL_MASK_3   0xffff^BANDWIDTH_ENABLE_3//0xffbf
 817 +#define BANDWIDTH_CTL_MASK_4   0xffff^BANDWIDTH_ENABLE_4//0xff7f
 818 +
 819 +/*static int disable_content[] = {0x0};
 820 +//static int enable_content[] = {0xd4, 0x0cff};//bit 7,6,4,2; Q1=11(50%),Q0=00(0%)*/
 821 +//static int sw_content[] = {0x0,0x0c00};//bit 7,6,4,2; Q1=11(50%),Q0=00(0%)
 822 +static int sw_content[] = {0x0,0xc000};//bit 7,6,4,2; Q1=11(50%),Q0=00(0%)
 823 +static int port_priority_content[] = {0x080,0x380};//Q0,Q3
 824 +//static int port_priority_content[] = {0x300,0x0};//Q1,Q0
 825 +static int port_flow_ctl_content[] = {0x0,0x1};
 826 +static int port_rate_limit_content_1[] = {0x0,0x00,0x10,0x20,0x30,0x40,0x50,0x60,0x70};
 827 +static int port_rate_limit_content_2[] = {0x0,0x000,0x100,0x200,0x300,0x400,0x500,0x600,0x700};
 828 +static int port_rate_limit_content_3[] = {0x0,0x0000,0x1000,0x2000,0x3000,0x4000,0x5000,0x6000,0x7000};
 829 +static int port_rate_limit_content_4[] = {0x0,0x0,0x1,0x2,0x3,0x4,0x5,0x6,0x7};
 830 +static int port_rate_limit_enable_1[] = {0x0, BANDWIDTH_ENABLE_1};
 831 +static int port_rate_limit_enable_2[] = {0x0, BANDWIDTH_ENABLE_2};
 832 +static int port_rate_limit_enable_3[] = {0x0, BANDWIDTH_ENABLE_3};
 833 +static int port_rate_limit_enable_4[] = {0x0, BANDWIDTH_ENABLE_4};
 834 +
 835 +static struct port_qos_t port_mii_disable[] = {
 836 +       { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK, sw_content},
 837 +       //{ DISCARD_MODE, DISCARD_MASK, sw_content},
 838 +       { PORT_CONFIG_1, PRIORITY_MASK, sw_content},//port_priority_1
 839 +       { PORT_CONFIG_2, PRIORITY_MASK, sw_content},//port_priority_2
 840 +       { PORT_CONFIG_3, PRIORITY_MASK, sw_content},//port_priority_3
 841 +       { PORT_CONFIG_4, PRIORITY_MASK, sw_content},//port_priority_4
 842 +       { PORT_CONFIG_1, FLOW_CTL_MASK, &port_flow_ctl_content[1]},//port_flow_control_1
 843 +       { PORT_CONFIG_2, FLOW_CTL_MASK, &port_flow_ctl_content[1]},//port_flow_control_2
 844 +       { PORT_CONFIG_3, FLOW_CTL_MASK, &port_flow_ctl_content[1]},//port_flow_control_3
 845 +       { PORT_CONFIG_4, FLOW_CTL_MASK, &port_flow_ctl_content[1]},//port_flow_control_4
 846 +       { -1}
 847 +};
 848 +
 849 +static struct port_qos_t port_mii_enable[] = {
 850 +       //{ BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK, enable_content},
 851 +       //{ DISCARD_MODE, DISCARD_MASK, sw_content},
 852 +       { -1}
 853 +};
 854 +
 855 +struct port_qos_t *port_mii_sw_array[] = {port_mii_disable, port_mii_enable};
 856 +
 857 +/*static struct port_qos_t port_mii_addr[] = {
 858 +       { PORT_CONFIG_1, PRIORITY_MASK, port_priority_content},//port_priority_1
 859 +       { PORT_CONFIG_1, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_1
 860 +       //{ "port_frame_type_1", 0x3},
 861 +       { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_14, port_rate_limit_content_14},//port_rate_limit_1
 862 +       { PORT_CONFIG_2, PRIORITY_MASK, port_priority_content},//port_priority_2
 863 +       { PORT_CONFIG_2, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_2
 864 +       //{ "port_frame_type_2", 0x5},
 865 +       { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_2, port_rate_limit_content_2},//port_rate_limit_2
 866 +       { PORT_CONFIG_3, PRIORITY_MASK, port_priority_content},//port_priority_3
 867 +       { PORT_CONFIG_3, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_3
 868 +       //{ "port_frame_type_3", 0x7},
 869 +       { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_3, port_rate_limit_content_3},//port_rate_limit_3
 870 +       //{ "port_priority_4", 0x8, 0x380},
 871 +       { PORT_CONFIG_4, PRIORITY_MASK, port_priority_content},//port_priority_4
 872 +       { PORT_CONFIG_4, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_4
 873 +       //{ "port_frame_type_4", 0x8},
 874 +       { BANDWIDTH_CTL_4, RATE_LIMIT_MASK_14, port_rate_limit_content_14},//port_rate_limit_4
 875 +       { -1}
 876 +};*/
 877 +
 878 +static struct port_qos_t priority_1[] = {
 879 +       { PORT_CONFIG_1, PRIORITY_MASK, port_priority_content},//port_priority_1
 880 +       { -1}
 881 +};
 882 +static struct port_qos_t flow_control_1[] = {
 883 +       { PORT_CONFIG_1, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_1
 884 +       { -1}
 885 +};
 886 +static struct port_qos_t rate_limit_1[] = {
 887 +       { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_1, port_rate_limit_content_1},//port_rate_limit_1
 888 +       { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK_1, port_rate_limit_enable_1},//port_rate_limit_4
 889 +       { -1}
 890 +};
 891 +static struct port_qos_t priority_2[] = {
 892 +       { PORT_CONFIG_2, PRIORITY_MASK, port_priority_content},//port_priority_2
 893 +       { -1}
 894 +};
 895 +static struct port_qos_t flow_control_2[] = {
 896 +       { PORT_CONFIG_2, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_2
 897 +       { -1}
 898 +};
 899 +static struct port_qos_t rate_limit_2[] = {
 900 +       { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_2, port_rate_limit_content_2},//port_rate_limit_2
 901 +       { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK_2, port_rate_limit_enable_2},//port_rate_limit_4
 902 +       { -1}
 903 +};
 904 +static struct port_qos_t priority_3[] = {
 905 +       { PORT_CONFIG_3, PRIORITY_MASK, port_priority_content},//port_priority_3
 906 +       { -1}
 907 +};
 908 +static struct port_qos_t flow_control_3[] = {
 909 +       { PORT_CONFIG_3, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_3
 910 +       { -1}
 911 +};
 912 +static struct port_qos_t rate_limit_3[] = {
 913 +       { BANDWIDTH_CTL_123, RATE_LIMIT_MASK_3, port_rate_limit_content_3},//port_rate_limit_3
 914 +       { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK_3, port_rate_limit_enable_3},//port_rate_limit_4
 915 +       { -1}
 916 +};
 917 +static struct port_qos_t priority_4[] = {
 918 +       { PORT_CONFIG_4, PRIORITY_MASK, port_priority_content},//port_priority_4
 919 +       { -1}
 920 +};
 921 +static struct port_qos_t flow_control_4[] = {
 922 +       { PORT_CONFIG_4, FLOW_CTL_MASK, port_flow_ctl_content},//port_flow_control_4
 923 +       { -1}
 924 +};
 925 +static struct port_qos_t rate_limit_4[] = {
 926 +       { BANDWIDTH_CTL_4, RATE_LIMIT_MASK_4, port_rate_limit_content_4},//port_rate_limit_4
 927 +       { BANDWIDTH_CTL_ENABLE, BANDWIDTH_CTL_MASK_4, port_rate_limit_enable_4},//port_rate_limit_4
 928 +       { -1}
 929 +};
 930 +
 931 +static struct port_qos_t *port_mii_addr[] = {
 932 +       priority_1,
 933 +       flow_control_1,
 934 +       rate_limit_1,
 935 +       priority_2,
 936 +       flow_control_2,
 937 +       rate_limit_2,
 938 +       priority_3,
 939 +       flow_control_3,
 940 +       rate_limit_3,
 941 +       priority_4,
 942 +       flow_control_4,
 943 +       rate_limit_4,
 944 +       NULL
 945 +};
 946 +
 947 +void WriteDataToRegister_(unsigned short reg_idx, unsigned short content_idx)
 948 +{
 949 +    short RegNumber;
 950 +    unsigned short data, hidata=0x0, lodata=0x0;
 951 +    int i;
 952 +    struct port_qos_t *port_qos = port_mii_addr[reg_idx];
 953 +
 954 +    //printk("\nWriteDataToRegister_:reg_idx=%d content_idx=%d\n", reg_idx, content_idx);
 955 +    if (!port_qos)
 956 +        port_qos = port_mii_sw_array[content_idx];
 957 +
 958 +    for (i=0; port_qos[i].addr != -1; i++)
 959 +    {
 960 +       RegNumber = port_qos[i].addr;
 961 +       ReadDataFromRegister(RegNumber, &hidata, &lodata, 0);
 962 +
 963 +        if (!(RegNumber % 2)) /* even port number use lower word */
 964 +               hidata = lodata;
 965 +
 966 +        data = (hidata & port_qos[i].content_mask) | (((i > 0) && (content_idx > 1))? port_qos[i].content_set[1] : port_qos[i].content_set[content_idx]);
 967 +
 968 +       write_eeprom(RegNumber, &data, 1);
 969 +       ReadDataFromRegister(RegNumber, &hidata, &lodata, 0);
 970 +    }
 971 +       ReadDataFromRegister(0xf, &hidata, &lodata, 0);
 972 +
 973 +       /*RegNumber = port_mii_addr[reg_idx].addr;
 974 +       if (RegNumber == -1)//Disable or Enable
 975 +       {
 976 +               struct port_qos_t *port_mii_sw = port_mii_sw_array[content_idx];
 977 +
 978 +               printk("\nWriteDataToRegister_:reg_idx=%d content_idx=%d\n", reg_idx, content_idx);
 979 +               for (i=0; port_mii_sw[i].addr != -1; i++)
 980 +               {
 981 +                       RegNumber = port_mii_sw[i].addr;
 982 +
 983 +                       ReadDataFromRegister(RegNumber, &hidata, &lodata, 0);
 984 +
 985 +                       if (!(RegNumber % 2))
 986 +                               hidata = lodata;
 987 +
 988 +                       data = (hidata & port_mii_sw[i].content_mask) | port_mii_sw[i].content_set[i];
 989 +
 990 +                       write_eeprom(RegNumber, &data, 1);
 991 +
 992 +                       ReadDataFromRegister(RegNumber, &hidata, &lodata, 0);
 993 +                       printk("\n============== %s===============\n", (content_idx==0)?"disable":"enable");
 994 +               }
 995 +       }
 996 +       else
 997 +       {
 998 +               ReadDataFromRegister(RegNumber, &hidata, &lodata, 0);
 999 +
1000 +               if (!(RegNumber % 2))
1001 +                       hidata = lodata;
1002 +
1003 +               data = (hidata & port_mii_addr[reg_idx].content_mask) | port_mii_addr[reg_idx].content_set[content_idx];
1004 +
1005 +               write_eeprom(RegNumber, &data, 1);
1006 +               ReadDataFromRegister(RegNumber, &hidata, &lodata, 0);
1007 +       }*/
1008 +}
1009 +#endif
1010 +
1011 +static int dev_do_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
1012 +{
1013 +    struct mii_ioctl_data *data = (struct mii_ioctl_data *)req->ifr_data;
1014 +#ifdef PERFORMANCE_SUPPORT
1015 +    int item, port;
1016 +
1017 +    unsigned long status_item;
1018 +#endif
1019 +
1020 +    switch (cmd)
1021 +    {
1022 +    case SIOCGMIIPHY:          /* Get address of MII PHY in use. */
1023 +    case SIOCDEVPRIVATE:       /* for binary compat, remove in 2.5 */
1024 +
1025 +       /* Fall through to the SIOCGMIIREG, taken from eepro100 and rtl
1026 +        * drivers */
1027 +#ifdef PERFORMANCE_SUPPORT
1028 +    case SIOCGMIIREG:          /* Read MII PHY register. */
1029 +       for (item=0; item<6; item++)
1030 +               for (port=1; port<5; port++){
1031 +                       qos[(item * 4) + (port-1)] = get_statistic_from_serial(port, item);
1032 +               }
1033 +
1034 +       status_item = get_statistic_from_serial(0, 6);
1035 +
1036 +       qos[24] = (0x1 & (status_item >> 8));
1037 +       qos[25] = (0x1 & (status_item >> 16));
1038 +       qos[26] = (0x1 & (status_item >> 24));
1039 +       qos[27] = (0x1 & (status_item >> 28));
1040 +
1041 +       return 0;
1042 +#endif
1043 +
1044 +    case SIOCDEVPRIVATE+1:     /* for binary compat, remove in 2.5 */
1045 +#ifdef HW_QOS_SUPPORT
1046 +    case SIOCSMIIREG:          /* Write MII PHY register. */
1047 +       {
1048 +       printk("\n x phy_id=%x\n", data->phy_id);
1049 +       printk("\n x reg_num=%x\n", data->reg_num);
1050 +       printk("\n x val_in=%x\n", data->val_in);
1051 +       printk("\n x val_out=%x\n", data->val_out);
1052 +
1053 +       WriteDataToRegister_(data->phy_id, data->val_in);
1054 +       return 0;
1055 +       }
1056 +#endif
1057 +    case SIOCDEVPRIVATE+2:     /* for binary compat, remove in 2.5 */
1058 +    default:
1059 +       return -EOPNOTSUPP;
1060 +    }
1061 +}
1062 +
1063 +static int __devinit qos_eth_probe(struct net_device *dev)
1064 +{
1065 +
1066 +    SET_MODULE_OWNER(dev);
1067 +
1068 +    ether_setup(dev);
1069 +
1070 +    strcpy(dev->name, DEVICE_NAME "0");
1071 +
1072 +    dev->do_ioctl = dev_do_ioctl;
1073 +
1074 +    return 0;
1075 +}
1076 +
1077 +#ifdef HW_QOS_SUPPORT
1078 +static char *port_option_name[] = {
1079 +       "port_priority_1",
1080 +       "port_flow_control_1",
1081 +       //{ "port_frame_type_1",
1082 +       "port_rate_limit_1",
1083 +       "port_priority_2",
1084 +       "port_flow_control_2",
1085 +       //{ "port_frame_type_2",
1086 +       "port_rate_limit_2",
1087 +       "port_priority_3",
1088 +       "port_flow_control_3",
1089 +       //{ "port_frame_type_3",
1090 +       "port_rate_limit_3",
1091 +       "port_priority_4",
1092 +       //{ "port_priority_4", PORT_CONFIG_4, PRIORITY_MASK, port_priority_content},
1093 +       "port_flow_control_4",
1094 +       //{ "port_frame_type_4",
1095 +       "port_rate_limit_4",
1096 +       "QoS",
1097 +       NULL
1098 +};
1099 +
1100 +extern char *nvram_get(const char *name);
1101 +extern uint bcm_atoi(char *s);
1102 +
1103 +static int set_port_option(struct net_device *dev, unsigned short port_addr, char *option_content)
1104 +{
1105 +    struct ifreq ifr;
1106 +    struct mii_ioctl_data stats;
1107 +
1108 +    stats.phy_id=port_addr;
1109 +    stats.val_in=bcm_atoi(option_content);
1110 +
1111 +    ifr.ifr_data = (void *)&stats;
1112 +
1113 +    return dev_do_ioctl(dev, &ifr, SIOCSMIIREG);
1114 +}
1115 +
1116 +
1117 +void
1118 +restore_default_from_NV(struct net_device *dev)
1119 +{
1120 +       unsigned short i;
1121 +       char *value = NULL;
1122 +
1123 +       for (i = 0; port_option_name[i]; i++)
1124 +       {
1125 +               if((value = nvram_get(port_option_name[i])))
1126 +                       set_port_option(dev, i, value);
1127 +       }
1128 +       return;
1129 +}
1130 +#endif
1131 +
1132 +static struct net_device qos_devices;
1133 +
1134 +/* Module initialization and cleanup */
1135 +int init_module(void)
1136 +{
1137 +    int res;
1138 +    struct net_device *dev;
1139 +
1140 +    printk("Initializing " MODULE_NAME " driver " MODULE_VERSION "\n");
1141 +
1142 +       dev = &qos_devices;
1143 +
1144 +       dev->init = qos_eth_probe;
1145 +
1146 +       if ((res = register_netdev(dev)))
1147 +           printk("Failed to register netdev. res = %d\n", res);
1148 +
1149 +#ifdef PERFORMANCE_SUPPORT
1150 +       qos_sysctl_header = register_sysctl_table(mytable, 0);
1151 +#endif
1152 +#ifdef HW_QOS_SUPPORT
1153 +       restore_default_from_NV(dev);
1154 +       write_eeprom(TOS_PRIO_MAP, &sw_content[0], 1);/* disable TOS priority map*/
1155 +#endif
1156 +    return 0;
1157 +}
1158 +
1159 +void cleanup_module(void)
1160 +{
1161 +       struct net_device *dev = &qos_devices;
1162 +       if (dev->priv != NULL)
1163 +       {
1164 +           unregister_netdev(dev);
1165 +           kfree(dev->priv);
1166 +           dev->priv = NULL;
1167 +       }
1168 +
1169 +#ifdef PERFORMANCE_SUPPORT
1170 +       unregister_sysctl_table(qos_sysctl_header);
1171 +#endif
1172 +}
1173 +