zybo eth test mii.c

/******************************************************************************
*
* Copyright (C) 2010 - 2019 Xilinx, Inc.  All rights reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
* THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
*
*
******************************************************************************/
/****************************************************************************/
/**
*
* @file xemacps_example_intr_dma.c
*
* Implements examples that utilize the EmacPs's interrupt driven DMA
* packet transfer mode to send and receive frames.
*
* These examples demonstrate:
*
* - How to perform simple send and receive.
* - Interrupt
* - Error handling
* - Device reset
*
* Functional guide to example:
*
* - EmacPsDmaSingleFrameIntrExample demonstrates the simplest way to send and
*   receive frames in in interrupt driven DMA mode.
*
* - EmacPsErrorHandler() demonstrates how to manage asynchronous errors.
*
* - EmacPsResetDevice() demonstrates how to reset the driver/HW without
*   losing all configuration settings.
*
*
* <pre>
* MODIFICATION HISTORY:
*
* Ver   Who  Date     Changes
* ----- ---- -------- -------------------------------------------------------
* 1.00a wsy  01/10/10 First release
* 1.00a asa  11/25/11 The cache disable routines are removed. So now both
*		      I-cache and D-cache are enabled. The array RxBuffer is
*		      removed to avoid an extra copy from RxBuffer to RxFrame.
*		      Now the address of RxFrame is submitted to the Rx BD
*		      instead of the address of RxBuffer.
*		      In function EmacPsDmaSingleFrameIntrExample, BdRxPtr
*		      is made as a pointer instead of array of pointers. This
*		      is done since on the Rx path we now submit a single BD
*		      instead of all 32 BDs. Because of this change, relevant
*		      changes are made throughout the function
*		      EmacPsDmaSingleFrameIntrExample.
*		      Cache invalidation is now being done for the RxFrame
*		      buffer.
*		      The unnecessary cache flush (Xil_DCacheFlushRange) is
*		      removed. This was being done towards the end of the
*		      example which was unnecessary.
* 1.00a asa 01/24/12  Support for Zynq board is added. The SLCR divisors are
* 		      different for Zynq. Changes are made for the same.
* 		      Presently the SLCR GEM clock divisors are hard-coded
*		      assuming that IO PLL output frequency is 1000 MHz.
* 		      The BDs are allocated at the address 0xFF00000 and the
* 		      1 MB address range starting from this address is made
* 		      uncached. This is because, for GEM the BDs need to be
* 		      placed in uncached memory. The RX BDs are allocated at
* 		      address 0xFF00000 and TX BDs are allocated at address
* 		      0xFF10000.
* 		      The MDIO divisor used of 224 is used for Zynq board.
* 1.01a asa 02/27/12  The hardcoded SLCR divisors for Zynq are removed. The
*		      divisors are obtained from xparameters.h.c. The sleep
*		      values are reduced for Zynq. One sleep is added after
*		      MDIO divisor is set. Some of the prints are removed.
* 1.01a asa 03/14/12  The SLCR divisor support for ENET1 is added.
* 1.01a asa 04/15/12  The funcation calls to Xil_DisableMMU and Xil_EnableMMU
*		      are removed for setting the translation table
*		      attributes for the BD memory region.
* 1.05a asa 09/22/13 Cache handling is changed to fix an issue (CR#663885).
*			  The cache invalidation of the Rx frame is now moved to
*			  XEmacPsRecvHandler so that invalidation happens after the
*			  received data is available in the memory. The variable
*			  TxFrameLength is now made global.
* 2.1	srt 07/11/14 Implemented 64-bit changes and modified as per
*		      Zynq Ultrascale Mp GEM specification
* 3.0  kpc  01/23/14 Removed PEEP board related code
* 3.0  hk   03/18/15 Added support for jumbo frames.
*                    Add cache flush after BD terminate entries.
* 3.2  hk   10/15/15 Added clock control using CRL_APB_GEM_REF_CTRL register.
*                    Enabled 1G speed for ZynqMP GEM.
*                    Select GEM interrupt based on instance present.
*                    Manage differences between emulation platform and silicon.
* 3.2  mus  20/02/16.Added support for INTC interrupt controlller.
*                    Added support to access zynq emacps interrupt from
*                    microblaze.
* 3.3 kpc   12/09/16 Fixed issue when -O2 is enabled
* 3.4 ms    01/23/17 Modified xil_printf statement in main function to
*                   ensure that "Successfully ran" and "Failed" strings
*                   are available in all examples. This is a fix for
*                   CR-965028.
* 3.5 hk    08/14/17 Don't perform data cache operations when CCI is enabled
*                    on ZynqMP.
* 3.8 hk    10/01/18 Fix warning for redefinition of interrupt number.
* 3.9 hk    02/12/19 Change MDC divisor for Versal emulation.
*           03/06/19 Fix BD space assignment and its memory attributes.
*           03/20/19 Fix alignment pragmas for IAR compiler.
* 3.10 hk   05/17/19 Use correct platform register for Versal.
*           08/12/19 Add clock setup support for Versal.
*           14/08/19 Move definition of Platform to _util file for common use.
*           08/24/19 Add support for clock configuration in EL1 Non Secure for
*                    Versal.
*
* </pre>
*
*****************************************************************************/

/***************************** Include Files ********************************/
#include "xemacps_example.h"
#include "xil_exception.h"

#ifndef __MICROBLAZE__
#include "xil_mmu.h"
#endif

#if EL1_NONSECURE
#include "xil_smc.h"
#endif
/*************************** Constant Definitions ***************************/

/*
 * The following constants map to the XPAR parameters created in the
 * xparameters.h file. They are defined here such that a user can easily
 * change all the needed parameters in one place.
 */
#ifdef __MICROBLAZE__
#define XPS_SYS_CTRL_BASEADDR	XPAR_PS7_SLCR_0_S_AXI_BASEADDR
#endif

#ifdef XPAR_INTC_0_DEVICE_ID
#define INTC		XIntc
#define EMACPS_DEVICE_ID	XPAR_XEMACPS_0_DEVICE_ID
#define INTC_DEVICE_ID		XPAR_INTC_0_DEVICE_ID
#else
#define INTC		XScuGic
#define EMACPS_DEVICE_ID	XPAR_XEMACPS_0_DEVICE_ID
#define INTC_DEVICE_ID		XPAR_SCUGIC_SINGLE_DEVICE_ID
#endif

#if defined(XPAR_INTC_0_DEVICE_ID)
#define EMACPS_IRPT_INTR	XPAR_AXI_INTC_0_PROCESSING_SYSTEM7_0_IRQ_P2F_ENET0_INTR
#elif defined(XPAR_PSV_ETHERNET_0_DEVICE_ID) || \
	defined(XPAR_PSU_ETHERNET_0_DEVICE_ID) || \
	defined(XPAR_PS7_ETHERNET_0_DEVICE_ID)
#define EMACPS_IRPT_INTR	XPS_GEM0_INT_ID
#elif defined(XPAR_PSV_ETHERNET_1_DEVICE_ID) || \
	defined(XPAR_PSU_ETHERNET_1_DEVICE_ID) || \
	defined(XPAR_PS7_ETHERNET_1_DEVICE_ID)
#define EMACPS_IRPT_INTR	XPS_GEM1_INT_ID
#elif defined(XPAR_PSU_ETHERNET_2_DEVICE_ID)
#define EMACPS_IRPT_INTR	XPS_GEM2_INT_ID
#elif defined(XPAR_PSU_ETHERNET_3_DEVICE_ID)
#define EMACPS_IRPT_INTR	XPS_GEM3_INT_ID
#endif

#define RXBD_CNT       4	/* Number of RxBDs to use */
#define TXBD_CNT       4	/* Number of TxBDs to use */

/*
 * SLCR 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)


#define SLCR_LOCK_KEY_VALUE		0x767B
#define SLCR_UNLOCK_KEY_VALUE		0xDF0D
#define SLCR_ADDR_GEM_RST_CTRL		(XPS_SYS_CTRL_BASEADDR + 0x214)

/* CRL APB registers for GEM clock control */
#ifdef XPAR_PSU_CRL_APB_S_AXI_BASEADDR
#define CRL_GEM0_REF_CTRL	(XPAR_PSU_CRL_APB_S_AXI_BASEADDR + 0x50)
#define CRL_GEM1_REF_CTRL	(XPAR_PSU_CRL_APB_S_AXI_BASEADDR + 0x54)
#define CRL_GEM2_REF_CTRL	(XPAR_PSU_CRL_APB_S_AXI_BASEADDR + 0x58)
#define CRL_GEM3_REF_CTRL	(XPAR_PSU_CRL_APB_S_AXI_BASEADDR + 0x5C)
#endif

#define CRL_GEM_DIV_MASK	0x003F3F00
#define CRL_GEM_1G_DIV0		0x00000C00
#define CRL_GEM_1G_DIV1		0x00010000

#ifdef XPAR_PSV_CRL_0_S_AXI_BASEADDR
#define CRL_GEM0_REF_CTRL	(XPAR_PSV_CRL_0_S_AXI_BASEADDR + 0x118)
#define CRL_GEM1_REF_CTRL	(XPAR_PSV_CRL_0_S_AXI_BASEADDR + 0x11C)
#endif

#define CRL_GEM_DIV_VERSAL_MASK		0x0003FF00
#define CRL_GEM_DIV_VERSAL_SHIFT	8

#define JUMBO_FRAME_SIZE	10240
#define FRAME_HDR_SIZE		18

#define GEMVERSION_ZYNQMP	0x7
#define GEMVERSION_VERSAL	0x107

/*************************** Variable Definitions ***************************/

#ifdef __ICCARM__
#pragma data_alignment = 64
EthernetFrame TxFrame;		/* Transmit buffer */
#pragma data_alignment = 64
EthernetFrame RxFrame;		/* Receive buffer */
#else
EthernetFrame TxFrame;		/* Transmit buffer */
EthernetFrame RxFrame;		/* Receive buffer */
#endif

/*
 * Buffer descriptors are allocated in uncached memory. The memory is made
 * uncached by setting the attributes appropriately in the MMU table.
 */
// #define RXBD_SPACE_BYTES XEmacPs_BdRingMemCalc(XEMACPS_BD_ALIGNMENT, RXBD_CNT)
// #define TXBD_SPACE_BYTES XEmacPs_BdRingMemCalc(XEMACPS_BD_ALIGNMENT, TXBD_CNT)


/*
 * Buffer descriptors are allocated in uncached memory. The memory is made
 * uncached by setting the attributes appropriately in the MMU table.
 * The minimum region for which attribute settings take effect is 2MB for
 * arm 64 variants(A53) and 1MB for the rest (R5 and A9). Hence the same
 * is allocated, even if not used fully by this example, to make sure none
 * of the adjacent global memory is affected.
 */
