xemacpsif_physpeed.c

/*
 * Copyright (c) 2007-2008, Advanced Micro Devices, Inc.
 *               All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *    * Redistributions of source code must retain the above copyright
 *      notice, this list of conditions and the following disclaimer.
 *    * Redistributions in binary form must reproduce the above copyright
 *      notice, this list of conditions and the following disclaimer in
 *      the documentation and/or other materials provided with the
 *      distribution.
 *    * Neither the name of Advanced Micro Devices, Inc. nor the names
 *      of its contributors may be used to endorse or promote products
 *      derived from this software without specific prior written
 *      permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * Some portions copyright (c) 2010-2013 Xilinx, Inc.  All rights reserved.
 *
 * Xilinx, Inc.
 * XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS" AS A
 * COURTESY TO YOU.  BY PROVIDING THIS DESIGN, CODE, OR INFORMATION AS
 * ONE POSSIBLE   IMPLEMENTATION OF THIS FEATURE, APPLICATION OR
 * STANDARD, XILINX IS MAKING NO REPRESENTATION THAT THIS IMPLEMENTATION
 * IS FREE FROM ANY CLAIMS OF INFRINGEMENT, AND YOU ARE RESPONSIBLE
 * FOR OBTAINING ANY RIGHTS YOU MAY REQUIRE FOR YOUR IMPLEMENTATION.
 * XILINX EXPRESSLY DISCLAIMS ANY WARRANTY WHATSOEVER WITH RESPECT TO
 * THE ADEQUACY OF THE IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO
 * ANY WARRANTIES OR REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE
 * FROM CLAIMS OF INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 */

#include "netif/xemacpsif.h"
#include "lwipopts.h"
#include "xparameters_ps.h"
#include "xparameters.h"

/*** IMPORTANT: Define PEEP in xemacpsif.h and sys_arch_raw.c
 *** to run it on a PEEP board
 ***/
 

/* Start Micrel KSZ9031NRX PHY definitions added to support the Z-turn board.*/

#define PHY_MIC_IDENTIFIER		0x0022

/* End Micrel KSZ9031NRX PHY definitions */

/* Start Lantiq PHY11G PHY definitions added to support the RedPitaya board.*/

#define PHY_LANTIQ_IDENTIFIER	0xd565
	
#define MMD_CTRL	0x0d
#define MMD_DATA	0x0e

#define MII_CTRL	0x17
#define MII_STAT	0x18

#define MII_IMASK	0x19	/* interrupt mask */
#define MII_ISTAT	0x1a	/* interrupt status */

#define GCTRL		0x09	/* Gigabit Control Register */
#define GSTAT		0x0A 	/* Gigabit Status Register */

#define INT_WOL		BIT(15)	/* Wake-On-LAN */
#define INT_ANE		BIT(11)	/* Auto-Neg error */
#define INT_ANC		BIT(10)	/* Auto-Neg complete */
#define INT_ADSC	BIT(5)	/* Link auto-downspeed detect */
#define INT_DXMC	BIT(2)	/* Duplex mode change */
#define INT_LSPC	BIT(1)	/* Link speed change */
#define INT_LSTC	BIT(0)	/* Link state change */
#define INT_MASK	(INT_LSTC | INT_ADSC)

#define ADVERTISED_MPD	0x0400	/* Multi-port device */

#define MMD_ACTYPE_SHIFT		14
#define MMD_ACTYPE_ADDRESS		(0 << MMD_ACTYPE_SHIFT)
#define MMD_ACTYPE_DATA			(1 << MMD_ACTYPE_SHIFT)
#define MMD_ACTYPE_DATA_PI		(2 << MMD_ACTYPE_SHIFT)
#define MMD_ACTYPE_DATA_PIWR	(3 << MMD_ACTYPE_SHIFT)

#define MDIO_ADDR_MAX   0x1f
#define MDIO_VAL_MAX    0xffff

#define MMD_DEVAD		0x1f
/* MMD LED configuration registers */
#define INT_LED0H 		0x01e2
#define INT_LED0L 		0x01e3
#define INT_LED1H 		0x01e4
#define INT_LED1L 		0x01e5
#define INT_LED2H 		0x01e6
#define INT_LED2L 		0x01e7

/* End Lantiq PHY11G PHY definitions */

/* LED function bits */
#define INT_LED_TX 0x01
#define INT_LED_RX 0x02
#define INT_LED_LINK10 0x1
#define INT_LED_LINK100 0x2
#define INT_LED_LINK1000 0x4

