/*
Copyright (C) 2008, 2009  GE Fanuc, Inc
 
This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.
 
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
 
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

#include <common.h>
#include <command.h>
#include <asm/au1x00.h>
#include <asm/mipsregs.h>
#include "octeon_boot.h"
#include <pci.h>
#include <lib_octeon_shared.h>
#include <lib_octeon.h>
#include <miiphy.h>
#include <configs/octeon_w56xx_shared.h>
#include "config.h"

#if defined(CONFIG_PCI)
extern void init_octeon_pcie (void);
void pci_init_board (void)
{
#if (CONFIG_OCTEON_PCI_HOST == 1)
    init_octeon_pcie();
#endif
}
#endif /* defined(CONFIG_PCI) */

unsigned char connector_type[MAX_NUM_SFP] = {VACANT, VACANT, VACANT, VACANT};

extern int eeprom_diags(void);
extern void uboot_version_num_store(void);
extern int write_tlv_tuple(octeon_eeprom_header_t *tlv_hdr_ptr);
extern int octeon_twsi_write8_addr16(uint8_t dev_addr, uint16_t addr, uint8_t data);
                 
int checkboard (void);
void cvm_oct_led_init(void);
int early_board_init(void);
void octeon_led_str_write(const char *str);
void phyRegWrite(uint8_t mdio_addr, uint16_t reg, uint16_t mask, uint16_t newVal, int wait, uint8_t dev_type);
void phyRegRead (uint8_t mdio_addr, uint16_t reg, uint16_t *val, uint8_t dev_type);
int cvmx_mdio_address_write_ieee_clause45(unsigned char phy_op, int bus_id, int phy_addr, uint8_t dev_type, uint16_t data);
int cvmx_mdio_read_ieee_clause45(int bus_id, int phy_addr, uint8_t dev_type);

int fanInit(void);

void late_board_init(void);
int octeon_post_error_store(uint8_t error_code);
int octeon_post_error_retrieve(uint8_t *post_error_code_buf_ptr);
int octeon_post_error_clear(void);

/* for boards with 10/100/1000 SFPs*/
int do_sfp_show(void);
int do_temp_show(void);
void sfpVSRegWrite(int dev, int reg, int mask, int newVal, int reset_wait);
void sfpVSRegRead(int dev, int reg, int *val);
void sfpInit(int interface);
int do_sfp_init(void);
void sfpLinkSetup(void);

/* for boards with 10G phy w\ SFP+ */
int amcc10gPhyLinkSetup(uint8_t mdio_addr);
int amcc_is_phy_alive(uint8_t mdio_addr);

/* for boards with PHYs*/
void phyInit(uint8_t addr);
void phyLinkSetup(void);

/* return 4 character processor model number */
int getProcessorStr(char *modelStr, int modelStrSize);

int checkboard (void)
{
#if defined(CONFIG_MII) || (CONFIG_COMMANDS & CFG_CMD_MII)
    /* Enable MDIO */
    cvmx_write_csr(CVMX_SMI_EN, 1);
    uint64_t data = cvmx_read_csr(CVMX_SMI_CLK);

    if (IS_10G_PHY_PRESENT)
    {
        data |= 0x1000000; //set bit 24(MODE) = 1 (Clause 45 compliant for 10G phy)
    }

    data |= 0x2000; //set bit 13 CLK_IDLE to 1. Makes clock not cycle when bus is idle
    cvmx_write_csr(CVMX_SMI_CLK, data);

#endif

    return 0;
}

void cvm_oct_led_init(void)

{
    cvmx_led_en_t          en;
    cvmx_led_udd_cntx_t    udd_cnt;
    cvmx_led_prt_t         prt_en;
    cvmx_led_prt_fmt_t     frmt;

    /* enable led interface and set the number of bits in the shift register */
    en.u64 = 1;            
    udd_cnt.u64 = 0; 

    /* enable ports that are allowed to be displayed */
    prt_en.u64 = 0xFF; // 0:3 = ports 0-3 (interface 0) 4:7 = ports 4-7 (interface 1)

    /* set the port format and set the status bit */
    frmt.u64 = 0x0;                    // { LED_PRT_STATUSn[0] }

    /* write it to the csr */
    cvmx_write_csr( CVMX_LED_EN, en.u64 );
    cvmx_write_csr( CVMX_LED_DBG, 0 );            
    cvmx_write_csr( CVMX_LED_UDD_CNTX(0), udd_cnt.u64 );
    cvmx_write_csr( CVMX_LED_UDD_CNTX(1), udd_cnt.u64 );
    cvmx_write_csr( CVMX_LED_PRT, prt_en.u64 );           
    cvmx_write_csr( CVMX_LED_PRT_FMT, frmt.u64 );

    // set LED_PRT_STATUSn[0]
    cvmx_write_csr( CVMX_LED_PRT_STATUSX(0), 0 );
    cvmx_write_csr( CVMX_LED_PRT_STATUSX(1), 0 );
    cvmx_write_csr( CVMX_LED_PRT_STATUSX(2), 0 );
    cvmx_write_csr( CVMX_LED_PRT_STATUSX(3), 0 );
    cvmx_write_csr( CVMX_LED_PRT_STATUSX(4), 0 );
    cvmx_write_csr( CVMX_LED_PRT_STATUSX(5), 0 );
    cvmx_write_csr( CVMX_LED_PRT_STATUSX(6), 0 );
    cvmx_write_csr( CVMX_LED_PRT_STATUSX(7), 0 );

    // DEBUGPRINT( "led: %s led frmt: 0x%lx led port enable: 0x%lx bit count: 0x%lx\n", 
    //             (en.u64) ? "enabled" : "disabled", 
    //             frmt.u64, prt_en.u64, udd_cnt.u64 );
}

/** 
 * Init all of the stuff that needs to be configured early in 
 * the boot process (pre RAM relocation) 
 * 
 * 
 * @return int
 */