//#ifdef __ICCARM__
//#if defined __aarch64__
//#pragma data_alignment = 0x200000
//u8 bd_space[0x200000];
//#else
//#pragma data_alignment = 0x100000
//u8 bd_space[0x100000];
//#endif
//#else
//#if defined __aarch64__
// u8 bd_space[0x200000] __attribute__ ((aligned (0x200000)));
//#else
u8 rxSpace[0x100000] __attribute__ ((aligned (0x100000)));
u8 txSpace[0x100000] __attribute__ ((aligned (0x100000)));
//#endif
//#endif
// #define RX_BD_START_ADDRESS	0x0FF00000
// #define TX_BD_START_ADDRESS	0x0FF10000

// u8 *RxBdSpacePtr;
// u8 *TxBdSpacePtr;


// #ifdef __ICCARM__
// #pragma data_alignment = XEMACPS_RX_BUF_ALIGNMENT
// u8 RxBuf[RXBD_CNT][1600];
// #else
u8 RxBuf[RXBD_CNT][1600] __attribute__ ((aligned(32)));
u8 FrameBuffer[1600] __attribute__ ((aligned(32)));
// #endif

// #define FIRST_FRAGMENT_SIZE 64

/*
 * Counters to be incremented by callbacks
 */
 volatile s32 FramesRx;		/* Frames have been received */
volatile s32 FramesTx;		/* Frames have been sent */
volatile s32 DeviceErrors;	/* Number of errors detected in the device */

u32 TxFrameLength;

#ifndef TESTAPP_GEN
static INTC IntcInstance;
#endif

#ifdef __ICCARM__
#pragma data_alignment = 64
XEmacPs_Bd BdTxTerminate;
#pragma data_alignment = 64
XEmacPs_Bd BdRxTerminate;
#else
XEmacPs_Bd BdTxTerminate __attribute__ ((aligned(64)));

XEmacPs_Bd BdRxTerminate __attribute__ ((aligned(64)));
#endif

u32 PayloadSize = 200;

// u32 GemVersion;

/*************************** Function Prototypes ****************************/

/*
 * Example
 */
LONG EmacPsDmaIntrExample(INTC *IntcInstancePtr,
			  XEmacPs *EmacPsInstancePtr,
			  u16 EmacPsDeviceId, u16 EmacPsIntrId);

LONG EmacPsDmaSingleFrameIntrExample(XEmacPs * EmacPsInstancePtr);

/*
 * Interrupt setup and Callbacks for examples
 */

static LONG EmacPsSetupIntrSystem(INTC * IntcInstancePtr,
				  XEmacPs * EmacPsInstancePtr,
				  u16 EmacPsIntrId);

static void EmacPsDisableIntrSystem(INTC * IntcInstancePtr,
				     u16 EmacPsIntrId);

static void XEmacPsSendHandler(void *Callback);
static void XEmacPsRecvHandler(void *Callback);
static void XEmacPsErrorHandler(void *Callback, u8 direction, u32 word);
// void XEmacPs_PHYSetup (XEmacPs *EmacPsInstancePtr);

/*
 * Utility routines
 */
static LONG EmacPsResetDevice(XEmacPs * EmacPsInstancePtr);
void XEmacPsClkSetup(XEmacPs *EmacPsInstancePtr, u16 EmacPsIntrId);
void XEmacPs_SetMdioDivisor(XEmacPs *InstancePtr, XEmacPs_MdcDiv Divisor);
/****************************************************************************/
/**
*
* This is the main function for the EmacPs example. This function is not
* included if the example is generated from the TestAppGen test tool.
*
* @param	None.
*
* @return	XST_SUCCESS to indicate success, otherwise XST_FAILURE.
*
* @note		None.
*
****************************************************************************/
#ifndef TESTAPP_GEN
int main(void)
{
	LONG Status;

	while (1) {

		xil_printf("\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n"
				"\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n"
				"\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n\r\n");
		xil_printf("Entering into main() \r\n");

		/*
		 * Call the EmacPs DMA interrupt example , specify the parameters
		 * generated in xparameters.h
		 */
		Status = EmacPsDmaIntrExample(&IntcInstance,
						   &EmacPsInstance,
						   EMACPS_DEVICE_ID,
						   EMACPS_IRPT_INTR);

		if (Status != XST_SUCCESS) {
			xil_printf("Emacps intr dma Example Failed\r\n");
			// return XST_FAILURE;
		} else {
			xil_printf("Successfully ran Emacps intr dma Example\r\n");
		}

		EmacpsDelay(2);
	}
	return XST_SUCCESS;
}
#endif


/* Generic MII registers. */
#define MII_BMCR		0x00	/* Basic mode control register */
#define MII_BMSR		0x01	/* Basic mode status register  */
#define MII_PHYSID1		0x02	/* PHYS ID 1                   */
#define MII_PHYSID2		0x03	/* PHYS ID 2                   */
#define MII_ADVERTISE		0x04	/* Advertisement control reg   */
#define MII_LPA			0x05	/* Link partner ability reg    */
#define MII_EXPANSION		0x06	/* Expansion register          */
#define MII_CTRL1000		0x09	/* 1000BASE-T control          */
#define MII_STAT1000		0x0a	/* 1000BASE-T status           */
#define	MII_MMD_CTRL		0x0d	/* MMD Access Control Register */
#define	MII_MMD_DATA		0x0e	/* MMD Access Data Register */
#define MII_ESTATUS		0x0f	/* Extended Status             */
#define MII_DCOUNTER		0x12	/* Disconnect counter          */
#define MII_FCSCOUNTER		0x13	/* False carrier counter       */
#define MII_NWAYTEST		0x14	/* N-way auto-neg test reg     */
#define MII_RERRCOUNTER		0x15	/* Receive error counter       */
#define MII_SREVISION		0x16	/* Silicon revision            */
#define MII_RESV1		0x17	/* Reserved...                 */
#define MII_LBRERROR		0x18	/* Lpback, rx, bypass error    */
#define MII_PHYADDR		0x19	/* PHY address                 */
#define MII_RESV2		0x1a	/* Reserved...                 */
#define MII_TPISTATUS		0x1b	/* TPI status for 10mbps       */
#define MII_NCONFIG		0x1c	/* Network interface config    */

#define MII_PHYCR		0x10

/* Basic mode control register. */
#define BMCR_RESV		0x003f	/* Unused...                   */
#define BMCR_SPEED1000		0x0040	/* MSB of Speed (1000)         */
#define BMCR_CTST		0x0080	/* Collision test              */
#define BMCR_FULLDPLX		0x0100	/* Full duplex                 */
#define BMCR_ANRESTART		0x0200	/* Auto negotiation restart    */
#define BMCR_ISOLATE		0x0400	/* Isolate data paths from MII */
#define BMCR_PDOWN		0x0800	/* Enable low power state      */
#define BMCR_ANENABLE		0x1000	/* Enable auto negotiation     */
#define BMCR_SPEED100		0x2000	/* Select 100Mbps              */
#define BMCR_LOOPBACK		0x4000	/* TXD loopback bits           */
#define BMCR_RESET		0x8000	/* Reset to default state      */
#define BMCR_SPEED10		0x0000	/* Select 10Mbps               */

/* Basic mode status register. */
#define BMSR_ERCAP		0x0001	/* Ext-reg capability          */
#define BMSR_JCD		0x0002	/* Jabber detected             */
#define BMSR_LSTATUS		0x0004	/* Link status                 */
#define BMSR_ANEGCAPABLE	0x0008	/* Able to do auto-negotiation */
#define BMSR_RFAULT		0x0010	/* Remote fault detected       */
#define BMSR_ANEGCOMPLETE	0x0020	/* Auto-negotiation complete   */
#define BMSR_RESV		0x00c0	/* Unused...                   */
#define BMSR_ESTATEN		0x0100	/* Extended Status in R15      */
#define BMSR_100HALF2		0x0200	/* Can do 100BASE-T2 HDX       */
#define BMSR_100FULL2		0x0400	/* Can do 100BASE-T2 FDX       */
#define BMSR_10HALF		0x0800	/* Can do 10mbps, half-duplex  */
#define BMSR_10FULL		0x1000	/* Can do 10mbps, full-duplex  */
#define BMSR_100HALF		0x2000	/* Can do 100mbps, half-duplex */
#define BMSR_100FULL		0x4000	/* Can do 100mbps, full-duplex */
#define BMSR_100BASE4		0x8000	/* Can do 100mbps, 4k packets  */

/* Advertisement control register. */
#define ADVERTISE_SLCT		0x001f	/* Selector bits               */
#define ADVERTISE_CSMA		0x0001	/* Only selector supported     */
#define ADVERTISE_10HALF	0x0020	/* Try for 10mbps half-duplex  */
#define ADVERTISE_1000XFULL	0x0020	/* Try for 1000BASE-X full-duplex */
#define ADVERTISE_10FULL	0x0040	/* Try for 10mbps full-duplex  */
#define ADVERTISE_1000XHALF	0x0040	/* Try for 1000BASE-X half-duplex */
#define ADVERTISE_100HALF	0x0080	/* Try for 100mbps half-duplex */
#define ADVERTISE_1000XPAUSE	0x0080	/* Try for 1000BASE-X pause    */
#define ADVERTISE_100FULL	0x0100	/* Try for 100mbps full-duplex */
#define ADVERTISE_1000XPSE_ASYM	0x0100	/* Try for 1000BASE-X asym pause */
#define ADVERTISE_100BASE4	0x0200	/* Try for 100mbps 4k packets  */
#define ADVERTISE_PAUSE_CAP	0x0400	/* Try for pause               */
#define ADVERTISE_PAUSE_ASYM	0x0800	/* Try for asymetric pause     */
#define ADVERTISE_RESV		0x1000	/* Unused...                   */
#define ADVERTISE_RFAULT	0x2000	/* Say we can detect faults    */
#define ADVERTISE_LPACK		0x4000	/* Ack link partners response  */
#define ADVERTISE_NPAGE		0x8000	/* Next page bit               */

#define ADVERTISE_FULL		(ADVERTISE_100FULL | ADVERTISE_10FULL | \
				  ADVERTISE_CSMA)
#define ADVERTISE_ALL		(ADVERTISE_10HALF | ADVERTISE_10FULL | \
				  ADVERTISE_100HALF | ADVERTISE_100FULL)