/* Advertisement control register. */
#define ADVERTISE_10HALF		0x0020  /* Try for 10mbps half-duplex  */
#define ADVERTISE_10FULL		0x0040  /* Try for 10mbps full-duplex  */
#define ADVERTISE_100HALF		0x0080  /* Try for 100mbps half-duplex */
#define ADVERTISE_100FULL		0x0100  /* Try for 100mbps full-duplex */

#define ADVERTISE_100_AND_10	(ADVERTISE_10FULL | ADVERTISE_100FULL | \
								ADVERTISE_10HALF | ADVERTISE_100HALF)
#define ADVERTISE_100			(ADVERTISE_100FULL | ADVERTISE_100HALF)
#define ADVERTISE_10			(ADVERTISE_10FULL | ADVERTISE_10HALF)

#define ADVERTISE_1000			0x0300


#define IEEE_CONTROL_REG_OFFSET				0
#define IEEE_STATUS_REG_OFFSET				1
#define IEEE_AUTONEGO_ADVERTISE_REG			4
#define IEEE_PARTNER_ABILITIES_1_REG_OFFSET	5
#define IEEE_1000_ADVERTISE_REG_OFFSET		9
#define IEEE_PARTNER_ABILITIES_3_REG_OFFSET	10
#define IEEE_COPPER_SPECIFIC_CONTROL_REG	16
#define IEEE_SPECIFIC_STATUS_REG			17
#define IEEE_COPPER_SPECIFIC_STATUS_REG_2	19
#define IEEE_CONTROL_REG_MAC				21
#define IEEE_PAGE_ADDRESS_REGISTER			22


#define IEEE_CTRL_1GBPS_LINKSPEED_MASK		0x2040
#define IEEE_CTRL_LINKSPEED_MASK			0x0040
#define IEEE_CTRL_LINKSPEED_1000M			0x0040
#define IEEE_CTRL_LINKSPEED_100M			0x2000
#define IEEE_CTRL_LINKSPEED_10M				0x0000
#define IEEE_CTRL_RESET_MASK				0x8000
#define IEEE_CTRL_AUTONEGOTIATE_ENABLE		0x1000
#if XPAR_GIGE_PCS_PMA_CORE_PRESENT == 1
#define IEEE_CTRL_RESET                         0x9140
#define IEEE_CTRL_ISOLATE_DISABLE               0xFBFF
#endif
#define IEEE_STAT_AUTONEGOTIATE_CAPABLE		0x0008
#define IEEE_STAT_AUTONEGOTIATE_COMPLETE	0x0020
#define IEEE_STAT_AUTONEGOTIATE_RESTART		0x0200
#define IEEE_STAT_1GBPS_EXTENSIONS			0x0100
#define IEEE_AN1_ABILITY_MASK				0x1FE0
#define IEEE_AN3_ABILITY_MASK_1GBPS			0x0C00
#define IEEE_AN1_ABILITY_MASK_100MBPS		0x0380
#define IEEE_AN1_ABILITY_MASK_10MBPS		0x0060
#define IEEE_RGMII_TXRX_CLOCK_DELAYED_MASK	0x0030

#define IEEE_ASYMMETRIC_PAUSE_MASK			0x0800
#define IEEE_PAUSE_MASK						0x0400
#define IEEE_AUTONEG_ERROR_MASK				0x8000

#define PHY_DETECT_REG  						1
#define PHY_IDENTIFIER_1_REG					2
#define PHY_DETECT_MASK 					0x1808

#define XEMACPS_GMII2RGMII_SPEED1000_FD		0x140
#define XEMACPS_GMII2RGMII_SPEED100_FD		0x2100
#define XEMACPS_GMII2RGMII_SPEED10_FD		0x100
#define XEMACPS_GMII2RGMII_REG_NUM			0x10

/* Frequency setting */
#define SLCR_LOCK_ADDR			(XPS_SYS_CTRL_BASEADDR + 0x4)
#define SLCR_UNLOCK_ADDR		(XPS_SYS_CTRL_BASEADDR + 0x8)
#define SLCR_GEM0_CLK_CTRL_ADDR	(XPS_SYS_CTRL_BASEADDR + 0x140)
#define SLCR_GEM1_CLK_CTRL_ADDR	(XPS_SYS_CTRL_BASEADDR + 0x144)
#ifdef PEEP
#define SLCR_GEM_10M_CLK_CTRL_VALUE		0x00103031
#define SLCR_GEM_100M_CLK_CTRL_VALUE	0x00103001
#define SLCR_GEM_1G_CLK_CTRL_VALUE		0x00103011
#endif
#define SLCR_LOCK_KEY_VALUE 			0x767B
#define SLCR_UNLOCK_KEY_VALUE			0xDF0D
#define SLCR_ADDR_GEM_RST_CTRL			(XPS_SYS_CTRL_BASEADDR + 0x214)
#define EMACPS_SLCR_DIV_MASK			0xFC0FC0FF