#include "cvmx-gpio.h"
int early_board_init(void)
{
    int cpu_ref;
    char processorStr[5];

    DECLARE_GLOBAL_DATA_PTR;

    cpu_ref = 33;

    memset((void *)&(gd->mac_desc), 0x0, sizeof(octeon_eeprom_mac_addr_t));
    memset((void *)&(gd->clock_desc), 0x0, sizeof(octeon_eeprom_clock_desc_t));
    memset((void *)&(gd->board_desc), 0x0, sizeof(octeon_eeprom_board_desc_t));

    /* NOTE: this is early in the init process, so the serial port is not yet configured */

    /* clear shift register LEDs on the Octeon */
    cvm_oct_led_init();

    /* Populate global data from eeprom */
    uint8_t ee_buf[OCTEON_EEPROM_MAX_TUPLE_LENGTH];
    int addr;
    octeon_eeprom_header_t *tlv_hdr_ptr = (void *)ee_buf;
    octeon_eeprom_board_desc_t *bd_ptr = (void *)ee_buf;

    addr = octeon_tlv_get_tuple_addr(CFG_DEF_EEPROM_ADDR, EEPROM_CLOCK_DESC_TYPE, 0, ee_buf, OCTEON_EEPROM_MAX_TUPLE_LENGTH);
    if (addr >= 0)
    {
        memcpy((void *)&(gd->clock_desc), ee_buf, sizeof(octeon_eeprom_clock_desc_t));
    }

    /* Determine board type/rev */
    strncpy((char *)(gd->board_desc.serial_str), "unknown", SERIAL_LEN);
    addr = octeon_tlv_get_tuple_addr(CFG_DEF_EEPROM_ADDR, EEPROM_BOARD_DESC_TYPE, 0, ee_buf, OCTEON_EEPROM_MAX_TUPLE_LENGTH);

    if (addr >= 0)
    {
        memcpy((void *)&(gd->board_desc), ee_buf, sizeof(octeon_eeprom_board_desc_t));
    }
    else
    {
        gd->flags |= GD_FLG_BOARD_DESC_MISSING;
        gd->board_desc.rev_minor = 0;

        getProcessorStr(processorStr,sizeof(processorStr));

        if (!strncmp(processorStr, "5650", 4))
        {    
            if (IS_SFP_PRESENT)
            {
                gd->board_desc.board_type     = CVMX_BOARD_TYPE_W5650;
            }
            else if (IS_10G_PHY_PRESENT)
            {
                gd->board_desc.board_type     = CVMX_BOARD_TYPE_W5651X;
            }                          
            else
            {
                gd->board_desc.board_type     = CVMX_BOARD_TYPE_NULL;
            }
        }
        else if (!strncmp(processorStr, "5434", 4))
        {
            gd->board_desc.board_type     = CVMX_BOARD_TYPE_W5434;
        }
        else
        {
            gd->board_desc.board_type     = CVMX_BOARD_TYPE_NULL;
        }

#ifdef OBSOLETE
        /* Try to determine board rev by looking at PAL */
        if (octeon_ebt3000_rev1())
        {
            gd->board_desc.rev_major = 1;
        }
        else
        {
            gd->board_desc.rev_major = 2;
        }
#endif

	gd->board_desc.rev_major = 1;

        /* ZBY: Set the Tuple if it was not there */
        tlv_hdr_ptr->type = EEPROM_BOARD_DESC_TYPE;
        tlv_hdr_ptr->length = sizeof(octeon_eeprom_board_desc_t);
        tlv_hdr_ptr->checksum = 0;
        tlv_hdr_ptr->major_version = OCTEON_EEPROM_BOARD_DESC_MAJOR_VER;
        tlv_hdr_ptr->minor_version = OCTEON_EEPROM_BOARD_DESC_MINOR_VER;
        bd_ptr->board_type     = gd->board_desc.board_type;
        bd_ptr->rev_major      = 1;
        bd_ptr->rev_minor      = 0;
        memset(bd_ptr->serial_str, 0, SERIAL_LEN);
        strncpy((char *)bd_ptr->serial_str, "unknown", SERIAL_LEN);
		
        write_tlv_tuple(tlv_hdr_ptr);
    }

#ifdef OBSOLETE
    if (gd->board_desc.rev_major == 1)
    {
        if (gd->clock_desc.cpu_ref_clock_mhz_x_8)
        {
            cpu_ref = gd->clock_desc.cpu_ref_clock_mhz_x_8/8;
            gd->ddr_clock_mhz = gd->clock_desc.ddr_clock_mhz;
        }
        else
        {
            gd->flags |= GD_FLG_CLOCK_DESC_MISSING;
            /* Default values */
            cpu_ref = 33;
            gd->ddr_clock_mhz = EBT3000_REV1_DEF_DRAM_FREQ;
        }
    }
    else
    {
        /* Even though the CPU ref freq is stored in the clock descriptor, we don't read it here.  The MCU
        ** reads it and configures the clock, and we read how the clock is actually configured.
        ** The bootloader does not need to read the clock descriptor tuple for normal operation on
        ** rev 2 and later boards
        */
        cpu_ref = octeon_mcu_read_cpu_ref();
        gd->ddr_clock_mhz = octeon_mcu_read_ddr_clock();
    }

	*BOARD_LED_CONTROL_REG = 0x7;/* give us an indication that U-BOOT is up and running */

    /* Some sanity checks */
    if (cpu_ref <= 0)
    {
        /* Default to 33 */
        cpu_ref = 33;
    }
    if (gd->ddr_clock_mhz <= 0)
    {
        /* Default to 33 */
        gd->ddr_clock_mhz = 200;
    }
#endif

    cpu_ref = 50;
    gd->ddr_clock_mhz = 333;

    addr = octeon_tlv_get_tuple_addr(CFG_DEF_EEPROM_ADDR, EEPROM_MAC_ADDR_TYPE, 0, ee_buf, OCTEON_EEPROM_MAX_TUPLE_LENGTH);
    if (addr >= 0)
    {
        memcpy((void *)&(gd->mac_desc), ee_buf, sizeof(octeon_eeprom_mac_addr_t));
    }
    else
    {
        /* Make up some MAC addresses */
        gd->mac_desc.count = 255;
        gd->mac_desc.mac_addr_base[0] = 0x00;
        gd->mac_desc.mac_addr_base[1] = 0xBE;
        gd->mac_desc.mac_addr_base[2] = 0xEF;
        gd->mac_desc.mac_addr_base[3] = (gd->board_desc.rev_major<<4) | gd->board_desc.rev_minor;
        gd->mac_desc.mac_addr_base[4] = gd->board_desc.serial_str[0];
        gd->mac_desc.mac_addr_base[5] = 0x00;
    }

    /* Read CPU clock multiplier */
    uint64_t data = cvmx_read_csr((uint64_t)0x80011F0000008510ull);
    data = data >> 18;
    data &= 0x1f;

    gd->cpu_clock_mhz = data * cpu_ref;

    /* adjust for 33.33 Mhz clock */
    if (cpu_ref == 33)
        gd->cpu_clock_mhz += (data)/4 + data/8;

    return 0;

}
void octeon_led_str_write(const char *str)
{
    octeon_led_str_write_std(str);
}