/* Link partner ability register. */
#define LPA_SLCT		0x001f	/* Same as advertise selector  */
#define LPA_10HALF		0x0020	/* Can do 10mbps half-duplex   */
#define LPA_1000XFULL		0x0020	/* Can do 1000BASE-X full-duplex */
#define LPA_10FULL		0x0040	/* Can do 10mbps full-duplex   */
#define LPA_1000XHALF		0x0040	/* Can do 1000BASE-X half-duplex */
#define LPA_100HALF		0x0080	/* Can do 100mbps half-duplex  */
#define LPA_1000XPAUSE		0x0080	/* Can do 1000BASE-X pause     */
#define LPA_100FULL		0x0100	/* Can do 100mbps full-duplex  */
#define LPA_1000XPAUSE_ASYM	0x0100	/* Can do 1000BASE-X pause asym*/
#define LPA_100BASE4		0x0200	/* Can do 100mbps 4k packets   */
#define LPA_PAUSE_CAP		0x0400	/* Can pause                   */
#define LPA_PAUSE_ASYM		0x0800	/* Can pause asymetrically     */
#define LPA_RESV		0x1000	/* Unused...                   */
#define LPA_RFAULT		0x2000	/* Link partner faulted        */
#define LPA_LPACK		0x4000	/* Link partner acked us       */
#define LPA_NPAGE		0x8000	/* Next page bit               */

#define LPA_DUPLEX		(LPA_10FULL | LPA_100FULL)
#define LPA_100			(LPA_100FULL | LPA_100HALF | LPA_100BASE4)

/* Expansion register for auto-negotiation. */
#define EXPANSION_NWAY		0x0001	/* Can do N-way auto-nego      */
#define EXPANSION_LCWP		0x0002	/* Got new RX page code word   */
#define EXPANSION_ENABLENPAGE	0x0004	/* This enables npage words    */
#define EXPANSION_NPCAPABLE	0x0008	/* Link partner supports npage */
#define EXPANSION_MFAULTS	0x0010	/* Multiple faults detected    */
#define EXPANSION_RESV		0xffe0	/* Unused...                   */

#define ESTATUS_1000_XFULL	0x8000	/* Can do 1000BaseX Full       */
#define ESTATUS_1000_XHALF	0x4000	/* Can do 1000BaseX Half       */
#define ESTATUS_1000_TFULL	0x2000	/* Can do 1000BT Full          */
#define ESTATUS_1000_THALF	0x1000	/* Can do 1000BT Half          */

/* N-way test register. */
#define NWAYTEST_RESV1		0x00ff	/* Unused...                   */
#define NWAYTEST_LOOPBACK	0x0100	/* Enable loopback for N-way   */
#define NWAYTEST_RESV2		0xfe00	/* Unused...                   */

/* MAC and PHY tx_config_Reg[15:0] for SGMII in-band auto-negotiation.*/
#define ADVERTISE_SGMII		0x0001	/* MAC can do SGMII            */
#define LPA_SGMII		0x0001	/* PHY can do SGMII            */
#define LPA_SGMII_SPD_MASK	0x0c00	/* SGMII speed mask            */
#define LPA_SGMII_FULL_DUPLEX	0x1000	/* SGMII full duplex           */
#define LPA_SGMII_DPX_SPD_MASK	0x1C00	/* SGMII duplex and speed bits */
#define LPA_SGMII_10		0x0000	/* 10Mbps                      */
#define LPA_SGMII_10HALF	0x0000	/* Can do 10mbps half-duplex   */
#define LPA_SGMII_10FULL	0x1000	/* Can do 10mbps full-duplex   */
#define LPA_SGMII_100		0x0400	/* 100Mbps                     */
#define LPA_SGMII_100HALF	0x0400	/* Can do 100mbps half-duplex  */
#define LPA_SGMII_100FULL	0x1400	/* Can do 100mbps full-duplex  */
#define LPA_SGMII_1000		0x0800	/* 1000Mbps                    */
#define LPA_SGMII_1000HALF	0x0800	/* Can do 1000mbps half-duplex */
#define LPA_SGMII_1000FULL	0x1800	/* Can do 1000mbps full-duplex */
#define LPA_SGMII_LINK		0x8000	/* PHY link with copper-side partner */

/* 1000BASE-T Control register */
#define ADVERTISE_1000FULL	0x0200  /* Advertise 1000BASE-T full duplex */
#define ADVERTISE_1000HALF	0x0100  /* Advertise 1000BASE-T half duplex */
#define CTL1000_PREFER_MASTER	0x0400  /* prefer to operate as master */
#define CTL1000_AS_MASTER	0x0800
#define CTL1000_ENABLE_MASTER	0x1000

/* 1000BASE-T Status register */
#define LPA_1000MSFAIL		0x8000	/* Master/Slave resolution failure */
#define LPA_1000MSRES		0x4000	/* Master/Slave resolution status */
#define LPA_1000LOCALRXOK	0x2000	/* Link partner local receiver status */
#define LPA_1000REMRXOK		0x1000	/* Link partner remote receiver status */
#define LPA_1000FULL		0x0800	/* Link partner 1000BASE-T full duplex */
#define LPA_1000HALF		0x0400	/* Link partner 1000BASE-T half duplex */

/* Flow control flags */
#define FLOW_CTRL_TX		0x01
#define FLOW_CTRL_RX		0x02

/* MMD Access Control register fields */
#define MII_MMD_CTRL_DEVAD_MASK	0x1f	/* Mask MMD DEVAD*/
#define MII_MMD_CTRL_ADDR	0x0000	/* Address */
#define MII_MMD_CTRL_NOINCR	0x4000	/* no post increment */
#define MII_MMD_CTRL_INCR_RDWT	0x8000	/* post increment on reads & writes */
#define MII_MMD_CTRL_INCR_ON_WT	0xC000	/* post increment on writes only */


void dumpregs(XEmacPs * EmacPsInstancePtr) {

	#define DUMP(k,v,t) \
		xil_printf("  %d (%04X) (%s)\r\n", (Temp & k) > 0, k, t );


	for( int j=0; j<32; j++) {
		u16 Temp;
		XEmacPs_PhyRead(EmacPsInstancePtr, 0, j, &Temp);
		xil_printf("phy reg %d = %4X => ", j, Temp);
		for(int k=0; k<16; k++) {
			xil_printf("%d", (Temp & (1 << k)) > 0);
		}

		xil_printf("\r\n");

		if (j == MII_BMCR) {
			xil_printf("^ MII_BMCR control register:\r\n");

			DUMP(BMCR_RESV, 0x003f, "Unused...");
			DUMP(BMCR_SPEED1000, 0x0040, "MSB of Speed (1000)");
			DUMP(BMCR_CTST, 0x0080, "Collision test");
			DUMP(BMCR_FULLDPLX, 0x0100, "Full duplex");
			DUMP(BMCR_ANRESTART, 0x0200, "Auto negotiation restart");
			DUMP(BMCR_ISOLATE, 0x0400, "Isolate data paths from");
			DUMP(BMCR_PDOWN, 0x0800, "Enable low power state");
			DUMP(BMCR_ANENABLE, 0x1000, "Enable auto negotiation");
			DUMP(BMCR_SPEED100, 0x2000, "Select 100Mbps");
			DUMP(BMCR_LOOPBACK, 0x4000, "TXD loopback bits");
			DUMP(BMCR_RESET, 0x8000, "Reset to default state");
			// DUMP(BMCR_SPEED10, 0x0000, "Select 10Mbps");

			xil_printf("\r\n");
		}

		if (j == MII_BMSR) {
			xil_printf("^ MII_BMCR status register:\r\n");

			DUMP(BMSR_ERCAP, 0x0001, "Ext-reg capability");
			DUMP(BMSR_JCD, 0x0002, "Jabber detected");
			DUMP(BMSR_LSTATUS, 0x0004, "Link status");
			DUMP(BMSR_ANEGCAPABLE, 0x0008, "Able to do auto-negotiation");
			DUMP(BMSR_RFAULT, 0x0010, "Remote fault detected");
			DUMP(BMSR_ANEGCOMPLETE, 0x0020, "Auto-negotiation complete");
			DUMP(BMSR_RESV, 0x00c0, "Unused...");
			DUMP(BMSR_ESTATEN, 0x0100, "Extended Status in R15");
			DUMP(BMSR_100HALF2, 0x0200, "Can do 100BASE-T2 HDX");
			DUMP(BMSR_100FULL2, 0x0400, "Can do 100BASE-T2 FDX");
			DUMP(BMSR_10HALF, 0x0800, "Can do 10mbps, half-duplex");
			DUMP(BMSR_10FULL, 0x1000, "Can do 10mbps, full-duplex");
			DUMP(BMSR_100HALF, 0x2000, "Can do 100mbps, half-duplex");
			DUMP(BMSR_100FULL, 0x4000, "Can do 100mbps, full-duplex");
			DUMP(BMSR_100BASE4, 0x8000, "Can do 100mbps, 4k packets");

			xil_printf("\r\n");
		}


		if (j == MII_ADVERTISE) {
			xil_printf("^ MII_ADVERTISE advertisement control register:\r\n");

			DUMP(ADVERTISE_SLCT, 0x001f, "Selector bits");
			DUMP(ADVERTISE_CSMA, 0x0001, "Only selector supported");
			DUMP(ADVERTISE_10HALF, 0x0020, "Try for 10mbps half-duplex");
			DUMP(ADVERTISE_1000XFULL, 0x0020, "Try for 1000BASE-X full-duplex");
			DUMP(ADVERTISE_10FULL, 0x0040, "Try for 10mbps full-duplex");
			DUMP(ADVERTISE_1000XHALF, 0x0040, "Try for 1000BASE-X half-duplex");
			DUMP(ADVERTISE_100HALF, 0x0080, "Try for 100mbps half-duplex");
			DUMP(ADVERTISE_1000XPAUSE, 0x0080, "Try for 1000BASE-X pause");
			DUMP(ADVERTISE_100FULL, 0x0100, "Try for 100mbps full-duplex");
			DUMP(ADVERTISE_1000XPSE_ASYM, 0x0100, "Try for 1000BASE-X asym pause");
			DUMP(ADVERTISE_100BASE4, 0x0200, "Try for 100mbps 4k packets");
			DUMP(ADVERTISE_PAUSE_CAP, 0x0400, "Try for pause");
			DUMP(ADVERTISE_PAUSE_ASYM, 0x0800, "Try for asymetric pause");
			DUMP(ADVERTISE_RESV, 0x1000, "Unused...");
			DUMP(ADVERTISE_RFAULT, 0x2000, "Say we can detect faults");
			DUMP(ADVERTISE_LPACK, 0x4000, "Ack link partners response");
			DUMP(ADVERTISE_NPAGE, 0x8000, "Next page bit");

			xil_printf("\r\n");
		}


		if (j == MII_LPA) {
			xil_printf("^ MII_LPA link partner ability register:\r\n");

			DUMP(LPA_SLCT, 0x001f, "Same as advertise selector");
			DUMP(LPA_10HALF, 0x0020, "Can do 10mbps half-duplex");
			DUMP(LPA_1000XFULL, 0x0020, "Can do 1000BASE-X full-duplex");
			DUMP(LPA_10FULL, 0x0040, "Can do 10mbps full-duplex");
			DUMP(LPA_1000XHALF, 0x0040, "Can do 1000BASE-X half-duplex");
			DUMP(LPA_100HALF, 0x0080, "Can do 100mbps half-duplex");
			DUMP(LPA_1000XPAUSE, 0x0080, "Can do 1000BASE-X pause");
			DUMP(LPA_100FULL, 0x0100, "Can do 100mbps full-duplex");
			DUMP(LPA_1000XPAUSE_ASYM, 0x0100, "Can do 1000BASE-X pause ");
			DUMP(LPA_100BASE4, 0x0200, "Can do 100mbps 4k packets");
			DUMP(LPA_PAUSE_CAP, 0x0400, "Can pause");
			DUMP(LPA_PAUSE_ASYM, 0x0800, "Can pause asymetrically");
			DUMP(LPA_RESV, 0x1000, "Unused...");
			DUMP(LPA_RFAULT, 0x2000, "Link partner faulted");
			DUMP(LPA_LPACK, 0x4000, "Link partner acked us");
			DUMP(LPA_NPAGE, 0x8000, "Next page bit");

			xil_printf("\r\n");
		}
	}
}



