#define EMAC0_BASE_ADDRESS				0xE000B000
#define EMAC1_BASE_ADDRESS				0xE000C000

static int detect_phy(XEmacPs *xemacpsp)
{
	u16 phy_reg;
	u32 phy_addr;


	for (phy_addr = 31; phy_addr > 0; phy_addr--) {
		XEmacPs_PhyRead(xemacpsp, phy_addr, PHY_DETECT_REG,
							&phy_reg);

		if ((phy_reg != 0xFFFF) &&
			((phy_reg & PHY_DETECT_MASK) == PHY_DETECT_MASK)) {
			/* Found a valid PHY address */
			LWIP_DEBUGF(NETIF_DEBUG, ("XEmacPs detect_phy: PHY detected at address %d.\r\n",
																	phy_addr));
			LWIP_DEBUGF(NETIF_DEBUG, ("XEmacPs detect_phy: PHY detected.\r\n"));
			return phy_addr;
		}
	}

	LWIP_DEBUGF(NETIF_DEBUG, ("XEmacPs detect_phy: No PHY detected.  Assuming a PHY at address 0\r\n"));

        /* default to zero */
	return 0;
}

#ifdef PEEP
unsigned get_IEEE_phy_speed(XEmacPs *xemacpsp)
{

	u16 control;
	u16 status;
	u16 partner_capabilities;
	u16 partner_capabilities_1000;
	u16 phylinkspeed;
	u32 phy_addr = detect_phy(xemacpsp);

	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_1000_ADVERTISE_REG_OFFSET,
															ADVERTISE_1000);
	/* Advertise PHY speed of 100 and 10 Mbps */
	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_AUTONEGO_ADVERTISE_REG,
													ADVERTISE_100_AND_10);

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET,
																&control);
	control |= (IEEE_CTRL_AUTONEGOTIATE_ENABLE |
					IEEE_STAT_AUTONEGOTIATE_RESTART);

	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, control);

	/* Read PHY control and status registers is successful. */
	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, &control);
	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_STATUS_REG_OFFSET, &status);

	if ((control & IEEE_CTRL_AUTONEGOTIATE_ENABLE) && (status &
					IEEE_STAT_AUTONEGOTIATE_CAPABLE)) {

		while ( !(status & IEEE_STAT_AUTONEGOTIATE_COMPLETE) ) {
			XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_STATUS_REG_OFFSET,
																&status);
		}

		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_PARTNER_ABILITIES_1_REG_OFFSET,
															&partner_capabilities);

		if (status & IEEE_STAT_1GBPS_EXTENSIONS) {
			XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_PARTNER_ABILITIES_3_REG_OFFSET,
														&partner_capabilities_1000);
			if (partner_capabilities_1000 & IEEE_AN3_ABILITY_MASK_1GBPS)
				return 1000;
		}

		if (partner_capabilities & IEEE_AN1_ABILITY_MASK_100MBPS)
			return 100;
		if (partner_capabilities & IEEE_AN1_ABILITY_MASK_10MBPS)
			return 10;

		xil_printf("%s: unknown PHY link speed, setting TEMAC speed to be 10 Mbps\r\n",
				__FUNCTION__);
		return 10;

	} else {

		/* Update TEMAC speed accordingly */
		if (status & IEEE_STAT_1GBPS_EXTENSIONS) {
			/* Get commanded link speed */
			phylinkspeed = control & IEEE_CTRL_1GBPS_LINKSPEED_MASK;

			switch (phylinkspeed) {
				case (IEEE_CTRL_LINKSPEED_1000M):
					return 1000;
				case (IEEE_CTRL_LINKSPEED_100M):
					return 100;
				case (IEEE_CTRL_LINKSPEED_10M):
					return 10;
				default:
					xil_printf("%s: unknown PHY link speed (%d), setting TEMAC speed to be 10 Mbps\r\n",
							__FUNCTION__, phylinkspeed);
					return 10;
			}

		} else {

			return (control & IEEE_CTRL_LINKSPEED_MASK) ? 100 : 10;

		}
	}
}

#else /* Zynq */

/* Start Lantiq PHY11G PHY functions added to support the RedPitaya board.*/