/** 
 * Function to write data to a register on the MDIO interface.
 * 
 * @param addr
 * @param reg
 * @param mask
 * @param newVal
 * @param wait
 * @param dev_type - dev_type is only used in clause 45 10G phy
 */
void phyRegWrite(uint8_t mdio_addr, uint16_t reg, uint16_t mask, uint16_t newVal, int wait, uint8_t dev_type)
{
    uint16_t data;
    int rc;

    if (IS_10G_PHY_PRESENT)
    {   /* clause 45 MDIO access */

        phyRegRead(mdio_addr, reg, &data, dev_type);
        data = (data & mask) | newVal;

        rc = cvmx_mdio_address_write_ieee_clause45(0x00, 0, mdio_addr, dev_type, reg);
        rc = cvmx_mdio_address_write_ieee_clause45(0x01, 0, mdio_addr, dev_type, data);                                  
    }
    else
    {   /* clause 22 MDIO access */

        /** 
         *  Read the current value out of the register on the MDIO
         *  interface.
         *  
         * int miiphy_read(unsigned char  addr, unsigned char  reg, 
         * unsigned short *value) 
         **/ 
        miiphy_read (mdio_addr, reg, &data);
        data = (data & mask) | newVal;
        /** 
         * Write the new data out to the register on the MDIO interface.
         *  
         * int miiphy_write(unsigned char  addr, unsigned char  reg, 
         * unsigned short value) 
         **/ 
        miiphy_write (mdio_addr, reg, data);
        if (wait)
        {
            do
            {
                miiphy_read (mdio_addr, reg, &data);
            }
            while ((data & 0x8000) != 0) ; /* wait till finished */
        }
    }
}

/** 
 * Function to read data from a register on the MDIO interface.
 * 
 * @param addr
 * @param reg
 * @param val
 * @param dev_type - dev_type is only used in clause 45 10G phy
 */
void phyRegRead(uint8_t mdio_addr, uint16_t reg, uint16_t *val, uint8_t dev_type)
{
    int rc;

    if (IS_10G_PHY_PRESENT)
    {   /* clause 45 MDIO access */
        rc = cvmx_mdio_address_write_ieee_clause45(0x00, 0, mdio_addr, dev_type, reg);
        *val = cvmx_mdio_read_ieee_clause45(0, mdio_addr, dev_type);                                  
        rc = *val;
    }
    else
    {   /* clause 22 MDIO access */

        /* read the current value out of the register on the MDIO interface */
        miiphy_read (mdio_addr, reg, val);
    }
}

#define FAN_TRY_COUNT	10
int fanInit(void)
{
    int data, i, j;

    data = octeon_twsi_read8( BOARD_TEMP_SENSOR_TWSI_ADDR, 0x00 );
    data |= 0x1; /* set the low bit for to activate monitoring */
    octeon_twsi_write8_addr16( BOARD_TEMP_SENSOR_TWSI_ADDR, 0x00, data );

    /* probe the fan speed RPM monitor for 'x' loops */
    for ( i = 0; i < FAN_TRY_COUNT; i++ )
    {
        data = octeon_twsi_read8( BOARD_TEMP_SENSOR_TWSI_ADDR, 0x08 );
        if ( 0xFF != data ) /* fan is spinning, we are good */
            break;

        printf("fanInit(): fan speed was invalid!\n");
        for ( j = 0; j < 1000000; j++ );
    }
    if ( FAN_TRY_COUNT == i )
    {
        printf("fanInit(): fan speed was never valid!\n");
        return -1;
    }
    return 0;
}


/** 
 * all of the post relocation setup stuff
 */
void late_board_init(void)
{
    int i, j, local, remote;

    printf("FPGA Version: 0x%02x\n", *(unsigned char *)BOARD_BASE_FPGA_ADDR);

    j = *BOARD_PAYLOAD_RESET_REG;
    j |= 0x7;
    printf("setting board reset control to: %x\n", j );
    *BOARD_PAYLOAD_RESET_REG = j;

    /* pause briefly */
    for ( i = 0; i < 1000000; i++ );

    if (IS_SFP_PRESENT)
    {
        /* bring up the SFPs */
        sfpLinkSetup();
    }
    else if (IS_10G_PHY_PRESENT)
    {
        printf("Setting up phy at addr 0x%02x\n", BOARD_10G_PHY0_MDIO_ADDR);
        amcc10gPhyLinkSetup(BOARD_10G_PHY0_MDIO_ADDR);
        if (IS_10G_DUAL_PHY_PRESENT)
        {
            printf("Setting up phy at addr 0x%02x\n", BOARD_10G_PHY1_MDIO_ADDR);
            amcc10gPhyLinkSetup(BOARD_10G_PHY1_MDIO_ADDR);
        }

    }
    else /* we must have Marvell PHYs */
    {
        /* bring up the PHYs */
        phyLinkSetup();
    }

    printf("Heatsink Type: ");
    if (((*BOARD_ID_DIPSWITCH_REG) & 0x1) == 0x0)
    {
        printf("A\n");
        /* toggle fan controller control from FPGA to the MAX6653 */
        j = *BOARD_TEMP_FAN_STATUS;
        j |= 0x80; /* set the MSB */
        *BOARD_TEMP_FAN_STATUS = j;

        /* bring up the fan controller */
        if ( 0 != fanInit())
        {
            printf("failed to initialize fan controller\n");
            j = *BOARD_TEMP_FAN_STATUS;
            j &= 0x7F; /* clear the MSB, set control back to FPGA */
            *BOARD_TEMP_FAN_STATUS = j;
        }
    }
    else
    {
        printf("P\n");
    }

    local = octeon_twsi_read8( BOARD_TEMP_SENSOR_TWSI_ADDR,
                               BOARD_TEMP_SENSOR_LOCAL_ADDR );
    remote = octeon_twsi_read8( BOARD_TEMP_SENSOR_TWSI_ADDR,
                                BOARD_TEMP_SENSOR_REMOTE_ADDR );
    printf("Board sensor: %d C, Cavium sensor: %d C\n", local, remote);

    uboot_version_num_store();
}