int XEmacPs_PtpTxPacket(XEmacPs *EmacPsInstance, u8 *PacketBuf,
							int PacketLen)
{
	int Status;
	XEmacPs_Bd *BdPtr;
	XEmacPs_BdRing *TxRingPtr;

	Xil_ExceptionDisable();

//    while((XEmacPs_ReadReg(EmacPsInstance->Config.BaseAddress, XEMACPS_TXSR_OFFSET)) & 0x08) {
//    };

	TxRingPtr = &(XEmacPs_GetTxRing(EmacPsInstance));

	Status = XEmacPs_BdRingAlloc(TxRingPtr, 1, &BdPtr);

	xil_printf("Got Bdptr %X\r\n", (u32)BdPtr);

	if (Status != XST_SUCCESS) {
		Xil_ExceptionEnable();
		EmacPsUtilErrorTrap("Failed to send: XEmacPs_BdRingAlloc");
		return XST_FAILURE;
	}

	Xil_DCacheFlushRange((u32)PacketBuf, 128);

	XEmacPs_BdSetAddressTx (BdPtr, PacketBuf);
	XEmacPs_BdSetLength(BdPtr, PacketLen);
	XEmacPs_BdClearTxUsed(BdPtr);
	XEmacPs_BdSetLast(BdPtr);
	dmb();
	dsb();

	Status = XEmacPs_BdRingToHw(TxRingPtr, 1, BdPtr);
	if (Status != XST_SUCCESS) {
		Xil_ExceptionEnable();
		EmacPsUtilErrorTrap("Failed to send: XEmacPs_BdRingToHw");
		return XST_FAILURE;
	}
	dmb();
	dsb();
	/*
	 * Start the Tx
	 */
	XEmacPs_Transmit(EmacPsInstance);

	xil_printf("Transmitted\r\n");

	Xil_ExceptionEnable();
	return XST_SUCCESS;
}





LONG SendPacket(XEmacPs * EmacPsInstancePtr)
{
	xil_printf("Preparing frame\r\n");

	/*
	EmacPsUtilFrameMemClear(&TxFrame);
	EmacPsUtilFrameHdrFormatMAC(&TxFrame, EmacPsMAC);
	EmacPsUtilFrameHdrFormatType(&TxFrame, 3);
	EmacPsUtilFrameSetPayloadData(&TxFrame, PayloadSize);
	*/

	u8 *Frame = (u8*) &TxFrame;

	Frame[0] = 0xff;
	Frame[1] = 0xff;
	Frame[2] = 0xff;
	Frame[3] = 0xff;
	Frame[4] = 0xff;
	Frame[5] = 0xff;

	Frame[6] = 0x01;
	Frame[7] = 0x02;
	Frame[8] = 0x03;
	Frame[9] = 0x04;
	Frame[10] = 0x05;
	Frame[11] = 0x06;

	Frame[12] = 0x88; // XEMACPS_PTP_ETHERTYPE
	Frame[13] = 0xF7;

	XEmacPs_PtpTxPacket(EmacPsInstancePtr, Frame, XEMACPS_HDR_SIZE + PayloadSize);
}