u16_t phy_mmd_read(XEmacPs *xemacpsp,u16_t phy_addr, u16_t mmd_offset, u16_t regnum, u16_t val)
{
	XEmacPs_PhyWrite(xemacpsp, phy_addr, MMD_CTRL, MMD_ACTYPE_ADDRESS | mmd_offset);
	XEmacPs_PhyWrite(xemacpsp, phy_addr, MMD_DATA, regnum);
	XEmacPs_PhyWrite(xemacpsp, phy_addr, MMD_CTRL, MMD_ACTYPE_DATA | mmd_offset);

	XEmacPs_PhyRead(xemacpsp, phy_addr, MMD_DATA,&val);
	
	return 0;
}

u16_t phy_mmd_write(XEmacPs *xemacpsp,u16_t phy_addr, u16_t mmd_offset,u16_t regnum, u16_t val)
{
	XEmacPs_PhyWrite(xemacpsp, phy_addr, MMD_CTRL, MMD_ACTYPE_ADDRESS | mmd_offset);
	XEmacPs_PhyWrite(xemacpsp, phy_addr, MMD_DATA, regnum);
	XEmacPs_PhyWrite(xemacpsp, phy_addr, MMD_CTRL, MMD_ACTYPE_DATA | mmd_offset);
	XEmacPs_PhyWrite(xemacpsp, phy_addr, MMD_DATA, val);

	return 0;
}

static u32_t get_Lantiq_phy_speed(XEmacPs *xemacpsp, u32_t phy_addr)
{
	u16_t temp;
	u16_t control;
	u16_t status;
	u16_t status_speed;
	u32_t timeout_counter = 0;
	u32_t temp_speed;
	u16_t phyregtemp;

	/* Set LED0 blinking on rx/tx. */
	phy_mmd_write(xemacpsp,phy_addr, MMD_DEVAD, INT_LED0H, 0);		
	phy_mmd_write(xemacpsp,phy_addr, MMD_DEVAD, INT_LED0L, INT_LED_RX | INT_LED_TX);

	/* Set LED1 binking on link speed: slow=10M, fast=100M, on=1G. */
	phy_mmd_write(xemacpsp,phy_addr, MMD_DEVAD, INT_LED1H, INT_LED_LINK1000 << 4 | INT_LED_LINK100);			
	phy_mmd_write(xemacpsp,phy_addr, MMD_DEVAD, INT_LED1L, INT_LED_LINK10 << 4);

	/* Mask all interrupts */
	XEmacPs_PhyWrite(xemacpsp,phy_addr, MII_IMASK, 0);

	/* Clear all pending interrupts */
	XEmacPs_PhyRead(xemacpsp, phy_addr, MII_ISTAT,	&phyregtemp);

	/* Set SGMII RX & TX timing skew to 2 ns & 2.5 ns respectively. */
	/* Set MII power supply to 2V5. */
	XEmacPs_PhyWrite(xemacpsp, phy_addr, MII_CTRL, 0x4d00);
	/* Disable all 10M modes due to Xilinx EMACPS driver bug - #3120. */
	XEmacPs_PhyWrite(xemacpsp, phy_addr, 0x04, 0x0581);
	
	/* Advertise as multi-port device */
	XEmacPs_PhyRead(xemacpsp, phy_addr, GCTRL, &phyregtemp);
	phyregtemp |= ADVERTISED_MPD;
	XEmacPs_PhyWrite(xemacpsp, phy_addr, GCTRL, phyregtemp);

	xil_printf("Start Lantiq PHY11G PHY autonegotiation \r\n");
	
	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, &control);
	control |= IEEE_CTRL_AUTONEGOTIATE_ENABLE;
	control |= IEEE_STAT_AUTONEGOTIATE_RESTART;
	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, control);

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_STATUS_REG_OFFSET, &status);

	xil_printf("Waiting for Lantiq PHY11G PHY to complete autonegotiation.\r\n");

	while ( !(status & IEEE_STAT_AUTONEGOTIATE_COMPLETE) )
	{
		sleep(10);
		timeout_counter++;

		if (timeout_counter == 30)
		{
			xil_printf("Auto negotiation error \r\n");
			return;
		}
		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_STATUS_REG_OFFSET, &status);
	}
	xil_printf("autonegotiation complete \r\n");

	XEmacPs_PhyRead(xemacpsp, phy_addr,MII_STAT,&status_speed);

	temp_speed = status_speed & 0x3;

	if (temp_speed == 2)
		return 1000;
	else if(temp_speed == 1)
		return 100;
	else
		return 10;

	return XST_SUCCESS;
}
/* End Lantiq PHY11G PHY functions.*/

/* Start Micrel KSZ9031NRX PHY functions added to support the Z-turn board.*/