int octeon_post_error_store(uint8_t error_code)
{
    int loc;

    /* find location to store error code */
    for (loc = 0; loc < BOARD_POST_LOG_SIZE; loc++)
    {
        if (octeon_twsi_read8(BOARD_EEPROM_TWSI_ADDR, BOARD_POST_LOG_START + loc) == NO_POST_ERROR)
        {
            octeon_twsi_write8_addr16(BOARD_EEPROM_TWSI_ADDR, BOARD_POST_LOG_START + loc, error_code);
            return 0;
        }
    }
    return -1;
}

int octeon_post_error_retrieve(uint8_t *post_error_code_buf_ptr)
{
    int loc;

    /* retrieve the entire buffer */
    for (loc = 0; loc < BOARD_POST_LOG_SIZE; loc++)
    {
        post_error_code_buf_ptr[loc] = octeon_twsi_read8(BOARD_EEPROM_TWSI_ADDR, BOARD_POST_LOG_START + loc);
    }
    return 0;
}

int octeon_post_error_clear(void)
{
    int loc;

    /* clear the entire buffer */
    for (loc = 0; loc < BOARD_POST_LOG_SIZE; loc++)
    {
        octeon_twsi_write8_addr16(BOARD_EEPROM_TWSI_ADDR, BOARD_POST_LOG_START + loc, NO_POST_ERROR);
    }
    return 0;
}

int do_temp_show(void)
{
    printf("Board sensor:  %d C\n", octeon_twsi_read8(BOARD_TEMP_SENSOR_TWSI_ADDR, BOARD_TEMP_SENSOR_LOCAL_ADDR));
    printf("Cavium sensor: %d C\n", octeon_twsi_read8(BOARD_TEMP_SENSOR_TWSI_ADDR, BOARD_TEMP_SENSOR_REMOTE_ADDR));

    return 0;
}

/**********************************************************
 ******  The following code is for boards with 1GigE SFP's. ******
 *********************************************************/
int do_sfp_show(void)
{
    int interface;
	uint8_t sfpType = 0;

    if (IS_SFP_PRESENT)
    {

        for (interface = 0; interface < MAX_NUM_SFP; interface++)
        {
            printf ("Interface %d:\n", interface);
            switch (octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, ID))
            {
            case 0:
                printf ("    ID: Unknown or unspecified\n");
                break;
            case 1:
                printf ("    ID: GBIC\n");
                break;
            case 2:
                printf ("    ID: Module/connector soldered to motherboard\n");
                break;
            case 3:
                printf ("    ID: SFP transceiver\n");
                break;
            default:
                printf ("    ID: 0x%02x\n", octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, ID));
                break;
            }
            printf ("    Ext ID: 0x%02x\n", octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, EXT_ID));
            switch (octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, CONNECTOR))
            {
            case 0:
                printf ("    Connector: Unknown or unspecified\n");
                break;
            case 1:
                printf ("    Connector: SC\n");
                break;
            case 2:
                printf ("    Connector: Fiber Channel Style 1 copper connector\n");
                break;
            case 3:
                printf ("    Connector: Fiber Channel Style 2 copper connector\n");
                break;
            case 4:
                printf ("    Connector: BNC/TNC\n");
                break;
            case 5:
                printf ("    Connector: Fiber Channel coaxial headers\n");
                break;
            case 6:
                printf ("    Connector: Fiber Jack\n");
                break;
            case 7:
                printf ("    Connector: LC\n");
                break;
            case 8:
                printf ("    Connector: MT-RJ\n");
                break;
            case 9:
                printf ("    Connector: MU\n");
                break;
            case 10:
                printf ("    Connector: SG\n");
                break;
            case 11:
                printf ("    Connector: Optical Pigtail\n");
                break;
            case 32:
                printf ("    Connector: HSSDC II\n");
                break;
            case 33:
                printf ("    Connector: Copper Pigtail\n");
                break;
            default:
                printf ("    Connector: 0x%02x\n", octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, CONNECTOR));
                break;
            }
            printf ("    Link Length: 9/125 mm Fiber (units of kilometers) - 0x%02x\n",
                    octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, FIBER_9_125MM_UNITS_KM));
            printf ("    Link Length: 9/125 mm Fiber (units of 100 meters) - 0x%02x\n",
                    octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, FIBER_9_125MM_UNITS_100M));
            printf ("    Link Length: 50/125 mm Fiber (units of 10 meters) - 0x%02x\n",
                    octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, FIBER_50_125MM_UNITS_10M));
            printf ("    Link Length: 62.5/125 mm Fiber (units of 10 meters) - 0x%02x\n",
                    octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, FIBER_62P5_125MM_UNITS_10M));
            printf ("    Link Length: Copper (units of meters) - 0x%02x\n",
                    octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, COPPER_UNITS_M));
        }
    }
    else if (IS_10G_PHY_PRESENT)
    {
        sfpType = SFP_TYPE(octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + BOARD_10G_PHY0_MDIO_ADDR, TRANSCEIVER_6));
        printf ("PHY%d SFP+ Type: %s\n", BOARD_10G_PHY0_MDIO_ADDR, SFP_STRING(sfpType));
        
        if (IS_10G_DUAL_PHY_PRESENT)
        {
            sfpType = SFP_TYPE(octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + BOARD_10G_PHY1_MDIO_ADDR, TRANSCEIVER_6));
            printf ("PHY%d SFP+ Type: %s\n", BOARD_10G_PHY1_MDIO_ADDR, SFP_STRING(sfpType));      
        }                                                                                       
    }
    else
    {
        printf("SFP's not supported on this board\n");
    }
    return(0);
}

void sfpVSRegWrite(int dev, int reg, int mask, int newVal, int reset_wait)
{
    int data;

    data = octeon_twsi_read8(dev, SFP_VS_BASE + reg);
    data = octeon_twsi_read8(dev, SFP_VS_BASE + reg) | (data << 8);
    data = (data & mask) | newVal;
    if (reset_wait)
    {
        octeon_twsi_write8_addr16 (dev, SFP_VS_BASE + reg, (data & 0x7FFF) >> 8);
        octeon_twsi_write8_addr16 (dev, SFP_VS_BASE + reg, data & 0x00FF);
        octeon_twsi_write8_addr16 (dev, SFP_VS_BASE + reg, data >> 8);
        octeon_twsi_write8_addr16 (dev, SFP_VS_BASE + reg, data & 0x00FF);
        do
        {
            data = octeon_twsi_read8(dev, SFP_VS_BASE + reg);
            data = octeon_twsi_read8(dev, SFP_VS_BASE + reg) | (data << 8);
        }
        while ((data & 0x8000) != 0);
    }
    else
    {
        octeon_twsi_write8_addr16 (dev, SFP_VS_BASE + reg, data >> 8);
        octeon_twsi_write8_addr16 (dev, SFP_VS_BASE + reg, data & 0x00FF);
    }
}

