package/uboot-lantiq/files/board/arcadyan/arv752DWP22/athrs26_phy.c

   1 /*
   2  * This file is subject to the terms and conditions of the GNU General Public
   3  * License.  See the file "COPYING" in the main directory of this archive
   4  * for more details.
   5  *
   6  * Copyright © 2003 Atheros Communications, Inc.,  All Rights Reserved.
   7  */
   8
   9 /*
  10  * Manage the atheros ethernet PHY.
  11  *
  12  * All definitions in this file are operating system independent!
  13  */
  14
  15 #include <config.h>
  16 #include <linux/types.h>
  17 #include <common.h>
  18 #include <miiphy.h>
  19 //#include "phy.h"
  20 //#include "ar7100_soc.h"
  21 #include "athrs26_phy.h"
  22
  23 #define phy_reg_read(base, addr, reg, datap)                    \
  24     miiphy_read("lq_cpe_eth", addr, reg, datap);
  25 #define phy_reg_write(base, addr, reg, data)                   \
  26     miiphy_write("lq_cpe_eth", addr, reg, data);
  27
  28
  29 /* PHY selections and access functions */
  30
  31 typedef enum {
  32     PHY_SRCPORT_INFO,
  33     PHY_PORTINFO_SIZE,
  34 } PHY_CAP_TYPE;
  35
  36 typedef enum {
  37     PHY_SRCPORT_NONE,
  38     PHY_SRCPORT_VLANTAG,
  39     PHY_SRCPORT_TRAILER,
  40 } PHY_SRCPORT_TYPE;
  41
  42 #ifdef DEBUG
  43 #define DRV_DEBUG 1
  44 #endif
  45 //#define DRV_DEBUG 1
  46
  47 #define DRV_DEBUG_PHYERROR  0x00000001
  48 #define DRV_DEBUG_PHYCHANGE 0x00000002
  49 #define DRV_DEBUG_PHYSETUP  0x00000004
  50
  51 #if DRV_DEBUG
  52 int athrPhyDebug = DRV_DEBUG_PHYERROR|DRV_DEBUG_PHYCHANGE|DRV_DEBUG_PHYSETUP;
  53
  54 #define DRV_LOG(FLG, X0, X1, X2, X3, X4, X5, X6)    \
  55 {                                                   \
  56     if (athrPhyDebug & (FLG)) {                       \
  57         logMsg(X0, X1, X2, X3, X4, X5, X6);         \
  58     }                                               \
  59 }
  60
  61 #define DRV_MSG(x,a,b,c,d,e,f)                      \
  62     logMsg(x,a,b,c,d,e,f)
  63
  64 #define DRV_PRINT(FLG, X)                           \
  65 {                                                   \
  66     if (athrPhyDebug & (FLG)) {                       \
  67         printf X;                                   \
  68     }                                               \
  69 }
  70
  71 #else /* !DRV_DEBUG */
  72 #define DRV_LOG(DBG_SW, X0, X1, X2, X3, X4, X5, X6)
  73 #define DRV_MSG(x,a,b,c,d,e,f)
  74 #define DRV_PRINT(DBG_SW,X)
  75 #endif
  76
  77 #define ATHR_LAN_PORT_VLAN          1
  78 #define ATHR_WAN_PORT_VLAN          2
  79
  80 #define ENET_UNIT_LAN 0
  81
  82 #define TRUE    1
  83 #define FALSE   0
  84
  85 #define ATHR_PHY0_ADDR   0x0
  86 #define ATHR_PHY1_ADDR   0x1
  87 #define ATHR_PHY2_ADDR   0x2
  88 #define ATHR_PHY3_ADDR   0x3
  89 #define ATHR_PHY4_ADDR   0x4
  90
  91 /*
  92  * Track per-PHY port information.
  93  */
  94 typedef struct {
  95     BOOL   isEnetPort;       /* normal enet port */
  96     BOOL   isPhyAlive;       /* last known state of link */
  97     int    ethUnit;          /* MAC associated with this phy port */
  98     uint32_t phyBase;
  99     uint32_t phyAddr;          /* PHY registers associated with this phy port */
 100     uint32_t VLANTableSetting; /* Value to be written to VLAN table */
 101 } athrPhyInfo_t;
 102
 103 /*
 104  * Per-PHY information, indexed by PHY unit number.
 105  */
 106 static athrPhyInfo_t athrPhyInfo[] = {
 107     {TRUE,   /* phy port 0 -- LAN port 0 */
 108      FALSE,
 109      ENET_UNIT_LAN,
 110      0,
 111      ATHR_PHY0_ADDR,
 112      ATHR_LAN_PORT_VLAN
 113     },
 114
 115     {TRUE,   /* phy port 1 -- LAN port 1 */
 116      FALSE,
 117      ENET_UNIT_LAN,
 118      0,
 119      ATHR_PHY1_ADDR,
 120      ATHR_LAN_PORT_VLAN
 121     },
 122
 123     {TRUE,   /* phy port 2 -- LAN port 2 */
 124      FALSE,
 125      ENET_UNIT_LAN,
 126      0,
 127      ATHR_PHY2_ADDR,
 128      ATHR_LAN_PORT_VLAN
 129     },
 130
 131     {TRUE,   /* phy port 3 -- LAN port 3 */
 132      FALSE,
 133      ENET_UNIT_LAN,
 134      0,
 135      ATHR_PHY3_ADDR,
 136      ATHR_LAN_PORT_VLAN
 137     },
 138
 139     {TRUE,   /* phy port 4 -- WAN port or LAN port 4 */
 140      FALSE,
 141      1,
 142      0,
 143      ATHR_PHY4_ADDR,
 144      ATHR_LAN_PORT_VLAN   /* Send to all ports */
 145     },
 146
 147     {FALSE,  /* phy port 5 -- CPU port (no RJ45 connector) */
 148      TRUE,
 149      ENET_UNIT_LAN,
 150      0,
 151      0x00,
 152      ATHR_LAN_PORT_VLAN    /* Send to all ports */
 153     },
 154 };
 155
 156 #ifdef CFG_ATHRHDR_EN
 157 typedef struct {
 158     uint8_t data[ATHRHDR_MAX_DATA];
 159     uint8_t len;
 160     uint32_t seq;
 161 } cmd_resp_t;
 162
 163 typedef struct {
 164  uint16_t reg_addr;
 165  uint16_t cmd_len;
 166  uint8_t *reg_data;
 167 }cmd_write_t;
 168
 169 static cmd_write_t cmd_write,cmd_read;
 170 static cmd_resp_t cmd_resp;
 171 static struct eth_device *lan_mac;
 172 //static atomic_t seqcnt = ATOMIC_INIT(0);
 173 static int  seqcnt = 0;
 174 static int cmd = 1;
 175 //volatile uchar AthrHdrPkt[60];
 176 #endif
 177
 178 #define ATHR_GLOBALREGBASE    0
 179
 180 //#define ATHR_PHY_MAX (sizeof(athrPhyInfo) / sizeof(athrPhyInfo[0]))
 181 #define ATHR_PHY_MAX 5
 182
 183 /* Range of valid PHY IDs is [MIN..MAX] */
 184 #define ATHR_ID_MIN 0
 185 #define ATHR_ID_MAX (ATHR_PHY_MAX-1)
 186
 187 /* Convenience macros to access myPhyInfo */
 188 #define ATHR_IS_ENET_PORT(phyUnit) (athrPhyInfo[phyUnit].isEnetPort)
 189 #define ATHR_IS_PHY_ALIVE(phyUnit) (athrPhyInfo[phyUnit].isPhyAlive)
 190 #define ATHR_ETHUNIT(phyUnit) (athrPhyInfo[phyUnit].ethUnit)
 191 #define ATHR_PHYBASE(phyUnit) (athrPhyInfo[phyUnit].phyBase)
 192 #define ATHR_PHYADDR(phyUnit) (athrPhyInfo[phyUnit].phyAddr)
 193 #define ATHR_VLAN_TABLE_SETTING(phyUnit) (athrPhyInfo[phyUnit].VLANTableSetting)
 194
 195
 196 #define ATHR_IS_ETHUNIT(phyUnit, ethUnit) \
 197             (ATHR_IS_ENET_PORT(phyUnit) &&        \
 198             ATHR_ETHUNIT(phyUnit) == (ethUnit))
 199
 200 #define ATHR_IS_WAN_PORT(phyUnit) (!(ATHR_ETHUNIT(phyUnit)==ENET_UNIT_LAN))
 201
 202 /* Forward references */
 203 BOOL       athrs26_phy_is_link_alive(int phyUnit);
 204 static uint32_t athrs26_reg_read(uint16_t reg_addr);
 205 static void athrs26_reg_write(uint16_t reg_addr,
 206                               uint32_t reg_val);
 207
 208 /******************************************************************************
 209 *
 210 * athrs26_phy_is_link_alive - test to see if the specified link is alive
 211 *
 212 * RETURNS:
 213 *    TRUE  --> link is alive
 214 *    FALSE --> link is down
 215 */
 216
 217 void athrs26_reg_init()
 218 {
 219
 220     athrs26_reg_write(0x200, 0x200);
 221     athrs26_reg_write(0x300, 0x200);
 222     athrs26_reg_write(0x400, 0x200);
 223     athrs26_reg_write(0x500, 0x200);
 224     athrs26_reg_write(0x600, 0x7d);
 225
 226 #ifdef S26_VER_1_0
 227     phy_reg_write(0, 0, 29, 41);
 228     phy_reg_write(0, 0, 30, 0);
 229     phy_reg_write(0, 1, 29, 41);
 230     phy_reg_write(0, 1, 30, 0);
 231     phy_reg_write(0, 2, 29, 41);
 232     phy_reg_write(0, 2, 30, 0);
 233     phy_reg_write(0, 3, 29, 41);
 234     phy_reg_write(0, 3, 30, 0);
 235     phy_reg_write(0, 4, 29, 41);
 236     phy_reg_write(0, 4, 30, 0);
 237 #endif
 238
 239     athrs26_reg_write(0x38, 0xc000050e);
 240
 241 #ifdef CFG_ATHRHDR_EN
 242     athrs26_reg_write(0x104, 0x4804);
 243 #else
 244     athrs26_reg_write(0x104, 0x4004);
 245 #endif
 246
 247     athrs26_reg_write(0x60, 0xffffffff);
 248     athrs26_reg_write(0x64, 0xaaaaaaaa);
 249     athrs26_reg_write(0x68, 0x55555555);
 250     athrs26_reg_write(0x6c, 0x0);
 251
 252     athrs26_reg_write(0x70, 0x41af);
 253 }
 254
 255 BOOL
 256 athrs26_phy_is_link_alive(int phyUnit)
 257 {
 258     uint16_t phyHwStatus;
 259     uint32_t phyBase;
 260     uint32_t phyAddr;
 261
 262     phyBase = ATHR_PHYBASE(phyUnit);
 263     phyAddr = ATHR_PHYADDR(phyUnit);
 264
 265     phy_reg_read(phyBase, phyAddr, ATHR_PHY_SPEC_STATUS, &phyHwStatus);
 266
 267     if (phyHwStatus & ATHR_STATUS_LINK_PASS)
 268         return TRUE;
 269
 270     return FALSE;
 271 }
 272
 273
 274 /******************************************************************************
 275 *
 276 * athrs26_phy_setup - reset and setup the PHY associated with
 277 * the specified MAC unit number.
 278 *
 279 * Resets the associated PHY port.
 280 *
 281 * RETURNS:
 282 *    TRUE  --> associated PHY is alive
 283 *    FALSE --> no LINKs on this ethernet unit
 284 */
 285
 286 BOOL
 287 athrs26_phy_setup(int ethUnit)
 288 {
 289     int         phyUnit;
 290     uint16_t    phyHwStatus;
 291     uint16_t    timeout;
 292     int         liveLinks = 0;
 293     uint32_t    phyBase = 0;
 294     BOOL        foundPhy = FALSE;
 295     uint32_t  phyAddr = 0;
 296     uint32_t  regVal;
 297
 298
 299     /* See if there's any configuration data for this enet */
 300     /* start auto negogiation on each phy */
 301     for (phyUnit=0; phyUnit < ATHR_PHY_MAX; phyUnit++) {
 302         if (!ATHR_IS_ETHUNIT(phyUnit, ethUnit)) {
 303                         continue;
 304         }
 305
 306
 307         foundPhy = TRUE;
 308         phyBase = ATHR_PHYBASE(phyUnit);
 309         phyAddr = ATHR_PHYADDR(phyUnit);
 310
 311         phy_reg_write(phyBase, phyAddr, ATHR_AUTONEG_ADVERT,
 312                       ATHR_ADVERTISE_ALL);
 313
 314         /* Reset PHYs*/
 315         phy_reg_write(phyBase, phyAddr, ATHR_PHY_CONTROL,
 316                       ATHR_CTRL_AUTONEGOTIATION_ENABLE
 317                       | ATHR_CTRL_SOFTWARE_RESET);
 318
 319         }
 320
 321     if (!foundPhy) {
 322         return FALSE; /* No PHY's configured for this ethUnit */
 323     }
 324
 325     /*
 326      * After the phy is reset, it takes a little while before
 327      * it can respond properly.
 328      */
 329     sysMsDelay(1000);
 330
 331     /*
 332      * Wait up to .75 seconds for ALL associated PHYs to finish
 333      * autonegotiation.  The only way we get out of here sooner is
 334      * if ALL PHYs are connected AND finish autonegotiation.
 335      */
 336     for (phyUnit=0; (phyUnit < ATHR_PHY_MAX) /*&& (timeout > 0) */; phyUnit++) {
 337         if (!ATHR_IS_ETHUNIT(phyUnit, ethUnit)) {
 338             continue;
 339         }
 340
 341         timeout=20;
 342         for (;;) {
 343                         phyHwStatus = 0;
 344             phy_reg_read(phyBase, phyAddr, ATHR_PHY_CONTROL, &phyHwStatus);
 345
 346                 if (ATHR_RESET_DONE(phyHwStatus)) {
 347                 DRV_PRINT(DRV_DEBUG_PHYSETUP,
 348                           ("Port %d, Neg Success\n", phyUnit));
 349                 break;
 350             }
 351             if (timeout == 0) {
 352                 DRV_PRINT(DRV_DEBUG_PHYSETUP,
 353                           ("Port %d, Negogiation timeout\n", phyUnit));
 354                 break;
 355             }
 356             if (--timeout == 0) {
 357                 DRV_PRINT(DRV_DEBUG_PHYSETUP,
 358                           ("Port %d, Negogiation timeout\n", phyUnit));
 359                 break;
 360             }
 361
 362             sysMsDelay(150);
 363         }
 364     }
 365
 366         /*
 367      * All PHYs have had adequate time to autonegotiate.
 368      * Now initialize software status.
 369      *
 370      * It's possible that some ports may take a bit longer
 371      * to autonegotiate; but we can't wait forever.  They'll
 372      * get noticed by mv_phyCheckStatusChange during regular
 373      * polling activities.
 374      */
 375     for (phyUnit=0; phyUnit < ATHR_PHY_MAX; phyUnit++) {
 376         if (!ATHR_IS_ETHUNIT(phyUnit, ethUnit)) {
 377             continue;
 378         }
 379
 380         if (athrs26_phy_is_link_alive(phyUnit)) {
 381             liveLinks++;
 382             ATHR_IS_PHY_ALIVE(phyUnit) = TRUE;
 383         } else {
 384             ATHR_IS_PHY_ALIVE(phyUnit) = FALSE;
 385         }
 386
 387         phy_reg_read(ATHR_PHYBASE(phyUnit), ATHR_PHYADDR(phyUnit),
 388                     ATHR_PHY_SPEC_STATUS, &regVal);
 389         DRV_PRINT(DRV_DEBUG_PHYSETUP,
 390             ("eth%d: Phy Specific Status=%4.4x\n", ethUnit, regVal));
 391     }
 392 #if 0
 393     /* if using header for register configuration, we have to     */
 394     /* configure s26 register after frame transmission is enabled */
 395
 396     athrs26_reg_write(0x200, 0x200);
 397     athrs26_reg_write(0x300, 0x200);
 398     athrs26_reg_write(0x400, 0x200);
 399     athrs26_reg_write(0x500, 0x200);
 400     athrs26_reg_write(0x600, 0x200);
 401         athrs26_reg_write(0x38, 0x50e);
 402 #endif
 403 #ifndef CFG_ATHRHDR_EN
 404 /* if using header for register configuration, we have to     */
 405     /* configure s26 register after frame transmission is enabled */
 406         athrs26_reg_init();
 407 #endif
 408
 409     return (liveLinks > 0);
 410 }
 411
 412 /******************************************************************************
 413 *
 414 * athrs26_phy_is_fdx - Determines whether the phy ports associated with the
 415 * specified device are FULL or HALF duplex.
 416 *
 417 * RETURNS:
 418 *    1  --> FULL
 419 *    0 --> HALF
 420 */
 421 int
 422 athrs26_phy_is_fdx(int ethUnit)
 423 {
 424     int         phyUnit;
 425     uint32_t    phyBase;
 426     uint32_t    phyAddr;
 427     uint16_t    phyHwStatus;
 428     int         ii = 200;
 429
 430     if (ethUnit == ENET_UNIT_LAN)
 431         return TRUE;
 432
 433     for (phyUnit=0; phyUnit < ATHR_PHY_MAX; phyUnit++) {
 434         if (!ATHR_IS_ETHUNIT(phyUnit, ethUnit)) {
 435             continue;
 436         }
 437
 438         if (athrs26_phy_is_link_alive(phyUnit)) {
 439
 440             phyBase = ATHR_PHYBASE(phyUnit);
 441             phyAddr = ATHR_PHYADDR(phyUnit);
 442
 443             do {
 444                 phy_reg_read(phyBase, phyAddr, ATHR_PHY_SPEC_STATUS, &phyHwStatus);
 445                     sysMsDelay(10);
 446             } while((!(phyHwStatus & ATHR_STATUS_RESOVLED)) && --ii);
 447
 448             if (phyHwStatus & ATHER_STATUS_FULL_DEPLEX)
 449                 return TRUE;
 450         }
 451     }
 452
 453     return FALSE;
 454 }
 455
 456
 457 /******************************************************************************
 458 *
 459 * athrs26_phy_speed - Determines the speed of phy ports associated with the
 460 * specified device.
 461 *
 462 * RETURNS:
 463 *               AG7100_PHY_SPEED_10T, AG7100_PHY_SPEED_100TX;
 464 *               AG7100_PHY_SPEED_1000T;
 465 */
 466
 467 BOOL
 468 athrs26_phy_speed(int ethUnit)
 469 {
 470     int         phyUnit;
 471     uint16_t    phyHwStatus;
 472     uint32_t    phyBase;
 473     uint32_t    phyAddr;
 474     int         ii = 200;
 475
 476     if (ethUnit == ENET_UNIT_LAN)
 477         return _100BASET;
 478
 479     for (phyUnit=0; phyUnit < ATHR_PHY_MAX; phyUnit++) {
 480         if (!ATHR_IS_ETHUNIT(phyUnit, ethUnit)) {
 481             continue;
 482         }
 483
 484         if (athrs26_phy_is_link_alive(phyUnit)) {
 485
 486             phyBase = ATHR_PHYBASE(phyUnit);
 487             phyAddr = ATHR_PHYADDR(phyUnit);
 488
 489             do {
 490                 phy_reg_read(phyBase, phyAddr,
 491                                            ATHR_PHY_SPEC_STATUS, &phyHwStatus);
 492                 sysMsDelay(10);
 493             }while((!(phyHwStatus & ATHR_STATUS_RESOVLED)) && --ii);
 494
 495             phyHwStatus = ((phyHwStatus & ATHER_STATUS_LINK_MASK) >>
 496                            ATHER_STATUS_LINK_SHIFT);
 497
 498             switch(phyHwStatus) {
 499             case 0:
 500                 return _10BASET;
 501             case 1:
 502                 return _100BASET;
 503             case 2:
 504                 return _1000BASET;
 505             default:
 506                 DRV_PRINT(DRV_DEBUG_PHYERROR, ("Unkown speed read!\n"));
 507             }
 508         }
 509     }
 510
 511     return _10BASET;
 512 }
 513
 514 /*****************************************************************************
 515 *
 516 * athr_phy_is_up -- checks for significant changes in PHY state.
 517 *
 518 * A "significant change" is:
 519 *     dropped link (e.g. ethernet cable unplugged) OR
 520 *     autonegotiation completed + link (e.g. ethernet cable plugged in)
 521 *
 522 * When a PHY is plugged in, phyLinkGained is called.
 523 * When a PHY is unplugged, phyLinkLost is called.
 524 */
 525
 526 int
 527 athrs26_phy_is_up(int ethUnit)
 528 {
 529     int             phyUnit;
 530     uint16_t        phyHwStatus;
 531     athrPhyInfo_t  *lastStatus;
 532     int             linkCount   = 0;
 533     int             lostLinks   = 0;
 534     int             gainedLinks = 0;
 535     uint32_t        phyBase;
 536     uint32_t        phyAddr;
 537 #ifdef CFG_ATHRHDR_REG
 538     /* if using header to config s26, the link of MAC0 should always be up */
 539     if (ethUnit == ENET_UNIT_LAN)
 540         return 1;
 541 #endif
 542
 543     for (phyUnit=0; phyUnit < ATHR_PHY_MAX; phyUnit++) {
 544         if (!ATHR_IS_ETHUNIT(phyUnit, ethUnit)) {
 545             continue;
 546         }
 547
 548         phyBase = ATHR_PHYBASE(phyUnit);
 549         phyAddr = ATHR_PHYADDR(phyUnit);
 550
 551
 552         lastStatus = &athrPhyInfo[phyUnit];
 553         phy_reg_read(phyBase, phyAddr, ATHR_PHY_SPEC_STATUS, &phyHwStatus);
 554
 555         if (lastStatus->isPhyAlive) { /* last known link status was ALIVE */
 556             /* See if we've lost link */
 557             if (phyHwStatus & ATHR_STATUS_LINK_PASS) {
 558                 linkCount++;
 559             } else {
 560                 lostLinks++;
 561                 DRV_PRINT(DRV_DEBUG_PHYCHANGE,("\nenet%d port%d down\n",
 562                                                ethUnit, phyUnit));
 563                 lastStatus->isPhyAlive = FALSE;
 564             }
 565         } else { /* last known link status was DEAD */
 566             /* Check for reset complete */
 567             phy_reg_read(phyBase, phyAddr, ATHR_PHY_STATUS, &phyHwStatus);
 568             if (!ATHR_RESET_DONE(phyHwStatus))
 569                 continue;
 570
 571             /* Check for AutoNegotiation complete */
 572             if (ATHR_AUTONEG_DONE(phyHwStatus)) {
 573                 //printk("autoneg done\n");
 574                 gainedLinks++;
 575                 linkCount++;
 576                 DRV_PRINT(DRV_DEBUG_PHYCHANGE,("\nenet%d port%d up\n",
 577                                                ethUnit, phyUnit));
 578                 lastStatus->isPhyAlive = TRUE;
 579             }
 580         }
 581     }
 582
 583     return (linkCount);
 584
 585 #if 0
 586     if (linkCount == 0) {
 587         if (lostLinks) {
 588             /* We just lost the last link for this MAC */
 589             phyLinkLost(ethUnit);
 590         }
 591     } else {
 592         if (gainedLinks == linkCount) {
 593             /* We just gained our first link(s) for this MAC */
 594             phyLinkGained(ethUnit);
 595         }
 596     }
 597 #endif
 598 }
 599
 600 #ifdef CFG_ATHRHDR_EN
 601 void athr_hdr_timeout(void){
 602         eth_halt();
 603         NetState = NETLOOP_FAIL;
 604 }
 605
 606 void athr_hdr_handler(uchar *recv_pkt, unsigned dest, unsigned src, unsigned len){
 607         header_receive_pkt(recv_pkt);
 608         NetState = NETLOOP_SUCCESS;
 609 }
 610 static int
 611 athrs26_header_config_reg (struct eth_device *dev, uint8_t wr_flag,
 612                            uint16_t reg_addr, uint16_t cmd_len,
 613                            uint8_t *val)
 614 {
 615     at_header_t at_header;
 616     reg_cmd_t reg_cmd;
 617     uchar *AthrHdrPkt;
 618
 619     AthrHdrPkt = NetTxPacket;
 620
 621     if(AthrHdrPkt == NULL) {
 622                 printf("Null packet\n");
 623                 return;
 624     }
 625     memset(AthrHdrPkt,0,60);
 626
 627     /*fill at_header*/
 628     at_header.reserved0 = 0x10;  //default
 629     at_header.priority = 0;
 630     at_header.type = 0x5;
 631     at_header.broadcast = 0;
 632     at_header.from_cpu = 1;
 633     at_header.reserved1 = 0x01; //default
 634     at_header.port_num = 0;
 635
 636     AthrHdrPkt[0] = at_header.port_num;
 637     AthrHdrPkt[0] |= at_header.reserved1 << 4;
 638     AthrHdrPkt[0] |= at_header.from_cpu << 6;
 639     AthrHdrPkt[0] |= at_header.broadcast << 7;
 640
 641     AthrHdrPkt[1] = at_header.type;
 642     AthrHdrPkt[1] |= at_header.priority << 4;
 643     AthrHdrPkt[1] |= at_header.reserved0 << 6;
 644
 645
 646     /*fill reg cmd*/
 647     if(cmd_len > 4)
 648         cmd_len = 4;//only support 32bits register r/w
 649
 650     reg_cmd.reg_addr = reg_addr&0x3FFFC;
 651     reg_cmd.cmd_len = cmd_len;
 652     reg_cmd.cmd = wr_flag;
 653     reg_cmd.reserved2 = 0x5; //default
 654     reg_cmd.seq_num = seqcnt;
 655
 656     AthrHdrPkt[2] = reg_cmd.reg_addr & 0xff;
 657     AthrHdrPkt[3] = (reg_cmd.reg_addr & 0xff00) >> 8;
 658     AthrHdrPkt[4] = (reg_cmd.reg_addr & 0x30000) >> 16;
 659     AthrHdrPkt[4] |= reg_cmd.cmd_len << 4;
 660     AthrHdrPkt[5] = reg_cmd.cmd << 4;
 661     AthrHdrPkt[5] |= reg_cmd.reserved2 << 5;
 662     AthrHdrPkt[6] = (reg_cmd.seq_num & 0x7f) << 1;
 663     AthrHdrPkt[7] = (reg_cmd.seq_num & 0x7f80) >> 7;
 664     AthrHdrPkt[8] = (reg_cmd.seq_num & 0x7f8000) >> 15;
 665     AthrHdrPkt[9] = (reg_cmd.seq_num & 0x7f800000) >> 23;
 666
 667     /*fill reg data*/
 668     if(!wr_flag)//write
 669         memcpy((AthrHdrPkt + 10), val, cmd_len);
 670
 671     /*start xmit*/
 672     if(dev == NULL) {
 673         printf("ERROR device not found\n");
 674         return -1;
 675     }
 676     header_xmit(dev, AthrHdrPkt ,60);
 677     return 0;
 678 }
 679 void athr_hdr_func(void) {
 680
 681    NetSetTimeout (1 * CFG_HZ,athr_hdr_timeout );
 682    NetSetHandler (athr_hdr_handler);
 683
 684    if(cmd)
 685         athrs26_header_config_reg(lan_mac, cmd, cmd_read.reg_addr, cmd_read.cmd_len, cmd_read.reg_data);
 686    else
 687         athrs26_header_config_reg(lan_mac, cmd, cmd_write.reg_addr, cmd_write.cmd_len, cmd_write.reg_data);
 688 }
 689 static int
 690 athrs26_header_write_reg(uint16_t reg_addr, uint16_t cmd_len, uint8_t *reg_data)
 691 {
 692     int i = 2;
 693     cmd_write.reg_addr = reg_addr;
 694     cmd_write.cmd_len = cmd_len;
 695     cmd_write.reg_data = reg_data;
 696     cmd = 0;
 697     seqcnt++;
 698
 699     do {
 700         if (NetLoop(ATHRHDR) >= 0) /* polls for read/write ack from PHY */
 701            break;
 702     } while (i--);
 703
 704     return i;
 705 }
 706
 707 static int
 708 athrs26_header_read_reg(uint16_t reg_addr, uint16_t cmd_len, uint8_t *reg_data)
 709 {
 710     int i = 2;
 711
 712     cmd_read.reg_addr = reg_addr;
 713     cmd_read.cmd_len = cmd_len;
 714     cmd_read.reg_data = reg_data;
 715     cmd = 1;
 716     seqcnt++;
 717
 718     do {
 719         if (NetLoop(ATHRHDR) >= 0) /* polls for read/write ack from PHY */
 720            break;
 721     } while (i--);
 722
 723     if ((i==0) || (seqcnt != cmd_resp.seq) || (cmd_len != cmd_resp.len)) {
 724         return -1;
 725     }
 726     memcpy (cmd_read.reg_data, cmd_resp.data, cmd_len);
 727     return 0;
 728 }
 729 int header_receive_pkt(uchar *recv_pkt)
 730 {
 731     cmd_resp.len = recv_pkt[4] >> 4;
 732     if (cmd_resp.len > 10)
 733         goto out;
 734
 735     cmd_resp.seq = recv_pkt[6] >> 1;
 736     cmd_resp.seq |= recv_pkt[7] << 7;
 737     cmd_resp.seq |= recv_pkt[8] << 15;
 738     cmd_resp.seq |= recv_pkt[9] << 23;
 739
 740     if (cmd_resp.seq < seqcnt)
 741         goto out;
 742     memcpy (cmd_resp.data, (recv_pkt + 10), cmd_resp.len);
 743 out:
 744      return 0;
 745 }
 746
 747 void athrs26_reg_dev(struct eth_device *mac)
 748 {
 749     lan_mac = mac;
 750 }
 751
 752 #endif
 753
 754 static uint32_t
 755 athrs26_reg_read(uint16_t reg_addr)
 756 {
 757 #ifndef CFG_ATHRHDR_REG
 758     uint16_t reg_word_addr = reg_addr / 2, phy_val;
 759     uint32_t phy_addr;
 760     uint8_t phy_reg;
 761
 762     /* configure register high address */
 763     phy_addr = 0x18;
 764     phy_reg = 0x0;
 765     phy_val = (reg_word_addr >> 8) & 0x1ff;         /* bit16-8 of reg address*/
 766     phy_reg_write (0, phy_addr, phy_reg, phy_val);
 767
 768     /* read register with low address */
 769     phy_addr = 0x10 | ((reg_word_addr >> 5) & 0x7); /* bit7-5 of reg address */
 770     phy_reg = reg_word_addr & 0x1f;                 /* bit 4-0 of reg address*/
 771     phy_reg_read(0, phy_addr, phy_reg, &phy_val);
 772
 773     return phy_val;
 774 #else
 775     uint8_t reg_data[4];
 776
 777     memset (reg_data, 0, 4);
 778     athrs26_header_read_reg(reg_addr, 4, reg_data);
 779     return (reg_data[0] | (reg_data[1] << 8) | (reg_data[2] << 16) | (reg_data[3] << 24));
 780 #endif
 781 }
 782
 783 static void
 784 athrs26_reg_write(uint16_t reg_addr, uint32_t reg_val)
 785 {
 786 #ifndef CFG_ATHRHDR_REG
 787     uint16_t reg_word_addr = reg_addr / 2, phy_val;
 788     uint32_t phy_addr;
 789     uint8_t phy_reg;
 790
 791     /* configure register high address */
 792     phy_addr = 0x18;
 793     phy_reg = 0x0;
 794     phy_val = (reg_word_addr >> 8) & 0x1ff;         /* bit16-8 of reg address*/
 795     phy_reg_write (0, phy_addr, phy_reg, phy_val);
 796
 797     /* read register with low address */
 798     phy_addr = 0x10 | ((reg_word_addr >> 5) & 0x7); /* bit7-5 of reg address */
 799     phy_reg = reg_word_addr & 0x1f;                 /* bit 4-0 of reg address */
 800     phy_reg_write (0, phy_addr, phy_reg, reg_val);
 801 #else
 802     uint8_t reg_data[4];
 803
 804     memset (reg_data, 0, 4);
 805     reg_data[0] = (uint8_t)(0x00ff & reg_val);
 806     reg_data[1] = (uint8_t)((0xff00 & reg_val) >> 8);
 807     reg_data[2] = (uint8_t)((0xff0000 & reg_val) >> 16);
 808     reg_data[3] = (uint8_t)((0xff000000 & reg_val) >> 24);
 809
 810     athrs26_header_write_reg (reg_addr, 4, reg_data);
 811 #endif
 812
 813 }
 814