static u32_t get_Micrel_phy_speed(XEmacPs *xemacpsp, u32_t phy_addr)
{
	u16_t temp;
	u16_t control;
	u16_t status;
	u16_t status_speed;
	u32_t timeout_counter = 0;
	u32_t temp_speed;
	u32_t phyregtemp;
	u16_t partner_capabilities;

	xil_printf("Start Micrel PHY autonegotiation  \r\n");

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_AUTONEGO_ADVERTISE_REG, &control);  //reg 0x04
	control |= IEEE_ASYMMETRIC_PAUSE_MASK;   //0x0800
	control |= IEEE_PAUSE_MASK;
	control |= ADVERTISE_100;
	control |= ADVERTISE_10;
	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_AUTONEGO_ADVERTISE_REG, control);

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_1000_ADVERTISE_REG_OFFSET,	&control);
	control |= ADVERTISE_1000;
	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_1000_ADVERTISE_REG_OFFSET,control);

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_COPPER_SPECIFIC_CONTROL_REG, &control);  //reg 0x0f
	control |= (7 << 12);	
	control |= (1 << 11);	
	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_COPPER_SPECIFIC_CONTROL_REG,	control);

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, &control);          //reg 0x00
	control |= IEEE_CTRL_AUTONEGOTIATE_ENABLE;
	control |= IEEE_STAT_AUTONEGOTIATE_RESTART;
	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, control);

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, &control);
	control |= IEEE_CTRL_RESET_MASK;
	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, control);

	while (1) {
		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, &control);
		if (control & IEEE_CTRL_RESET_MASK)
			continue;
		else
			break;
	}

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_STATUS_REG_OFFSET, &status);

	xil_printf("Waiting for PHY to complete autonegotiation.\r\n");

	while ( !(status & IEEE_STAT_AUTONEGOTIATE_COMPLETE) ) {
		sleep(1);
		XEmacPs_PhyRead(xemacpsp, phy_addr,IEEE_COPPER_SPECIFIC_STATUS_REG_2,  &temp);
		timeout_counter++;
		if (timeout_counter == 30) {
			xil_printf("Auto negotiation error \r\n");
			return;
		}

		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_STATUS_REG_OFFSET, &status);
	}
	xil_printf("autonegotiation complete \r\n");

#define IEEE_1000BASE_STATUS_REG 0x0a


	/*
	XEmacPs_PhyRead(xemacpsp, phy_addr,IEEE_1000BASE_STATUS_REG, &status_speed);
	if (status_speed & 0x0800) {
		xil_printf("micrel phy ksz9031 speed %x \r\n",status_speed);
		//return 1000;
	}
    */

	XEmacPs_PhyRead(xemacpsp, phy_addr, 0x1F, &status);

	xil_printf("micrel phy ksz9031 final speed status %x \r\n",status & 0x71);

	if ( (status  & 0x70) == 0x40)/* 1000Mbps */
		return 1000;
	else if ( (status  & 0x70) == 0x20)/* 100Mbps */
		return 100;
	else					/* 10Mbps */
		return 10;

	return XST_SUCCESS;
}

/* End Micrel KSZ9031NRX PHY functions */