void sfpVSRegRead(int dev, int reg, int *val)
{
    int data;

    data = octeon_twsi_read8(dev, SFP_VS_BASE + reg);
    data = octeon_twsi_read8(dev, SFP_VS_BASE + reg) | ((data) << 8);
    *val = data;
}

void sfpInit(int interface)
{
    uint64_t pcs_misc_control_reg;
    uint64_t pcs_mrx_control_reg;

    connector_type[interface] = octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + interface, CONNECTOR);

    if (connector_type[interface] == VACANT)
    {
        printf ("V ");
    }
    else if (connector_type[interface] == UNSPECIFIED)
    {
        printf ("C ");

        /* command SFPs/M88E1111 into SGMII mode */
        sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + interface, EXTENDED_PHY_SPECIFIC_STATUS_REG, 0x0000, 0x9084, 0);
        /* command SFPs/M88E1111 to advertise 1000BASE-T Full/Half-Duplex */
        sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + interface, CONTROL_REG_1000BASE_T, 0x0000, 0x0F00, 0);
        /* command SFPs/M88E1111 to advertise 100/10BASE-T Full/Half-Duplex */
        sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + interface, AUTONEG_ADVERTISEMENT, 0x0000, 0x0DE1, 0);
        /* command SFPs into reset */
        sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + interface, CONTROL_REG, 0x0000, 0x9140, 1);
        /* command SFPs/M88E1111 to advertise 100/10BASE-T Full/Half-Duplex */
        sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + interface, AUTONEG_ADVERTISEMENT, 0x0000, 0x0DE1, 0);
    }
    else
    {
        printf ("F ");

        pcs_misc_control_reg = 0;
        pcs_mrx_control_reg = 0;

        switch (interface)
        {
        
        case 0:

            pcs_misc_control_reg = 0x11800B8001078ull;
            pcs_mrx_control_reg = 0x11800B8001000ull;
            break;
        case 1:

            pcs_misc_control_reg = 0x11800B8001478ull;
            pcs_mrx_control_reg = 0x11800B8001400ull;
            break;
        case 2:
            pcs_misc_control_reg = 0x11800B8001878ull;
            pcs_mrx_control_reg = 0x11800B8001800ull;
            break;
        case 3:
            pcs_misc_control_reg = 0x11800B8001C78ull;
            pcs_mrx_control_reg = 0x11800B8001C00ull;
            break;

        }

        /* check to make sure we assigned an address above */
        if (pcs_misc_control_reg != 0)
        {
            uint64_t data;

            data = cvmx_read_csr(CVMX_ADD_IO_SEG(pcs_misc_control_reg));
            /* setup PCS for 1000-Basex*/
            cvmx_write_csr(CVMX_ADD_IO_SEG(pcs_misc_control_reg), (data|0x100));

        }
    }    
    return;
}

int do_sfp_init(void)
{
    if (IS_SFP_PRESENT)
    {
        /* initialize SFPs */
        printf ("SFP Type: ");
        sfpInit (0);
        sfpInit (1);
        sfpInit (2);
        sfpInit (3);
        printf ("\n");
    }
    else if (IS_10G_PHY_PRESENT)
    {
        printf("Setting up phy at addr 0x%02x\n", BOARD_10G_PHY0_MDIO_ADDR);
        amcc10gPhyLinkSetup(BOARD_10G_PHY0_MDIO_ADDR);
        if (IS_10G_DUAL_PHY_PRESENT)
        {
            printf("Setting up phy at addr 0x%02x\n", BOARD_10G_PHY1_MDIO_ADDR);
            amcc10gPhyLinkSetup(BOARD_10G_PHY1_MDIO_ADDR);
        }

    }
    else
    {
        printf("SFP's not supported on this board\n");
    }
    return(0);
}


/** 
 * Function to configure all 4 SFPs
 */
void sfpLinkSetup(void)
{
    printf ("SFP Type: ");
    sfpInit (0);
    sfpInit (1);
    sfpInit (2);
    sfpInit (3);
    printf ("\n");
}

/* use for forcing 10base-t for regression testing */
int sfp_force_10baset(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
{
    printf("Commanding all SFPs to base 10T...");
    /* command SFPs/M88E1111 to advertise 10BASE-T Full/Half-Duplex */
    sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR , AUTONEG_ADVERTISEMENT, 0x0000, 0x0C61, 0);
    sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + 1, AUTONEG_ADVERTISEMENT, 0x0000, 0x0C61, 0);
    sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + 2, AUTONEG_ADVERTISEMENT, 0x0000, 0x0C61, 0);
    sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + 3, AUTONEG_ADVERTISEMENT, 0x0000, 0x0C61, 0);

    /* command SFPs into reset */
    sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR , CONTROL_REG, 0x0000, 0x9140, 1);
    sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + 1, CONTROL_REG, 0x0000, 0x9140, 1);
    sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + 2, CONTROL_REG, 0x0000, 0x9140, 1);
    sfpVSRegWrite (BOARD_SFP0_TWSI_ADDR + 3, CONTROL_REG, 0x0000, 0x9140, 1);

    printf("done\n");

    return 0;
}

/**********************************************************
 ******  The following code is for boards with 10G PHYs.***
 *********************************************************/