LONG RunCommLoop(INTC * IntcInstancePtr,
			  XEmacPs * EmacPsInstancePtr,
			  u16 EmacPsDeviceId,
			  u16 EmacPsIntrId)
{
	LONG Status, Index;
	XEmacPs_Bd BdTemplate;
	u16 Temp;
	u32 Reg;
	XEmacPs_BdRing *TxRingPtr;
	XEmacPs_BdRing *RxRingPtr;
	XEmacPs_Bd *RxBdPtr;
	XEmacPs_Bd *CurrBdPtr;

	/* Allocate Rx and Tx BD space each */
	u32 RxBdSpacePtr = (u32)&rxSpace;
	u32 TxBdSpacePtr = (u32)&txSpace;


	TxRingPtr = &XEmacPs_GetTxRing(EmacPsInstancePtr);
	RxRingPtr = &XEmacPs_GetRxRing(EmacPsInstancePtr);




	// RX BDs

	XEmacPs_BdClear(&BdTemplate);

	Status = XEmacPs_BdRingCreate(RxRingPtr,
				RxBdSpacePtr,
				RxBdSpacePtr,
				XEMACPS_BD_ALIGNMENT,
				RXBD_CNT);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error setting up RxBD space, BdRingCreate");
		return XST_FAILURE;
	}


	Status = XEmacPs_BdRingClone(RxRingPtr, &BdTemplate, XEMACPS_RECV);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error setting up RxBD space, BdRingClone");
		return XST_FAILURE;
	}



	xil_printf("RX BDs setup\r\n");




	// TX BDs

	XEmacPs_BdClear(&BdTemplate);
	XEmacPs_BdSetStatus(&BdTemplate, XEMACPS_TXBUF_USED_MASK);

	Status = XEmacPs_BdRingCreate(TxRingPtr,
					TxBdSpacePtr,
					TxBdSpacePtr,
				    XEMACPS_BD_ALIGNMENT,
				    TXBD_CNT);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error setting up TxBD space, BdRingCreate");
		return XST_FAILURE;
	}


	Status = XEmacPs_BdRingClone(TxRingPtr, &BdTemplate, XEMACPS_SEND);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error setting up TxBD space, BdRingClone");
		return XST_FAILURE;
	}


	xil_printf("TX BDs setup\r\n");


	Status = XEmacPs_BdRingAlloc(RxRingPtr, RXBD_CNT, &RxBdPtr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error allocating RxBD");
		return XST_FAILURE;
	}

	CurrBdPtr = RxBdPtr;
	for (Index = 0; Index < RXBD_CNT; Index++) {
		XEmacPs_BdSetAddressRx (CurrBdPtr, &(RxBuf[Index][0]));
		XEmacPs_BdSetLast(CurrBdPtr);
		CurrBdPtr = XEmacPs_BdRingNext (RxRingPtr, CurrBdPtr);
	}

	Status = XEmacPs_BdRingToHw(RxRingPtr, RXBD_CNT, RxBdPtr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error committing RxBD to HW");
		return XST_FAILURE;
	}


	xil_printf("RX Ring setup\r\n");

	XEmacPs_Start(EmacPsInstancePtr);

	xil_printf("XEmacPs started\r\n");


	// Ex https://github.com/Xilinx/embeddedsw/blob/xilinx-v2020.1/XilinxProcessorIPLib/drivers/emacps/examples/xemacps_ieee1588_example.c

	/*
	 * Wait for transmission to complete
	 */
	u16 counter = 0;
	while (1) {

		if (counter % 5 == 0) {
			SendPacket(EmacPsInstancePtr);
		}

		sleep(1);

		xil_printf("waiting.. %d\r\n", counter);
		xil_printf("  %d frames received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXCNT_OFFSET));
		xil_printf("  %d frames sent\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_TXCNT_OFFSET));
		xil_printf("  %d undersize frames received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXUNDRCNT_OFFSET));
		xil_printf("  %d oversize frames received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXOVRCNT_OFFSET));
		xil_printf("  %d jabbers received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXJABCNT_OFFSET));
		xil_printf("  %d fcs errors received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXFCSCNT_OFFSET));
		xil_printf("  %d length errors received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXLENGTHCNT_OFFSET));
		xil_printf("  %d receive overrun errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXORCNT_OFFSET));
		xil_printf("  %d receive resource errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXRESERRCNT_OFFSET));
		xil_printf("  %d counter\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXALIGNCNT_OFFSET));
		xil_printf("  %d ip checksum errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXIPCCNT_OFFSET));
		xil_printf("  %d tcp checksum errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXTCPCCNT_OFFSET));
		xil_printf("  %d udp checksum errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXUDPCCNT_OFFSET));
		xil_printf("  %d ip checksum errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXIPCCNT_OFFSET));


		counter += 1;
	}
































	// xil_printf("x7\r\n");

	/* Gem IP version on Zynq-7000 */
	// Xil_SetTlbAttributes((INTPTR)bd_space, DEVICE_MEMORY);
 	// xil_printf("x8\r\n");


	// xil_printf("x13\r\n");

	Status = XEmacPs_BdRingClone(&(XEmacPs_GetTxRing(EmacPsInstancePtr)),
				      &BdTemplate, XEMACPS_SEND);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap
			("Error setting up TxBD space, BdRingClone");
		return XST_FAILURE;
	}

	// xil_printf("x14\r\n");

	XEmacPs_SetOperatingSpeed(EmacPsInstancePtr, 100);
	XEmacPs_SetOptions(EmacPsInstancePtr, XEMACPS_PROMISC_OPTION);
	// XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_FCS_INSERT_OPTION);
	XEmacPs_SetOptions(EmacPsInstancePtr, XEMACPS_FCS_INSERT_OPTION);


	Reg = XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_NWCFG_OFFSET);
	Reg &= ~XEMACPS_NWCFG_FDEN_MASK;
	// Reg |= XEMACPS_NWCFG_FDEN_MASK;
	// Reg |= XEMACPS_NWCFG_BADPREAMBEN_MASK;
	// Reg |= XEMACPS_NWCFG_FCSIGNORE_MASK;
	XEmacPs_WriteReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_NWCFG_OFFSET, Reg);

	//	XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_FLOW_CONTROL_OPTION);
	// XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_TRANSMITTER_ENABLE_OPTION);
	//	XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_TX_CHKSUM_ENABLE_OPTION);
	//	XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_RX_CHKSUM_ENABLE_OPTION);
	// dumpregs(EmacPsInstancePtr);

	// Disable crossover otherwise autoneg is always on (1gbit)
	// https://datasheet.lcsc.com/szlcsc/Realtek-Semicon-RTL8211EG-VB-CG_C69264.pdf
	xil_printf("disable crossover\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_PHYCR, &Temp);
	Temp &= ~(1 << 6); // Disable Crossover
	Temp &= ~(1 << 5); // MDI
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_PHYCR, Temp);

	// From https://github.com/torvalds/linux/blob/master/drivers/net/phy/realtek.c#L260
/*
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, 0x17, &Temp);
	Temp = 0x2138;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, 0x17, Temp);

	XEmacPs_PhyRead(EmacPsInstancePtr, 0, 0x0e, &Temp);
	Temp = 0x0260;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, 0x0e, Temp);
	*/

	xil_printf("resetting...\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	Temp |= BMCR_RESET;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_BMCR, Temp);
	do {
		EmacpsDelay(1);
		XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	} while (Temp & BMCR_RESET);
	xil_printf("resetted\r\n");

	/*
	xil_printf("change advertising\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_ADVERTISE, &Temp);
	Temp &= ~ADVERTISE_10HALF;
	Temp &= ~ADVERTISE_10FULL;
	// Temp &= ~ADVERTISE_100FULL;
	Temp |= ADVERTISE_100FULL;
	Temp |= ADVERTISE_100HALF;
	// Temp &= ~ADVERTISE_PAUSE_CAP;
	// Temp &= ~ADVERTISE_PAUSE_ASYM;
	// Temp &= ~ADVERTISE_1000XPAUSE;
	// Temp &= ~ADVERTISE_RFAULT;
	// Temp |= ADVERTISE_LPACK;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_ADVERTISE, Temp);
	 */

	xil_printf("change speed\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	// Temp |= BMCR_FULLDPLX;
	Temp &= ~BMCR_FULLDPLX;
	Temp &= ~BMCR_SPEED1000;
	Temp &= ~BMCR_SPEED10;
	Temp |= BMCR_SPEED100;
	Temp &= ~BMCR_ANENABLE;
	Temp |= BMCR_ANRESTART;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_BMCR, Temp);

	/*
	xil_printf("resetting...\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	Temp |= BMCR_RESET;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_BMCR, Temp);
	do {
		EmacpsDelay(1);
		XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	} while (Temp & BMCR_RESET);
	xil_printf("resetted\r\n");
	 */

	// dumpregs(EmacPsInstancePtr);

	// xil_printf("x15.2\r\n");
	// xil_printf("x15.3\r\n");

	EmacpsDelay(1);


	dumpregs(EmacPsInstancePtr);
	xil_printf("x15.41\r\n");
	// xil_printf("x15.42\r\n");


	for(int k=0; k<50; k++) {

		/*
		 * Setup the interrupt controller and enable interrupts
		 */

		/*
		 * Run the EmacPs DMA Single Frame Interrupt example
		 */
		xil_printf("\r\n\r\nattempt #%d\r\n\r\n\r\n", k);

		Status = EmacPsDmaSingleFrameIntrExample(EmacPsInstancePtr);

		xil_printf("attempt result %d\r\n", Status);

		if (Status != XST_SUCCESS) {
			return XST_FAILURE;
		}

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

	}
	/*
	 * Disable the interrupts for the EmacPs device
	 */
	//	EmacPsDisableIntrSystem(IntcInstancePtr, EmacPsIntrId);


	/*
	 * Stop the device
	 */
	XEmacPs_Stop(EmacPsInstancePtr);

	return XST_SUCCESS;
}










































void XEmacPs_PHYSetup (INTC * IntcInstancePtr, XEmacPs * EmacPsInstancePtr, u16 EmacPsDeviceId, u16 EmacPsIntrId)
{
	/*
	u16 Control = 0;
	u16 Status;
	u32 SlcrTxClkCntrl;

#ifdef PHY_AUTONEGOTIATION
	u16 Temp;
	u16 Partner_capabilities;
#ifdef DEBUG_XEMACPS_LEVEL1
	xil_printf("In %s: Start PHY autonegotiation \r\n",__func__);
#endif
	PhyAddress = XEmacPs_DetectPHY(EmacPsInstancePtr);
	XEmacPs_PhyWrite(EmacPsInstancePtr,PhyAddress, 22, 2);
	XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress, 21, &Control);
	Control |= 0x30;
	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 21, Control);

	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 22, 0);

	XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress,
				IEEE_AUTONEGO_ADVERTISE_REG, &Control);
	Control |= 0xd80;
	Control |= 0x60;
	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress,
				IEEE_AUTONEGO_ADVERTISE_REG, Control);

	XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress,
				IEEE_1000_ADVERTISE_REG_OFFSET, &Control);
	Control |= ADVERTISE_1000;
	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress,
				IEEE_1000_ADVERTISE_REG_OFFSET, Control);

	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 22, 0);
	XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress, 16, &Control);
	Control |= (7 << 12);
	Control |= (1 << 11);
	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 16, Control);

	XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress,
					IEEE_CONTROL_REG_OFFSET, &Control);
	Control |= IEEE_CTRL_AUTONEGOTIATE_ENABLE;
	Control |= IEEE_STAT_AUTONEGOTIATE_RESTART;
	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress,
					IEEE_CONTROL_REG_OFFSET, Control);

	XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress,
					IEEE_CONTROL_REG_OFFSET, &Control);
	Control |= IEEE_CTRL_RESET_MASK;
	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress,
					IEEE_CONTROL_REG_OFFSET, Control);

	while (1) {
		XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress,
					IEEE_CONTROL_REG_OFFSET, &Control);
		if (Control & IEEE_CTRL_RESET_MASK)
			continue;
		else
			break;

	}

#ifdef DEBUG_XEMACPS_LEVEL1
	xil_printf("In %s: Waiting for PHY to complete autonegotiation.\r\n",
							__func__);
#endif
	XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress,
					IEEE_STATUS_REG_OFFSET, &Status);
	while ( !(Status & IEEE_STAT_AUTONEGOTIATE_COMPLETE) ) {
		sleep(1);
		XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress, 19, &Temp);
		if (Temp & 0x8000) {
#ifdef DEBUG_XEMACPS_LEVEL1
			xil_printf("In %s: Auto negotiation error \r\n",
							__func__);
#endif
		}
		XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress,
				IEEE_STATUS_REG_OFFSET,
				&Status);
		}
#ifdef DEBUG_XEMACPS_LEVEL1
	xil_printf("In %s: Autonegotiation complete \r\n", __func__);
#endif

	XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress,
				IEEE_SPECIFIC_STATUS_REG,
				&Partner_capabilities);
	if ( ((Partner_capabilities >> 14) & 3) == 2) {
		Link_Speed = 1000;
		xil_printf("Partner_capabilities are %x\r\n",
						Partner_capabilities);

		*(volatile unsigned int *)(SLCR_UNLOCK_ADDR) =
						SLCR_UNLOCK_KEY_VALUE;
		SlcrTxClkCntrl =
		*(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR);
		SlcrTxClkCntrl &= EMACPS_SLCR_DIV_MASK;
		SlcrTxClkCntrl |=
			(XPAR_PS7_ETHERNET_0_ENET_SLCR_1000MBPS_DIV1 << 20);
		SlcrTxClkCntrl
			|= (XPAR_PS7_ETHERNET_0_ENET_SLCR_1000MBPS_DIV0 << 8);
		*(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR) =
						SlcrTxClkCntrl;
		*(volatile unsigned int *)(SLCR_LOCK_ADDR) =
						SLCR_LOCK_KEY_VALUE;
		sleep(1);
	}
	else if ( ((Partner_capabilities >> 14) & 3) == 1) {
		Link_Speed = 100;
		*(volatile unsigned int *)(SLCR_UNLOCK_ADDR) =
						SLCR_UNLOCK_KEY_VALUE;
		SlcrTxClkCntrl =
		*(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR);
		SlcrTxClkCntrl &= EMACPS_SLCR_DIV_MASK;
		SlcrTxClkCntrl
			|= (XPAR_PS7_ETHERNET_0_ENET_SLCR_100MBPS_DIV1 << 20);
		SlcrTxClkCntrl
			|= (XPAR_PS7_ETHERNET_0_ENET_SLCR_100MBPS_DIV0 << 8);
		*(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR) =
						SlcrTxClkCntrl;
		*(volatile unsigned int *)(SLCR_LOCK_ADDR) =
						SLCR_LOCK_KEY_VALUE;
		sleep(1);
	}
	else				{
		Link_Speed = 10;
		*(volatile unsigned int *)(SLCR_UNLOCK_ADDR) =
						SLCR_UNLOCK_KEY_VALUE;
		SlcrTxClkCntrl =
			*(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR);
		SlcrTxClkCntrl &= EMACPS_SLCR_DIV_MASK;
		SlcrTxClkCntrl |=
			(XPAR_PS7_ETHERNET_0_ENET_SLCR_10MBPS_DIV1 << 20);
		SlcrTxClkCntrl |=
			(XPAR_PS7_ETHERNET_0_ENET_SLCR_10MBPS_DIV0 << 8);
		*(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR) =
						SlcrTxClkCntrl;
		*(volatile unsigned int *)(SLCR_LOCK_ADDR) =
		SLCR_LOCK_KEY_VALUE;
		sleep(1);
	}
	xil_printf("In %s: Autonegotiated link speed is  %d\r\n",
							__func__, Link_Speed);
	return;
#else
	u16 PhyReg0  = 0;

	PhyAddress = XEmacPs_DetectPHY(EmacPsInstancePtr);
	Status  = XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress, 0, &PhyReg0);
	PhyReg0 &= (~IEEE_CTRL_AUTONEGOTIATE_ENABLE);
	Status  |= XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 0, PhyReg0);

	Status  |= XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress, 0, &PhyReg0);
	Status  |= XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 0,
						(PhyReg0 |PHY_R0_RESET));
	sleep(1);
	XEmacPs_PhyWrite(EmacPsInstancePtr,PhyAddress, 22, 2);
	Control |= PHY_REG21_100;
	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 21, Control);
	XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 22, 0);

	Status  = XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress, 0, &PhyReg0);
	PhyReg0 = PHY_REG0_100;
	Status  |= XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 0, PhyReg0);

	Status  |= XEmacPs_PhyRead(EmacPsInstancePtr, PhyAddress, 0, &PhyReg0);
	Status  |= XEmacPs_PhyWrite(EmacPsInstancePtr, PhyAddress, 0,
						(PhyReg0 |PHY_R0_RESET));
	sleep(1);
	*(volatile unsigned int *)(SLCR_UNLOCK_ADDR) = SLCR_UNLOCK_KEY_VALUE;
	SlcrTxClkCntrl = *(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR);
	SlcrTxClkCntrl &= EMACPS_SLCR_DIV_MASK;
	SlcrTxClkCntrl |= (XPAR_PS7_ETHERNET_0_ENET_SLCR_100MBPS_DIV1 << 20);
	SlcrTxClkCntrl |= (XPAR_PS7_ETHERNET_0_ENET_SLCR_100MBPS_DIV0 << 8);
	*(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR) = SlcrTxClkCntrl;
	*(volatile unsigned int *)(SLCR_LOCK_ADDR) = SLCR_LOCK_KEY_VALUE;
	sleep(1);
#endif
	*/
	u16 Temp;
	u32 Reg;

	xil_printf("Setting up phy\r\n");

	XEmacPs_SetMdioDivisor(EmacPsInstancePtr, MDC_DIV_224);

	XEmacPs_SetOperatingSpeed(EmacPsInstancePtr, 100);
	XEmacPs_SetOptions(EmacPsInstancePtr, XEMACPS_PROMISC_OPTION);
	// XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_FCS_INSERT_OPTION);
	XEmacPs_SetOptions(EmacPsInstancePtr, XEMACPS_FCS_INSERT_OPTION);


	Reg = XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_NWCFG_OFFSET);
	Reg &= ~XEMACPS_NWCFG_FDEN_MASK;
	// Reg |= XEMACPS_NWCFG_FDEN_MASK;
	// Reg |= XEMACPS_NWCFG_BADPREAMBEN_MASK;
	// Reg |= XEMACPS_NWCFG_FCSIGNORE_MASK;
	XEmacPs_WriteReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_NWCFG_OFFSET, Reg);

	// XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_FLOW_CONTROL_OPTION);
	// XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_TRANSMITTER_ENABLE_OPTION);
	// XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_TX_CHKSUM_ENABLE_OPTION);
	// XEmacPs_ClearOptions(EmacPsInstancePtr, XEMACPS_RX_CHKSUM_ENABLE_OPTION);
	// dumpregs(EmacPsInstancePtr);

	// Disable crossover otherwise autoneg is always on (1gbit)
	// https://datasheet.lcsc.com/szlcsc/Realtek-Semicon-RTL8211EG-VB-CG_C69264.pdf
	xil_printf("disable crossover\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_PHYCR, &Temp);
	Temp &= ~(1 << 6); // Disable Crossover
	Temp &= ~(1 << 5); // MDI
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_PHYCR, Temp);

	// From https://github.com/torvalds/linux/blob/master/drivers/net/phy/realtek.c#L260
	/*
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, 0x17, &Temp);
	Temp = 0x2138;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, 0x17, Temp);

	XEmacPs_PhyRead(EmacPsInstancePtr, 0, 0x0e, &Temp);
	Temp = 0x0260;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, 0x0e, Temp);
	*/

	xil_printf("resetting...\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	Temp |= BMCR_RESET;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_BMCR, Temp);
	do {
		EmacpsDelay(1);
		XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	} while (Temp & BMCR_RESET);
	xil_printf("resetted\r\n");

	xil_printf("change advertising\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_ADVERTISE, &Temp);
	Temp &= ~ADVERTISE_10HALF;
	Temp &= ~ADVERTISE_10FULL;
	Temp &= ~ADVERTISE_100FULL;
	// Temp |= ADVERTISE_100FULL;
	Temp |= ADVERTISE_100HALF;
	// Temp &= ~ADVERTISE_PAUSE_CAP;
	// Temp &= ~ADVERTISE_PAUSE_ASYM;
	Temp &= ~ADVERTISE_1000XPAUSE;
	// Temp &= ~ADVERTISE_RFAULT;
	// Temp |= ADVERTISE_LPACK;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_ADVERTISE, Temp);

	xil_printf("change speed\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	// Temp |= BMCR_FULLDPLX;
	Temp &= ~BMCR_FULLDPLX;
	Temp &= ~BMCR_SPEED1000;
	Temp &= ~BMCR_SPEED10;
	Temp |= BMCR_SPEED100;
	Temp &= ~BMCR_ANENABLE;
	Temp |= BMCR_ANRESTART;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_BMCR, Temp);

	/*
	xil_printf("resetting...\r\n");
	XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	Temp |= BMCR_RESET;
	XEmacPs_PhyWrite(EmacPsInstancePtr, 0, MII_BMCR, Temp);
	do {
		EmacpsDelay(1);
		XEmacPs_PhyRead(EmacPsInstancePtr, 0, MII_BMCR, &Temp);
	} while (Temp & BMCR_RESET);
	xil_printf("resetted\r\n");
	 */

	// dumpregs(EmacPsInstancePtr);

	// xil_printf("x15.2\r\n");
	// xil_printf("x15.3\r\n");

	EmacpsDelay(1);


	dumpregs(EmacPsInstancePtr);
	xil_printf("phy setup done\r\n");









	XEmacPsClkSetup(EmacPsInstancePtr, EmacPsIntrId);


}





LONG EmacPsDmaIntrExample(INTC * IntcInstancePtr,
			  XEmacPs * EmacPsInstancePtr,
			  u16 EmacPsDeviceId,
			  u16 EmacPsIntrId)
{
	LONG Status;
	XEmacPs_Config *Config;
	XEmacPs_Bd BdTemplate;

	u16 Temp;
	u32 Reg;

	/*************************************/
	/* Setup device for first-time usage */
	/*************************************/

	/*
	 *  Initialize instance. Should be configured for DMA
	 *  This example calls _CfgInitialize instead of _Initialize due to
	 *  retiring _Initialize. So in _CfgInitialize we use
	 *  XPAR_(IP)_BASEADDRESS to make sure it is not virtual address.
	 */
	Config = XEmacPs_LookupConfig(EmacPsDeviceId);

	Status = XEmacPs_CfgInitialize(EmacPsInstancePtr, Config, Config->BaseAddress);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error in XEmacPs_CfgInitialize");
		return XST_FAILURE;
	}

	Status = XEmacPs_SetMacAddress(EmacPsInstancePtr, EmacPsMAC, 1);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error setting MAC address");
		return XST_FAILURE;
	}

	XEmacPs_PHYSetup(IntcInstancePtr, EmacPsInstancePtr, EmacPsDeviceId, EmacPsIntrId);
	sleep(1);

	//	XEmacPs_SetOptions(EmacPsInstancePtr, XEMACPS_PROMISC_OPTION);

	Status = XEmacPs_SetHandler(EmacPsInstancePtr, XEMACPS_HANDLER_DMASEND, (void *) XEmacPsSendHandler, EmacPsInstancePtr);
	Status |= XEmacPs_SetHandler(EmacPsInstancePtr, XEMACPS_HANDLER_DMARECV, (void *) XEmacPsRecvHandler, EmacPsInstancePtr);
	Status |= XEmacPs_SetHandler(EmacPsInstancePtr, XEMACPS_HANDLER_ERROR, (void *) XEmacPsErrorHandler, EmacPsInstancePtr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error assigning handlers");
		return XST_FAILURE;
	}

	Status = EmacPsSetupIntrSystem(IntcInstancePtr, EmacPsInstancePtr, EmacPsIntrId);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error setting up interrupts");
		return XST_FAILURE;
	}

	RunCommLoop(IntcInstancePtr, EmacPsInstancePtr, EmacPsDeviceId, EmacPsIntrId);

	XEmacPs_Stop(EmacPsInstancePtr);

	return XST_SUCCESS;
}


/****************************************************************************/
/**
*
* This function demonstrates the usage of the EMACPS by sending and
* receiving a single frame in DMA interrupt mode.
* The source packet will be described by two descriptors. It will be
* received into a buffer described by a single descriptor.
*
* @param	EmacPsInstancePtr is a pointer to the instance of the EmacPs
*		driver.
*
* @return	XST_SUCCESS to indicate success, otherwise XST_FAILURE.
*
* @note		None.
*
*****************************************************************************/
LONG EmacPsDmaSingleFrameIntrExample(XEmacPs *EmacPsInstancePtr)
{
	LONG Status;
	u32 NumRxBuf = 0;
	// u32 RxFrLen;
	XEmacPs_Bd *Bd1Ptr;
	XEmacPs_Bd *BdRxPtr;

	/*
	 * Clear variables shared with callbacks
	 */
	 FramesRx = 0;
	FramesTx = 0;
	DeviceErrors = 0;

	/*
	if (GemVersion > 2) {
		PayloadSize = (JUMBO_FRAME_SIZE - FRAME_HDR_SIZE);
	}*/

	/*
	 * Calculate the frame length (not including FCS)
	 */
	TxFrameLength = XEMACPS_HDR_SIZE + PayloadSize;

	/*
	 * Setup packet to be transmitted
	 */
	EmacPsUtilFrameMemClear(&TxFrame);
	EmacPsUtilFrameHdrFormatMAC(&TxFrame, EmacPsMAC);
	EmacPsUtilFrameHdrFormatType(&TxFrame, 3);
	EmacPsUtilFrameSetPayloadData(&TxFrame, PayloadSize);

	u8 *Frame = (u8*) &TxFrame;

	Frame[0] = 0x00;
	Frame[1] = 0x10;
	Frame[2] = 0x20;
	Frame[3] = 0x30;
	Frame[4] = 0x40;
	Frame[5] = 0x50;

	Frame[6] = 0x00;
	Frame[7] = 0x01;
	Frame[8] = 0x02;
	Frame[9] = 0x03;
	Frame[10] = 0x04;
	Frame[11] = 0x05;

	Frame[12] = 0x12;
	Frame[13] = 0x34;

	// xil_printf("x3.1\r\n");

	if (EmacPsInstancePtr->Config.IsCacheCoherent == 0) {
		Xil_DCacheFlushRange((UINTPTR)&TxFrame, sizeof(EthernetFrame));
	}

	/*
	 * Clear out receive packet memory area
	 */
	EmacPsUtilFrameMemClear(&RxFrame);

	if (EmacPsInstancePtr->Config.IsCacheCoherent == 0) {
		Xil_DCacheFlushRange((UINTPTR)&RxFrame, sizeof(EthernetFrame));
	}

	// xil_printf("x3.2\r\n");

	/*
	 * Allocate RxBDs since we do not know how many BDs will be used
	 * in advance, use RXBD_CNT here.
	 */

	Status = XEmacPs_BdRingAlloc(&(XEmacPs_GetRxRing(EmacPsInstancePtr)),
				      1, &BdRxPtr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error allocating RxBD");
		return XST_FAILURE;
	}

	XEmacPs_BdSetAddressRx(BdRxPtr, (UINTPTR)&RxFrame);

	/*
	 * Enqueue to HW
	 */

	Status = XEmacPs_BdRingToHw(&(XEmacPs_GetRxRing(EmacPsInstancePtr)),
				    1, BdRxPtr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error committing RxBD to HW");
		return XST_FAILURE;
	}


	Status = XEmacPs_BdRingAlloc(&(XEmacPs_GetTxRing(EmacPsInstancePtr)),
				      1, &Bd1Ptr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error allocating TxBD");
		return XST_FAILURE;
	}
























	/*
	 * Setup first TxBD
	 */
	XEmacPs_BdSetAddressTx(Bd1Ptr, (UINTPTR)&TxFrame);
	XEmacPs_BdSetLength(Bd1Ptr, TxFrameLength);
	XEmacPs_BdClearTxUsed(Bd1Ptr);
	XEmacPs_BdSetLast(Bd1Ptr);

	/*
	 * Enqueue to HW
	 */
	Status = XEmacPs_BdRingToHw(&(XEmacPs_GetTxRing(EmacPsInstancePtr)),
				     1, Bd1Ptr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error committing TxBD to HW");
		return XST_FAILURE;
	}
	if (EmacPsInstancePtr->Config.IsCacheCoherent == 0) {
		Xil_DCacheFlushRange((UINTPTR)Bd1Ptr, 64);
	}

	/*
	 * Set the Queue pointers
	 */
	XEmacPs_SetQueuePtr(EmacPsInstancePtr, EmacPsInstancePtr->RxBdRing.BaseBdAddr, 0, XEMACPS_RECV);
	XEmacPs_SetQueuePtr(EmacPsInstancePtr, EmacPsInstancePtr->TxBdRing.BaseBdAddr, 0, XEMACPS_SEND);
	/* if (GemVersion > 2) {
	XEmacPs_SetQueuePtr(EmacPsInstancePtr, EmacPsInstancePtr->TxBdRing.BaseBdAddr, 1, XEMACPS_SEND);
	} else {
	} */

	// xil_printf("x3.5\r\n");
	// dumpregs(EmacPsInstancePtr);

	/*
	 * Start the device
	 */
	xil_printf("Waiting to send\r\n");
	sleep(1);
	xil_printf("Sending\r\n");

	XEmacPs_Start(EmacPsInstancePtr);

	/* Start transmit */
	XEmacPs_Transmit(EmacPsInstancePtr);


	/*
	 * Wait for transmission to complete
	 */
	u16 waiting = 0;
	while (!FramesTx && waiting < 10) {

		sleep(2);
		xil_printf("waiting for transmit.. %d\r\n", FramesTx);
		xil_printf("  %d frames received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXCNT_OFFSET));
		xil_printf("  %d frames sent\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_TXCNT_OFFSET));
		xil_printf("  %d undersize frames received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXUNDRCNT_OFFSET));
		xil_printf("  %d oversize frames received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXOVRCNT_OFFSET));
		xil_printf("  %d jabbers received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXJABCNT_OFFSET));
		xil_printf("  %d fcs errors received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXFCSCNT_OFFSET));
		xil_printf("  %d length errors received\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXLENGTHCNT_OFFSET));
		xil_printf("  %d receive overrun errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXORCNT_OFFSET));
		xil_printf("  %d receive resource errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXRESERRCNT_OFFSET));
		xil_printf("  %d counter\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXALIGNCNT_OFFSET));
		xil_printf("  %d ip checksum errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXIPCCNT_OFFSET));
		xil_printf("  %d tcp checksum errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXTCPCCNT_OFFSET));
		xil_printf("  %d udp checksum errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXUDPCCNT_OFFSET));
		xil_printf("  %d ip checksum errors\r\n", XEmacPs_ReadReg(EmacPsInstancePtr->Config.BaseAddress, XEMACPS_RXIPCCNT_OFFSET));

		waiting += 1;
	}

	xil_printf("Sent?!\r\n");
	sleep(1);

	/*
	 * Now that the frame has been sent, post process our TxBDs.
	 * Since we have only submitted 1 to hardware, then there should
	 * be only 1 ready for post processing.
	 */
	if (XEmacPs_BdRingFromHwTx(&(XEmacPs_GetTxRing(EmacPsInstancePtr)), 1, &Bd1Ptr) == 0) {
		EmacPsUtilErrorTrap("TxBDs were not ready for post processing");
		return XST_FAILURE;
	}

	/*
	 * Examine the TxBDs.
	 *
	 * There isn't much to do. The only thing to check would be DMA
	 * exception bits. But this would also be caught in the error
	 * handler. So we just return these BDs to the free list.
	 */


	Status = XEmacPs_BdRingFree(&(XEmacPs_GetTxRing(EmacPsInstancePtr)), 1, Bd1Ptr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error freeing up TxBDs");
		return XST_FAILURE;
	}

	/*
	 * Wait for Rx indication
	 */
	/*
	xil_printf("x3.86 receiving %d\r\n", FramesRx);

	u16 waiting = 0;
	while (!FramesRx && waiting < 5) {
	*/
		/* waiting ++;
	}

	xil_printf("x3.9 received\r\n");
*/
	/*
	 * Now that the frame has been received, post process our RxBD.
	 * Since we have submitted to hardware, then there should be only 1
	 * ready for post processing.
	 */

	NumRxBuf = XEmacPs_BdRingFromHwRx(&(XEmacPs_GetRxRing
					  (EmacPsInstancePtr)), 1,
					 &BdRxPtr);
	xil_printf("x3.10 %d\r\n", NumRxBuf);
	// if (0 == NumRxBuf) {
//		EmacPsUtilErrorTrap("RxBD was not ready for post processing");
	//	return XST_FAILURE;
	// }

	/*
	*/

	/*
	 * There is no device status to check. If there was a DMA error,
	 * it should have been reported to the error handler. Check the
	 * receive length against the transmitted length, then verify
	 * the data.
	 */
	/*
	if (GemVersion > 2) {
		RxFrLen = XEmacPs_GetRxFrameSize(EmacPsInstancePtr, BdRxPtr);
	} else {
		RxFrLen = XEmacPs_BdGetLength(BdRxPtr);
	}

	xil_printf("received %d bytes, expecting %d bytes\r\n", RxFrLen, TxFrameLength);
	for(int k=0; k<RxFrLen; k++) {
		xil_printf("%02X ", RxFrame[k]);
	}

	xil_printf("\r\n");

	if (RxFrLen != TxFrameLength) {
		EmacPsUtilErrorTrap("Length mismatch");
		return XST_FAILURE;
	}

	if (EmacPsUtilFrameVerify(&TxFrame, &RxFrame) != 0) {
		EmacPsUtilErrorTrap("Data mismatch");
		return XST_FAILURE;
	}
*/
	/*
	 * Return the RxBD back to the channel for later allocation. Free
	 * the exact number we just post processed.
	 */

	Status = XEmacPs_BdRingFree(&(XEmacPs_GetRxRing(EmacPsInstancePtr)),
				     NumRxBuf, BdRxPtr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error freeing up RxBDs");
		return XST_FAILURE;
	}


	/*
	 * Finished this example. If everything worked correctly, all TxBDs
	 * and RxBDs should be free for allocation. Stop the device.
	 */
	XEmacPs_Stop(EmacPsInstancePtr);

	return XST_SUCCESS;
}


/****************************************************************************/
/**
* This function resets the device but preserves the options set by the user.
*
* The descriptor list could be reinitialized with the same calls to
* XEmacPs_BdRingClone() as used in main(). Doing this is a matter of
* preference.
* In many cases, an OS may have resources tied up in the descriptors.
* Reinitializing in this case may bad for the OS since its resources may be
* permamently lost.
*
* @param	EmacPsInstancePtr is a pointer to the instance of the EmacPs
*		driver.
*
* @return	XST_SUCCESS if successful, else XST_FAILURE.
*
* @note		None.
*
*****************************************************************************/
static LONG EmacPsResetDevice(XEmacPs * EmacPsInstancePtr)
{
	/*
	LONG Status = 0;
	u8 MacSave[6];
	u32 Options;
	XEmacPs_Bd BdTemplate;


	XEmacPs_Stop(EmacPsInstancePtr);

	XEmacPs_GetMacAddress(EmacPsInstancePtr, &MacSave, 1);
	Options = XEmacPs_GetOptions(EmacPsInstancePtr);

	XEmacPs_Reset(EmacPsInstancePtr);

	XEmacPs_SetMacAddress(EmacPsInstancePtr, &MacSave, 1);
	Status |= XEmacPs_SetOptions(EmacPsInstancePtr, Options);

	// XEMACPS_RECEIVER_ENABLE_OPTION


	Status |= XEmacPs_ClearOptions(EmacPsInstancePtr, ~Options);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error restoring state after reset");
		return XST_FAILURE;
	}

	Status = XEmacPs_SetHandler(EmacPsInstancePtr,
				     XEMACPS_HANDLER_DMASEND,
				     (void *) XEmacPsSendHandler,
				     EmacPsInstancePtr);
	Status |= XEmacPs_SetHandler(EmacPsInstancePtr,
				    XEMACPS_HANDLER_DMARECV,
				    (void *) XEmacPsRecvHandler,
				    EmacPsInstancePtr);
	Status |= XEmacPs_SetHandler(EmacPsInstancePtr, XEMACPS_HANDLER_ERROR,
				    (void *) XEmacPsErrorHandler,
				    EmacPsInstancePtr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap("Error assigning handlers");
		return XST_FAILURE;
	}

	XEmacPs_BdClear(&BdTemplate);

	Status = XEmacPs_BdRingCreate(&(XEmacPs_GetRxRing
				      (EmacPsInstancePtr)),
				      (UINTPTR) RxBdSpacePtr,
				      (UINTPTR) RxBdSpacePtr,
				      XEMACPS_BD_ALIGNMENT,
				      RXBD_CNT);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap
			("Error setting up RxBD space, BdRingCreate");
		return XST_FAILURE;
	}

	Status = XEmacPs_BdRingClone(&
				      (XEmacPs_GetRxRing(EmacPsInstancePtr)),
				      &BdTemplate, XEMACPS_RECV);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap
			("Error setting up RxBD space, BdRingClone");
		return XST_FAILURE;
	}

	XEmacPs_BdClear(&BdTemplate);
	XEmacPs_BdSetStatus(&BdTemplate, XEMACPS_TXBUF_USED_MASK);

	Status = XEmacPs_BdRingCreate(&(XEmacPs_GetTxRing
				      (EmacPsInstancePtr)),
				      (UINTPTR) TxBdSpacePtr,
				      (UINTPTR) TxBdSpacePtr,
				      XEMACPS_BD_ALIGNMENT,
				      TXBD_CNT);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap
			("Error setting up TxBD space, BdRingCreate");
		return XST_FAILURE;
	}
	Status = XEmacPs_BdRingClone(&
				      (XEmacPs_GetTxRing(EmacPsInstancePtr)),
				      &BdTemplate, XEMACPS_SEND);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap
			("Error setting up TxBD space, BdRingClone");
		return XST_FAILURE;
	}

	XEmacPs_Start(EmacPsInstancePtr);
*/
	return XST_SUCCESS;
}


/****************************************************************************/
/**
*
* This function setups the interrupt system so interrupts can occur for the
* EMACPS.
* @param	IntcInstancePtr is a pointer to the instance of the Intc driver.
* @param	EmacPsInstancePtr is a pointer to the instance of the EmacPs
*		driver.
* @param	EmacPsIntrId is the Interrupt ID and is typically
*		XPAR_<EMACPS_instance>_INTR value from xparameters.h.
*
* @return	XST_SUCCESS if successful, otherwise XST_FAILURE.
*
* @note		None.
*
*****************************************************************************/
static LONG EmacPsSetupIntrSystem(INTC *IntcInstancePtr,
				  XEmacPs *EmacPsInstancePtr,
				  u16 EmacPsIntrId)
{
	LONG Status;

// #ifndef TESTAPP_GEN
	XScuGic_Config *GicConfig;
	Xil_ExceptionInit();

	/*
	 * Initialize the interrupt controller driver so that it is ready to
	 * use.
	 */
	GicConfig = XScuGic_LookupConfig(INTC_DEVICE_ID);
	if (NULL == GicConfig) {
		return XST_FAILURE;
	}

	Status = XScuGic_CfgInitialize(IntcInstancePtr, GicConfig,
		    GicConfig->CpuBaseAddress);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}


	/*
	 * Connect the interrupt controller interrupt handler to the hardware
	 * interrupt handling logic in the processor.
	 */
	Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_IRQ_INT,
			(Xil_ExceptionHandler)XScuGic_InterruptHandler,
			IntcInstancePtr);
// #endif

	/*
	 * Connect a device driver handler that will be called when an
	 * interrupt for the device occurs, the device driver handler performs
	 * the specific interrupt processing for the device.
	 */
	Status = XScuGic_Connect(IntcInstancePtr, EmacPsIntrId,
			(Xil_InterruptHandler) XEmacPs_IntrHandler,
			(void *) EmacPsInstancePtr);
	if (Status != XST_SUCCESS) {
		EmacPsUtilErrorTrap
			("Unable to connect ISR to interrupt controller");
		return XST_FAILURE;
	}

	/*
	 * Enable interrupts from the hardware
	 */
	XScuGic_Enable(IntcInstancePtr, EmacPsIntrId);

// #ifndef TESTAPP_GEN
	/*
	 * Enable interrupts in the processor
	 */
//	Xil_ExceptionEnable();
	Xil_ExceptionEnableMask(XIL_EXCEPTION_IRQ);
// #endif
	return XST_SUCCESS;
}


/****************************************************************************/
/**
*
* This function disables the interrupts that occur for EmacPs.
*
* @param	IntcInstancePtr is the pointer to the instance of the ScuGic
*		driver.
* @param	EmacPsIntrId is interrupt ID and is typically
*		XPAR_<EMACPS_instance>_INTR value from xparameters.h.
*
* @return	None.
*
* @note		None.
*
*****************************************************************************/
static void EmacPsDisableIntrSystem(INTC * IntcInstancePtr,
				     u16 EmacPsIntrId)
{
	/*
	 * Disconnect and disable the interrupt for the EmacPs device
	 */
	XScuGic_Disconnect(IntcInstancePtr, EmacPsIntrId);

}

/****************************************************************************/
/**
*
* This the Transmit handler callback function and will increment a shared
* counter that can be shared by the main thread of operation.
*
* @param	Callback is the pointer to the instance of the EmacPs device.
*
* @return	None.
*
* @note		None.
*
*****************************************************************************/
static void XEmacPsSendHandler(void *Callback)
{
	unsigned int NumBds;
		unsigned int NumBdsToProcess;
		XEmacPs_Bd *BdPtr, *CurBdPtr;
		void *BufAddr;
		int BufLen;
		int Status;
		XEmacPs_BdRing *TxRingPtr;
		unsigned int *Temp;

		XEmacPs *EmacPsInstancePtr = (XEmacPs *) Callback;

	xil_printf("in send handler()\r\n");
	FramesTx++;
	/*
	 * Disable the transmit related interrupts
	 */

	// XEmacPs_IntDisable(EmacPsInstancePtr, (XEMACPS_IXR_TXCOMPL_MASK |
// 		XEMACPS_IXR_TX_ERR_MASK));

	/*
	 * Increment the counter so that main thread knows something
	 * happened.
	 */



		TxRingPtr = &XEmacPs_GetTxRing(EmacPsInstancePtr);

		NumBds = XEmacPs_BdRingFromHwTx( TxRingPtr,
			TXBD_CNT, &BdPtr);

		xil_printf("numbds=%d\n", NumBds);
		if (NumBds == 0) {
				return;
		}

		NumBdsToProcess = NumBds;
		CurBdPtr = BdPtr;
		while (NumBdsToProcess > 0) {
			BufAddr = (void*)(INTPTR)(XEmacPs_BdGetBufAddr(CurBdPtr));
			BufLen = XEmacPs_BdGetLength(CurBdPtr);

			// XEmacPs_PtpTxDoFurtherProcessing (EmacPsInstancePtr, (u8 *)BufAddr);
			Temp = (unsigned int *)CurBdPtr;
			Temp++;
			*Temp &= XEMACPS_TXBUF_WRAP_MASK;
			*Temp |= XEMACPS_TXBUF_USED_MASK;

			CurBdPtr = XEmacPs_BdRingNext(TxRingPtr, CurBdPtr);
			NumBdsToProcess--;
			dmb();
			dsb();
		}
		Status = XEmacPs_BdRingFree( TxRingPtr, NumBds, BdPtr);
		if (Status != XST_SUCCESS) {
			return;
		}
		return;



}


/****************************************************************************/
/**
*
* This is the Receive handler callback function and will increment a shared
* counter that can be shared by the main thread of operation.
*
* @param	Callback is a pointer to the instance of the EmacPs device.
*
* @return	None.
*
* @note		None.
*
*****************************************************************************/
static void XEmacPsRecvHandler(void *Callback)
{
	XEmacPs *EmacPsInstancePtr = (XEmacPs *) Callback;

	/*
	 * Disable the transmit related interrupts
	 */
//	XEmacPs_IntDisable(EmacPsInstancePtr, (XEMACPS_IXR_FRAMERX_MASK |
	//	XEMACPS_IXR_RX_ERR_MASK));


	xil_printf("in recv handler()\r\n");

	/*
	 * Increment the counter so that main thread knows something
	 * happened.
	 */
	 FramesRx++;

	// if (EmacPsInstancePtr->Config.IsCacheCoherent == 0) {
// 		Xil_DCacheInvalidateRange((UINTPTR)&RxFrame, sizeof(EthernetFrame));
	// }

}


/****************************************************************************/
/**
*
* This is the Error handler callback function and this function increments
* the error counter so that the main thread knows the number of errors.
*
* @param	Callback is the callback function for the driver. This
*		parameter is not used in this example.
* @param	Direction is passed in from the driver specifying which
*		direction error has occurred.
* @param	ErrorWord is the status register value passed in.
*
* @return	None.
*
* @note		None.
*
*****************************************************************************/
static void XEmacPsErrorHandler(void *Callback, u8 Direction, u32 ErrorWord)
{
	XEmacPs *EmacPsInstancePtr = (XEmacPs *) Callback;

	/*
	 * Increment the counter so that main thread knows something
	 * happened. Reset the device and reallocate resources ...
	 */
	DeviceErrors++;

	xil_printf("in error callback.\r\n");

	switch (Direction) {
	case XEMACPS_RECV:
		if (ErrorWord & XEMACPS_RXSR_HRESPNOK_MASK) {
			EmacPsUtilErrorTrap("Receive DMA error");
		}
		if (ErrorWord & XEMACPS_RXSR_RXOVR_MASK) {
			EmacPsUtilErrorTrap("Receive over run");
		}
		if (ErrorWord & XEMACPS_RXSR_BUFFNA_MASK) {
			EmacPsUtilErrorTrap("Receive buffer not available");
		}
		break;
	case XEMACPS_SEND:
		if (ErrorWord & XEMACPS_TXSR_HRESPNOK_MASK) {
			EmacPsUtilErrorTrap("Transmit DMA error");
		}
		if (ErrorWord & XEMACPS_TXSR_URUN_MASK) {
			EmacPsUtilErrorTrap("Transmit under run");
		}
		if (ErrorWord & XEMACPS_TXSR_BUFEXH_MASK) {
			EmacPsUtilErrorTrap("Transmit buffer exhausted");
		}
		if (ErrorWord & XEMACPS_TXSR_RXOVR_MASK) {
			EmacPsUtilErrorTrap("Transmit retry excessed limits");
		}
		if (ErrorWord & XEMACPS_TXSR_FRAMERX_MASK) {
			EmacPsUtilErrorTrap("Transmit collision");
		}
		if (ErrorWord & XEMACPS_TXSR_USEDREAD_MASK) {
			EmacPsUtilErrorTrap("Transmit buffer not available");
		}
		break;
	}
	/*
	 * Bypassing the reset functionality as the default tx status for q0 is
	 * USED BIT READ. so, the first interrupt will be tx used bit and it resets
	 * the core always.
	 */
	// if (GemVersion == 2) {
	//	EmacPsResetDevice(EmacPsInstancePtr);
	// }
}

/****************************************************************************/
/**
*
* This function sets up the clock divisors for 1000Mbps.
*
* @param	EmacPsInstancePtr is a pointer to the instance of the EmacPs
*			driver.
* @param	EmacPsIntrId is the Interrupt ID and is typically
*			XPAR_<EMACPS_instance>_INTR value from xparameters.h.
* @return	None.
*
* @note		None.
*
*****************************************************************************/
void XEmacPsClkSetup(XEmacPs *EmacPsInstancePtr, u16 EmacPsIntrId)
{
	u32 ClkCntrl;
	// if (GemVersion == 2)
	// {
	/*************************************/
	/* Setup device for first-time usage */
	/*************************************/

	/* SLCR unlock */
	*(volatile unsigned int *)(SLCR_UNLOCK_ADDR) = SLCR_UNLOCK_KEY_VALUE;

	// if (EmacPsIntrId == XPS_GEM0_INT_ID) {
	// #ifdef XPAR_PS7_ETHERNET_0_ENET_SLCR_1000MBPS_DIV0

	ClkCntrl =
		*(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR);
		ClkCntrl &= EMACPS_SLCR_DIV_MASK;
		// ClkCntrl |= (XPAR_PS7_ETHERNET_0_ENET_SLCR_1000MBPS_DIV1 << 20);
		// ClkCntrl |= (XPAR_PS7_ETHERNET_0_ENET_SLCR_1000MBPS_DIV0 << 8);
		ClkCntrl |= (XPAR_PS7_ETHERNET_0_ENET_SLCR_100MBPS_DIV1 << 20);
		ClkCntrl |= (XPAR_PS7_ETHERNET_0_ENET_SLCR_100MBPS_DIV0 << 8);

	*(volatile unsigned int *)(SLCR_GEM0_CLK_CTRL_ADDR) = ClkCntrl;
	// #endif
	// } else if (EmacPsIntrId == XPS_GEM1_INT_ID) {
	// }

	/* SLCR lock */
	*(unsigned int *)(SLCR_LOCK_ADDR) = SLCR_LOCK_KEY_VALUE;
	sleep(1);

	//	}

}