unsigned get_IEEE_phy_speed(XEmacPs *xemacpsp)
{
	u16 temp;
	u16 control;
	u16 status;
	u16 partner_capabilities;
	
	u16_t phy_identity;
	u32_t RetStatus;
	
#if XPAR_GIGE_PCS_PMA_CORE_PRESENT == 1
	u32 phy_addr = XPAR_PCSPMA_SGMII_PHYADDR;
#else
	u32 phy_addr = detect_phy(xemacpsp);
#endif
	XEmacPs_PhyRead(xemacpsp, phy_addr, PHY_IDENTIFIER_1_REG,
					&phy_identity);
	if (phy_identity == PHY_LANTIQ_IDENTIFIER) // Detect the Lantiq PHY11G PHY included in the RedPitaya board.
	{
		RetStatus = get_Lantiq_phy_speed(xemacpsp, phy_addr);
		return RetStatus;
	}
	else if(phy_identity == PHY_MIC_IDENTIFIER) // Detect the Micrel KSZ9031NRX PHY included in the Z-turn board.*/
	{
		RetStatus = get_Micrel_phy_speed(xemacpsp, phy_addr);
		return RetStatus;
	}
	else
	{
		xil_printf("Start PHY autonegotiation \r\n");

	#if XPAR_GIGE_PCS_PMA_CORE_PRESENT == 1
	#else
		XEmacPs_PhyWrite(xemacpsp,phy_addr, IEEE_PAGE_ADDRESS_REGISTER, 2);
		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_MAC, &control);
		control |= IEEE_RGMII_TXRX_CLOCK_DELAYED_MASK;
		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_CONTROL_REG_MAC, control);

		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_PAGE_ADDRESS_REGISTER, 0);

		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_AUTONEGO_ADVERTISE_REG, &control);
		control |= IEEE_ASYMMETRIC_PAUSE_MASK;
		control |= IEEE_PAUSE_MASK;
		control |= ADVERTISE_100;
		control |= ADVERTISE_10;
		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_AUTONEGO_ADVERTISE_REG, control);

		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_1000_ADVERTISE_REG_OFFSET,
																		&control);
		control |= ADVERTISE_1000;
		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_1000_ADVERTISE_REG_OFFSET,
																		control);

		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_PAGE_ADDRESS_REGISTER, 0);
		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_COPPER_SPECIFIC_CONTROL_REG,
																	&control);
		control |= (7 << 12);	/* max number of gigabit attempts */
		control |= (1 << 11);	/* enable downshift */
		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_COPPER_SPECIFIC_CONTROL_REG,
																	control);
	#endif
		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, &control);
		control |= IEEE_CTRL_AUTONEGOTIATE_ENABLE;
		control |= IEEE_STAT_AUTONEGOTIATE_RESTART;
	#if XPAR_GIGE_PCS_PMA_CORE_PRESENT == 1
		control &= IEEE_CTRL_ISOLATE_DISABLE;
	#endif

		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, control);


	#if XPAR_GIGE_PCS_PMA_CORE_PRESENT == 1
	#else
		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, &control);
		control |= IEEE_CTRL_RESET_MASK;
		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, control);

		while (1) {
			XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, &control);
			if (control & IEEE_CTRL_RESET_MASK)
				continue;
			else
				break;
		}
	#endif
		xil_printf("Waiting for PHY to complete autonegotiation.\r\n");

		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_STATUS_REG_OFFSET, &status);
		while ( !(status & IEEE_STAT_AUTONEGOTIATE_COMPLETE) ) {
			sleep(1);
	#if XPAR_GIGE_PCS_PMA_CORE_PRESENT == 1
	#else
			XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_COPPER_SPECIFIC_STATUS_REG_2,
																		&temp);
			if (temp & IEEE_AUTONEG_ERROR_MASK) {
				xil_printf("Auto negotiation error \r\n");
			}
	#endif
			XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_STATUS_REG_OFFSET,
																	&status);
			}

		xil_printf("autonegotiation complete \r\n");

	#if XPAR_GIGE_PCS_PMA_CORE_PRESENT == 1
	#else
		XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_SPECIFIC_STATUS_REG, &partner_capabilities);
	#endif

	#if XPAR_GIGE_PCS_PMA_CORE_PRESENT == 1
		xil_printf("Waiting for Link to be up; Polling for SGMII core Reg \r\n");
		XEmacPs_PhyRead(xemacpsp, phy_addr, 5, &temp);
		while(!(temp & 0x8000)) {
			XEmacPs_PhyRead(xemacpsp, phy_addr, 5, &temp);
		}
		if((temp & 0x0C00) == 0x0800) {
			XEmacPs_PhyRead(xemacpsp, phy_addr, 0, &temp);
			return 1000;
		}
		else if((temp & 0x0C00) == 0x0400) {
			XEmacPs_PhyRead(xemacpsp, phy_addr, 0, &temp);
			return 100;
		}
		else if((temp & 0x0C00) == 0x0000) {
			XEmacPs_PhyRead(xemacpsp, phy_addr, 0, &temp);
			return 10;
		} else {
			xil_printf("get_IEEE_phy_speed(): Invalid speed bit value, Deafulting to Speed = 10 Mbps\r\n");
			XEmacPs_PhyRead(xemacpsp, phy_addr, 0, &temp);
			XEmacPs_PhyWrite(xemacpsp, phy_addr, 0, 0x0100);
			return 10;
		}
	#else
		if ( ((partner_capabilities >> 14) & 3) == 2)/* 1000Mbps */
			return 1000;
		else if ( ((partner_capabilities >> 14) & 3) == 1)/* 100Mbps */
			return 100;
		else					/* 10Mbps */
			return 10;
	#endif
	}
}
#endif