int amcc10gPhyLinkSetup(uint8_t  mdio_addr)
{
    uint16_t data;
    uint16_t fw_major, fw_minor, fw_bugfix;
    uint16_t fw_year, fw_month, fw_day;
    int rc;
    int timeout;
    uint16_t data_last;
    uint16_t heartbeat;
	uint8_t sfpType = 0;

    rc = 0;
    timeout = 1000; /*ms*/
    heartbeat = 0;
    data_last = 0;/* clear it out before we start */

	sfpType = SFP_TYPE(octeon_twsi_read8(BOARD_SFP0_TWSI_ADDR + mdio_addr, TRANSCEIVER_6));

    printf ("SFP+ Type: %s", SFP_STRING(sfpType));

	uint16_t opMode = (sfpType == SFP_FIBER) ? 0x0008 : (sfpType == SFP_DIRECT_ATTACHED) ? 0x0020 : 0x0000;

    /* Send down the MDIO start commands to the AMCC phy */

    /* issue a initial soft reset to PHY */                       
    phyRegWrite(mdio_addr/*phy addr*/, QT2025_PMA_PMD_CTRL1_REG/*reg*/, 0xFFFF /*mask*/, 0x8000/*data*/, 0, AMCC_DEV_ADDR_PMA_PMD);
    
    /* hold the internal Phy's 8051 in reset during initial MDIO command set */
    phyRegWrite(mdio_addr, QT2025_PMA_MDIO_RESETS_REG, 0x0000 /*mask*/, 0x0000, 0, AMCC_DEV_ADDR_PMA_PMD);

    /* setup the internal microconroller config register, uc clock and sync error ref clock */
    phyRegWrite(mdio_addr, QT2025_PMA_UC8051_CFG_REG, 0x0000 /*mask*/, 0x0004 /*UC_CLK_SEL=156.25*/, 0, AMCC_DEV_ADDR_PMA_PMD);

    /* set the PMA i2c slave address where eeprom is connected to internal microcontroller */
    phyRegWrite(mdio_addr, QT2025_PMA_I2C_SLAVE_ADDR_REG, 0x0000 /*mask*/, 0x0050, 0, AMCC_DEV_ADDR_PMA_PMD);

    /* fill PMA spare 2 reg, oper mode, and bit rate selection */
    phyRegWrite(mdio_addr, QT2025_PMA_UC8051_SPARE_2_REG, 0x0000 /*mask*/, opMode, 0, AMCC_DEV_ADDR_PMA_PMD);

    /* fill PMA spare 3 reg, sfp+ module type, FTX and XDRV overrides */
    phyRegWrite(mdio_addr, QT2025_PMA_UC8051_SPARE_3_REG, 0x0000 /*mask*/, 0x0098, 0, AMCC_DEV_ADDR_PMA_PMD);

    /* set 10G seed 0 register, freeze point control */
    phyRegWrite(mdio_addr, QT2025_PCS_10G_R_SEED_B0_REG, 0x0000 /*mask*/, 0x0E00, 0, AMCC_DEV_ADDR_PCS);

    /* when in reset this is BOOT_CTRL_REG, sets the FTX and FRX FR4 loss, setup hardware LED control*/
    phyRegWrite(mdio_addr, QT2025_PCS_10G_R_SEED_B1_REG, 0x0000 /*mask*/, 0x0013, 0, AMCC_DEV_ADDR_PCS);

    /* set 10G seed 2 register, boot key */
    phyRegWrite(mdio_addr, QT2025_PCS_10G_R_SEED_B2_REG, 0x0000 /*mask*/, 0xA528, 0, AMCC_DEV_ADDR_PCS);

    /* invert the Tx polarity */
    phyRegWrite(mdio_addr, QT2025_PMA_PMD_MODE_REG, ~0x0080 /*mask*/, 0x80, 0, AMCC_DEV_ADDR_PMA_PMD);

    /* NOTE: LED settings will not work unless reg 3.0027h LED1_CTRL=1 */
    /* set the Activity LED to blink on activity only */
    phyRegWrite(mdio_addr, QT2025_GPIO_LED1_REG, 0x0000 /*mask*/, 0x0002, 0, AMCC_DEV_ADDR_PMA_PMD);

    /* set the Link LED to report link state only */
    phyRegWrite(mdio_addr, QT2025_GPIO_LED2_REG, 0x0000 /*mask*/, 0x0009, 0, AMCC_DEV_ADDR_PMA_PMD);
    /****** end LED setup ******/

    /* turn off pre-emphasis if it's not already disabled */
    phyRegRead(mdio_addr, QT2025_XDRV_CTRL_5_REG, &data, AMCC_DEV_ADDR_XGXS);

    if ((data & 0xFF) != 0xF0)
    {
        printf("\nQt2025: Turning Off Pre-emphasis");
        /* turn pre-emphasis off */
        phyRegWrite(mdio_addr, QT2025_XDRV_CTRL_5_REG, 0x0000 /*mask*/, 0xF0, 0, AMCC_DEV_ADDR_XGXS);
    }
    /* 
     * release micro controller from reset, allow controller to boot out of eeprom
     * check for heart beat at this time 
     */
    phyRegWrite(mdio_addr, QT2025_PMA_MDIO_RESETS_REG, 0x0000 /*mask*/, 0x0002, 0, AMCC_DEV_ADDR_PMA_PMD);

    while (timeout && (heartbeat < AMCC_HEARTBEAT_THRESHOLD) )
    {
        /* check the heartbeat reg*/
        phyRegRead(mdio_addr, QT2025_PCS_REG_FW_HEARTB, &data, AMCC_DEV_ADDR_PCS);
        /* compare and see if the heartbeat register is changing*/
        if (data != data_last)
        {
            heartbeat++;
        }
        
        data_last = data;
        timeout--;
        udelay(1000);/* sleep for 1 ms before we try again */
    }

    if (heartbeat < AMCC_HEARTBEAT_THRESHOLD)
    {
        printf("\n\n***Qt2025 PHY[%d] ERROR!!! Phy did not start!***\n", mdio_addr);
        rc = -1;
    }

    if (!rc)
    {
        timeout = 50; /*ms x 100*/
        do
        {
            /* check the status reg to see if the firmware is fully booted */
            phyRegRead(mdio_addr, QT2025_PCS_REG_UC_STATUS, &data, AMCC_DEV_ADDR_PCS);
            timeout--;
            udelay(100000);/* sleep for 100 ms before we try again */

        }while (timeout && (data == 0x00 || data == 0x10 || data == 0x20 || data == 0x40 ));

        /* use this to determine if the phy is booted */
        if (timeout)/* data should be 0x30 || 0x50 || 0x60 || 0x70 */
        {
            /* get date of FW version */
            phyRegRead(mdio_addr, QT2025_PCS_FW_BUILD1_REG, &fw_year, AMCC_DEV_ADDR_PCS);
            phyRegRead(mdio_addr, QT2025_PCS_FW_BUILD2_REG, &fw_month, AMCC_DEV_ADDR_PCS);
            phyRegRead(mdio_addr, QT2025_PCS_FW_BUILD3_REG, &fw_day, AMCC_DEV_ADDR_PCS);

            /* check to see if we need to apply the finisar workaround */
            if ( (fw_year == 9 && (fw_month < 4 || (fw_month == 4 && fw_day < 23))) || (fw_year < 9) )/* if date of build is < 4/23/2009 */
            {   
                printf("\nQt2025 Applying clock phase workaround.");
                /**
                 * Workaround for Finisar SFP+s not working with loopback plugs. 
                 * This should not affect any other SFP+ actions. 
                 */
                /* set phase to work with Finisar SFP+s */
                phyRegWrite(mdio_addr, QT2025_PCS_REG_PHASE_REG, 0x0000 /*mask*/, 0xFA, 0, AMCC_DEV_ADDR_PCS);
                /* read LOS state: RX_LOS; 1=LOS 0=Signal OK */
                phyRegRead(mdio_addr, QT2025_PMA_PMD_PAD_IN_STATUS_REG, &data, AMCC_DEV_ADDR_PMA_PMD);
                
                /* if link up */
                if ((data & 0x04) == 0)
                {
                    printf("\nQt2025 forcing link down to apply clock phase workaround.");
                    /* Force link down*/
                    phyRegWrite(mdio_addr, QT2025_PMA_PMD_PAD_IN_INV_REG, 0x0000 /*mask*/, 0x04, 0, AMCC_DEV_ADDR_PMA_PMD);
                    udelay(100000);/* sleep for 100 ms to give firmware time to respond to LOS condition */
                    /* bring link back up */
                    phyRegWrite(mdio_addr, QT2025_PMA_PMD_PAD_IN_INV_REG, 0x0000 /*mask*/, 0x00, 0, AMCC_DEV_ADDR_PMA_PMD);
                }
                /******* end workaround *******/ 
            }
            

            /* get major, minor, bugfix FW version */
            phyRegRead(mdio_addr, QT2025_PCS_FW_VERSION1_REG, &fw_major, AMCC_DEV_ADDR_PCS);
            phyRegRead(mdio_addr, QT2025_PCS_FW_VERSION2_REG, &fw_minor, AMCC_DEV_ADDR_PCS);
            phyRegRead(mdio_addr, QT2025_PCS_FW_VERSION3_REG, &fw_bugfix, AMCC_DEV_ADDR_PCS);
            if (fw_major == 255)/* early release versioning scheme */
            {
                printf ("\nQt2025 Phy%d is running, Version %d.%d.%d ", mdio_addr, fw_major, fw_minor, fw_bugfix);
            }
            else /* new versioning scheme */
            {
                printf ("\nQt2025 Phy%d is running, Version %d.%d.%d.%d ", 
                        mdio_addr, fw_major >> 4/*upper nibble*/, fw_major & 0x0F /* lower nibble */, fw_minor, fw_bugfix);
            }
            printf("%d/%d/%02d", fw_month, fw_day, fw_year); /* print out the date stamp of the firmware build */

        }
        else
        {
            printf("\nUnexpected Qt2025 Phy%d error! UC_STATUS:0x%02x", mdio_addr, data);
        }
    }
    printf ("\n");
    return rc;
}