unsigned configure_IEEE_phy_speed(XEmacPs *xemacpsp, unsigned speed)
{
	u16 control;
	u32 phy_addr = detect_phy(xemacpsp);

	XEmacPs_PhyWrite(xemacpsp,phy_addr, IEEE_PAGE_ADDRESS_REGISTER, 2);
	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_MAC, &control);
	control |= IEEE_RGMII_TXRX_CLOCK_DELAYED_MASK;
	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_CONTROL_REG_MAC, control);

	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_PAGE_ADDRESS_REGISTER, 0);

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_AUTONEGO_ADVERTISE_REG, &control);
	control |= IEEE_ASYMMETRIC_PAUSE_MASK;
	control |= IEEE_PAUSE_MASK;
	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_AUTONEGO_ADVERTISE_REG, control);

	XEmacPs_PhyRead(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET, &control);
	control &= ~IEEE_CTRL_LINKSPEED_1000M;
	control &= ~IEEE_CTRL_LINKSPEED_100M;
	control &= ~IEEE_CTRL_LINKSPEED_10M;

	if (speed == 1000) {
		control |= IEEE_CTRL_LINKSPEED_1000M;
	}

	else if (speed == 100) {
		control |= IEEE_CTRL_LINKSPEED_100M;
		/* Dont advertise PHY speed of 1000 Mbps */
		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_1000_ADVERTISE_REG_OFFSET, 0);
		/* Dont advertise PHY speed of 10 Mbps */
		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_AUTONEGO_ADVERTISE_REG,
																ADVERTISE_100);
	}

	else if (speed == 10) {
		control |= IEEE_CTRL_LINKSPEED_10M;
		/* Dont advertise PHY speed of 1000 Mbps */
		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_1000_ADVERTISE_REG_OFFSET,
																			0);
		/* Dont advertise PHY speed of 100 Mbps */
		XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_AUTONEGO_ADVERTISE_REG,
																ADVERTISE_10);
	}

	XEmacPs_PhyWrite(xemacpsp, phy_addr, IEEE_CONTROL_REG_OFFSET,
											control | IEEE_CTRL_RESET_MASK);
	{
		volatile int wait;
		for (wait=0; wait < 100000; wait++);
	}
	return 0;
}

static void SetUpSLCRDivisors(int mac_baseaddr, int speed)
{
	volatile u32 slcrBaseAddress;
#ifndef PEEP
	u32 SlcrDiv0;
	u32 SlcrDiv1;
	u32 SlcrTxClkCntrl;
#endif

	*(volatile unsigned int *)(SLCR_UNLOCK_ADDR) = SLCR_UNLOCK_KEY_VALUE;

	if (mac_baseaddr == EMAC0_BASE_ADDRESS) {
		slcrBaseAddress = SLCR_GEM0_CLK_CTRL_ADDR;
	} else {
		slcrBaseAddress = SLCR_GEM1_CLK_CTRL_ADDR;
	}
#ifdef PEEP
	if (speed == 1000) {
		*(volatile unsigned int *)(slcrBaseAddress) =
											SLCR_GEM_1G_CLK_CTRL_VALUE;
	} else if (speed == 100) {
		*(volatile unsigned int *)(slcrBaseAddress) =
											SLCR_GEM_100M_CLK_CTRL_VALUE;
	} else {
		*(volatile unsigned int *)(slcrBaseAddress) =
											SLCR_GEM_10M_CLK_CTRL_VALUE;
	}
#else
	if (speed == 1000) {
		if (mac_baseaddr == EMAC0_BASE_ADDRESS) {
#ifdef XPAR_PS7_ETHERNET_0_ENET_SLCR_1000MBPS_DIV0
			SlcrDiv0 = XPAR_PS7_ETHERNET_0_ENET_SLCR_1000MBPS_DIV0;
			SlcrDiv1 = XPAR_PS7_ETHERNET_0_ENET_SLCR_1000MBPS_DIV1;
#endif
		} else {
#ifdef XPAR_PS7_ETHERNET_1_ENET_SLCR_1000MBPS_DIV0
			SlcrDiv0 = XPAR_PS7_ETHERNET_1_ENET_SLCR_1000MBPS_DIV0;
			SlcrDiv1 = XPAR_PS7_ETHERNET_1_ENET_SLCR_1000MBPS_DIV1;
#endif
		}
	} else if (speed == 100) {
		if (mac_baseaddr == EMAC0_BASE_ADDRESS) {
#ifdef XPAR_PS7_ETHERNET_0_ENET_SLCR_100MBPS_DIV0
			SlcrDiv0 = XPAR_PS7_ETHERNET_0_ENET_SLCR_100MBPS_DIV0;
			SlcrDiv1 = XPAR_PS7_ETHERNET_0_ENET_SLCR_100MBPS_DIV1;
#endif
		} else {
#ifdef XPAR_PS7_ETHERNET_1_ENET_SLCR_100MBPS_DIV0
			SlcrDiv0 = XPAR_PS7_ETHERNET_1_ENET_SLCR_100MBPS_DIV0;
			SlcrDiv1 = XPAR_PS7_ETHERNET_1_ENET_SLCR_100MBPS_DIV1;
#endif
		}
	} else {
		if (mac_baseaddr == EMAC0_BASE_ADDRESS) {
#ifdef XPAR_PS7_ETHERNET_0_ENET_SLCR_10MBPS_DIV0
			SlcrDiv0 = XPAR_PS7_ETHERNET_0_ENET_SLCR_10MBPS_DIV0;
			SlcrDiv1 = XPAR_PS7_ETHERNET_0_ENET_SLCR_10MBPS_DIV1;
#endif
		} else {
#ifdef XPAR_PS7_ETHERNET_1_ENET_SLCR_10MBPS_DIV0
			SlcrDiv0 = XPAR_PS7_ETHERNET_1_ENET_SLCR_10MBPS_DIV0;
			SlcrDiv1 = XPAR_PS7_ETHERNET_1_ENET_SLCR_10MBPS_DIV1;
#endif
		}
	}
	SlcrTxClkCntrl = *(volatile unsigned int *)(slcrBaseAddress);
	SlcrTxClkCntrl &= EMACPS_SLCR_DIV_MASK;
	SlcrTxClkCntrl |= (SlcrDiv1 << 20);
	SlcrTxClkCntrl |= (SlcrDiv0 << 8);
	*(volatile unsigned int *)(slcrBaseAddress) = SlcrTxClkCntrl;
#endif
	*(volatile unsigned int *)(SLCR_LOCK_ADDR) = SLCR_LOCK_KEY_VALUE;
	return;
}