int amcc_is_phy_alive(uint8_t mdio_addr)
{
    uint16_t data;
    int rc;
    int timeout;

    rc = 0;     
    timeout = 100; /*ms x 100*/

    do
    {
        /* check the status reg to see if the firmware is fully booted */
        phyRegRead(mdio_addr, QT2025_PCS_REG_UC_STATUS, &data, AMCC_DEV_ADDR_PCS);
        timeout--;
        udelay(100000);/* sleep for 100 ms before we try again */

    }while (timeout && (data == 0x00 || data == 0x10 || data == 0x20 || data == 0x40 ));

    /* use this to determine if the SFP+ is there */
    if ( data < 0x40 && data != 0x30)
    {
        rc = -1;
    }


    return rc;
}


/**********************************************************
 ******  The following code is for boards with PHYs. ******
 *********************************************************/

/** 
 * Function to bring up and configure a Marvel M88E1111 PHY
 * 
 * @param addr
 */
void phyInit(uint8_t addr)

{
    uint16_t fVal = 0xFFFF, cVal = 0xFFFF;

    printf ("C ");

    /* command SFPs/M88E1111 into SGMII mode */
    phyRegWrite (addr, EXTENDED_PHY_SPECIFIC_STATUS_REG, 0x0000, 0x9484, 0, CLAUSE22_NOT_USED);

    /* command SFPs/M88E1111 to advertise 1000BASE-T Full/Half-Duplex */
    phyRegWrite (addr, CONTROL_REG_1000BASE_T, 0x0000, 0x0F00, 0, CLAUSE22_NOT_USED);

    /* command SFPs/M88E1111 to advertise 100/10BASE-T Full/Half-Duplex */
    phyRegWrite (addr, AUTONEG_ADVERTISEMENT, 0x0000, 0x0DE1, 0, CLAUSE22_NOT_USED);

    /* command SFPs/M88E1111 into reset */
    phyRegWrite (addr, CONTROL_REG, 0x0000, 0x9140, 1, CLAUSE22_NOT_USED);

    /* command SFPs/M88E1111 to advertise 100/10BASE-T Full/Half-Duplex */
    phyRegWrite (addr, AUTONEG_ADVERTISEMENT, 0x0000, 0x0DE1, 0, CLAUSE22_NOT_USED);

    /* command SFPs/M88E1111 to blink tx/rx LED's on activity and solid ON for link up */
    phyRegWrite (addr, LED_CONTROL_REG, 0xFFFF, 0x0003, 0, CLAUSE22_NOT_USED);

    /* Clear and Disable PHY interrupts */

    while ( fVal != 0x0000 )
    {
        phyRegWrite( addr, 22, 0xFF00, 0x0001, 0, CLAUSE22_NOT_USED );
        phyRegRead( addr, 19, &fVal, CLAUSE22_NOT_USED );
    }

    phyRegWrite( addr, 18, 0x0000, 0x0000, 0 , CLAUSE22_NOT_USED);

    while ( cVal != 0x0000 )
    {
        phyRegWrite( addr, 22, 0xFF00, 0x0000, 0, CLAUSE22_NOT_USED );
        phyRegRead( addr, 19, &cVal, CLAUSE22_NOT_USED );
    }

    phyRegWrite( addr, 18, 0x0000, 0x0000, 0, CLAUSE22_NOT_USED );

    return;                           
}