unsigned Phy_Setup (XEmacPs *xemacpsp)
{
	unsigned link_speed;
	unsigned short regval;
	unsigned long conv_present = 0;
	unsigned long convspeeddupsetting = 0;
	unsigned long convphyaddr = 0;

#ifdef XPAR_GMII2RGMIICON_0N_ETH0_ADDR
	convphyaddr = XPAR_GMII2RGMIICON_0N_ETH0_ADDR;
	conv_present = 1;
#else
#ifdef XPAR_GMII2RGMIICON_0N_ETH1_ADDR
	convphyaddr = XPAR_GMII2RGMIICON_0N_ETH1_ADDR;
	conv_present = 1;
#endif
#endif

#ifdef  CONFIG_LINKSPEED_AUTODETECT
	link_speed = get_IEEE_phy_speed(xemacpsp);
	if (link_speed == 1000) {
		SetUpSLCRDivisors(xemacpsp->Config.BaseAddress,1000);
		convspeeddupsetting = XEMACPS_GMII2RGMII_SPEED1000_FD;
	} else if (link_speed == 100) {
		SetUpSLCRDivisors(xemacpsp->Config.BaseAddress,100);
		convspeeddupsetting = XEMACPS_GMII2RGMII_SPEED100_FD;
	} else {
		SetUpSLCRDivisors(xemacpsp->Config.BaseAddress,10);
		convspeeddupsetting = XEMACPS_GMII2RGMII_SPEED10_FD;
	}
#elif	defined(CONFIG_LINKSPEED1000)
	SetUpSLCRDivisors(xemacpsp->Config.BaseAddress,1000);
	link_speed = 1000;
	configure_IEEE_phy_speed(xemacpsp, link_speed);
	convspeeddupsetting = XEMACPS_GMII2RGMII_SPEED1000_FD;
	sleep(1);
#elif	defined(CONFIG_LINKSPEED100)
	SetUpSLCRDivisors(xemacpsp->Config.BaseAddress,100);
	link_speed = 100;
	configure_IEEE_phy_speed(xemacpsp, link_speed);
	convspeeddupsetting = XEMACPS_GMII2RGMII_SPEED100_FD;
	sleep(1);
#elif	defined(CONFIG_LINKSPEED10)
	SetUpSLCRDivisors(xemacpsp->Config.BaseAddress,10);
	link_speed = 10;
	configure_IEEE_phy_speed(xemacpsp, link_speed);
	convspeeddupsetting = XEMACPS_GMII2RGMII_SPEED10_FD;
	sleep(1);
#endif
	if (conv_present) {
		XEmacPs_PhyWrite(xemacpsp, convphyaddr,
		XEMACPS_GMII2RGMII_REG_NUM, convspeeddupsetting);
	}

	xil_printf("link speed: %d\r\n", link_speed);
	return link_speed;
}