/** 
 * Function to configure all 4 Marvel M88E1111 PHYs
 */
void phyLinkSetup(void)
{
    printf("PHY Type: ");
    phyInit(BOARD_PHY0_MDIO_ADDR);
    phyInit(BOARD_PHY1_MDIO_ADDR);
    phyInit(BOARD_PHY2_MDIO_ADDR);
    phyInit(BOARD_PHY3_MDIO_ADDR);
    printf("\n");
}


int getProcessorStr(char *modelStr, int modelStrSize)
{
    char processorStr[30];
    int rc;

    memset(processorStr, 0x00, sizeof(processorStr));
    memset(modelStr, 0x00, modelStrSize);

    rc = -1;

    /* we need 4 chars for the model string and 1 null terminator */
    if (modelStrSize > 4)
    {
        octeon_model_get_string_buffer(cvmx_get_proc_id(), processorStr);
        strncpy(modelStr, &processorStr[2], 4);/* only copy the 4 digit processor number */
        //* I shouldn't need the below code
        /* this is a TEMP patch b/c the 5650 pass 1 is reporting as a 5750 b/c of the Raid enable fuse */
        if (modelStr[1] == '7')
        {
            modelStr[1] = '6';

        }

        rc = 0;
    }
    else
    {
        strncpy(modelStr,"ERROR", modelStrSize);
    }


    return rc;
}

int getCommandPromptStr(char *cmdPromptStr, int cmdPromptStrSize)
{
    char tempStr[CFG_MAX_PROMPT_SIZE];
    char processorIDStr[10];
    int rc;

    memset(tempStr, 0x00, sizeof(tempStr));
    memset(processorIDStr, 0x00, sizeof(processorIDStr));
    memset(cmdPromptStr, 0x00, sizeof(cmdPromptStrSize));

    rc = -1;

    if (cmdPromptStrSize > 49)
    {
        getProcessorStr(processorIDStr,sizeof(processorIDStr));

        if (!strcmp(processorIDStr, "5650"))
        {
            /* check to see if this is a 10G SFP+ card */
            if (IS_10G_PHY_PRESENT)
            {
                strcpy(processorIDStr, "5651X");
            }
        }

#ifdef CONFIG_OCTEON_FAILSAFE
        sprintf(tempStr, "Octeon W%s Failsafe bootloader# ", processorIDStr);
#else
#if CONFIG_RAM_RESIDENT
        sprintf(tempStr, "Octeon W%s(ram)# ", processorIDStr);
#else
        sprintf(tempStr, "Octeon W%s# ", processorIDStr);
#endif/* CONFIG_RAM_RESIDENT */
#endif/* CONFIG_OCTEON_FAILSAFE */

        if (strlen(tempStr) < cmdPromptStrSize)
        {
            strncpy(cmdPromptStr,tempStr, strlen(tempStr)); 
            rc = 0;
        }
        else
        {
            strncpy(cmdPromptStr, "BUFFER OVERRUN in getCommandPromptStr# ",MIN(strlen("BUFFER OVERRUN in getCommandPromptStr# "),cmdPromptStrSize)); 
        }
    }

    return rc;
}


/**
 * Perform an MII read. This function is used to read PHY
 * registers controlling auto negotiation.
 *
 * @param bus_id   MDIO bus number. Zero on most chips, but some chips (ex CN56XX)
 *                 support multiple busses.
 * @param phy_id   The MII phy id
 * @param location Register location to read
 *
 * @return Result from the read or -1 on failure
 */
int cvmx_mdio_read_ieee_clause45(int bus_id, int phy_addr, uint8_t dev_type)
{
    cvmx_smix_cmd_t smi_cmd;
    cvmx_smix_rd_dat_t smi_rd;
    int timeout = 1000;
    unsigned char phy_op;

    phy_op = 0x03;//(0x00 = address; 0x01 = write, 0x11 = read)

    smi_cmd.u64 = 0;
    smi_cmd.s.phy_op = phy_op;
    smi_cmd.s.phy_adr = phy_addr;
    smi_cmd.s.reg_adr = dev_type;
    cvmx_write_csr(CVMX_SMIX_CMD(bus_id), smi_cmd.u64);

    do
    {
        cvmx_wait(1000);
        smi_rd.u64 = cvmx_read_csr(CVMX_SMIX_RD_DAT(bus_id));
    } while (smi_rd.s.pending && timeout--);

    if (!timeout)
    {
        printf("*** TIMEOUT waiting for CVMX_SMIX_RD_DAT\n");
    }
    if (smi_rd.s.val)
        return smi_rd.s.dat;
    else
        return -1;
}
/**
 * Perform an MII write. This function is used to write PHY
 * registers controlling auto negotiation.
 *
 * @param bus_id   MDIO bus number. Zero on most chips, but some chips (ex CN56XX)
 *                 support multiple busses.
 * @param phy_id   The MII phy id
 * @param location Register location to write
 * @param val      Value to write
 *
 * @return -1 on error
 *         0 on success
 */
int cvmx_mdio_address_write_ieee_clause45(unsigned char phy_op, int bus_id, int phy_addr, uint8_t dev_type, uint16_t data)
{
    cvmx_smix_cmd_t smi_cmd;
    cvmx_smix_wr_dat_t smi_wr;
    int timeout = 1000;

    smi_wr.u64 = 0;
    smi_wr.s.dat = data;/* for writes this will be the data. For address commands this will be the address */
    cvmx_write_csr(CVMX_SMIX_WR_DAT(bus_id), smi_wr.u64);

    smi_cmd.u64 = 0;
    smi_cmd.s.phy_op = (phy_op & 0x03);/*phy_op is only two bits so we mask it.(0x00 = address; 0x01 = write, 0x11 = read)*/
    smi_cmd.s.phy_adr = phy_addr;
    smi_cmd.s.reg_adr = dev_type;
    cvmx_write_csr(CVMX_SMIX_CMD(bus_id), smi_cmd.u64);

    do
    {
        cvmx_wait(1000);
        smi_wr.u64 = cvmx_read_csr(CVMX_SMIX_WR_DAT(bus_id));
    } while (smi_wr.s.pending && --timeout);
    if (timeout <= 0)
        return -1;

    return 0;
}



