Data Viz Button Code
Fork of lwip-eth by
Revision 31:da93f0f73711, committed 2016-05-03
- Comitter:
- mbed_official
- Date:
- Tue May 03 00:16:23 2016 +0100
- Parent:
- 30:da4b2487ee7b
- Child:
- 32:3534f0b45d38
- Child:
- 34:dc5aa37ba8e8
- Commit message:
- Synchronized with git revision 9cef243de23875778f461bbe9a8c1bc47e65212b
Full URL: https://github.com/mbedmicro/mbed/commit/9cef243de23875778f461bbe9a8c1bc47e65212b/
Switch to KSDK 2.0
Changed in this revision
--- a/arch/TARGET_Freescale/fsl_enet_driver.c Fri Apr 29 01:16:05 2016 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,469 +0,0 @@ -/* -* Copyright (c) 2013 - 2014, Freescale Semiconductor, 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: -* -* o Redistributions of source code must retain the above copyright notice, this list -* of conditions and the following disclaimer. -* -* o 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. -* -* o Neither the name of Freescale Semiconductor, 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 HOLDER 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. -*/ - -/* Modified by mbed for the lwIP port */ - -#include "fsl_enet_driver.h" -#include "fsl_enet_hal.h" -#include "fsl_clock_manager.h" -#include "fsl_interrupt_manager.h" -#include <string.h> - -#include "sys_arch.h" - -/******************************************************************************* - * Variables - ******************************************************************************/ -/*! @brief Define ENET's IRQ list */ - -void *enetIfHandle; - -/*! @brief Define MAC driver API structure and for application of stack adaptor layer*/ -const enet_mac_api_t g_enetMacApi = -{ - enet_mac_init, - NULL, // enet_mac_deinit, - NULL, // enet_mac_send, -#if !ENET_RECEIVE_ALL_INTERRUPT - NULL, // enet_mac_receive, -#endif - enet_mii_read, - enet_mii_write, - NULL, // enet_mac_add_multicast_group, - NULL, //enet_mac_leave_multicast_group, -}; -/******************************************************************************* - * Code - ******************************************************************************/ - -// NOTE: we need these functions to be non-blocking fpr the PHY task, hence the -// osDelay() below - -/*FUNCTION**************************************************************** - * - * Function Name: enet_mii_read - * Return Value: The execution status. - * Description: Read function. - * This interface read data over the (R)MII bus from the specified PHY register, - * This function is called by all PHY interfaces. - *END*********************************************************************/ -uint32_t enet_mii_read(uint32_t instance, uint32_t phyAddr, uint32_t phyReg, uint32_t *dataPtr) -{ - uint32_t counter; - - /* Check the input parameters*/ - if (!dataPtr) - { - return kStatus_ENET_InvalidInput; - } - - /* Check if the mii is enabled*/ - if (!enet_hal_is_mii_enabled(instance)) - { - return kStatus_ENET_Miiuninitialized; - } - - /* Clear the MII interrupt event*/ - enet_hal_clear_interrupt(instance, kEnetMiiInterrupt); - - /* Read command operation*/ - enet_hal_set_mii_command(instance, phyAddr, phyReg, kEnetReadValidFrame, 0); - - /* Poll for MII complete*/ - for (counter = 0; counter < kEnetMaxTimeout; counter++) - { - if (enet_hal_get_interrupt_status(instance, kEnetMiiInterrupt)) - { - break; - } - osDelay(1); - } - - /* Check for timeout*/ - if (counter == kEnetMaxTimeout) - { - return kStatus_ENET_TimeOut; - } - - /* Get data from mii register*/ - *dataPtr = enet_hal_get_mii_data(instance); - - /* Clear MII interrupt event*/ - enet_hal_clear_interrupt(instance, kEnetMiiInterrupt); - - return kStatus_ENET_Success; -} - -/*FUNCTION**************************************************************** - * - * Function Name: enet_mii_write - * Return Value: The execution status. - * Description: Write function. - * This interface write data over the (R)MII bus to the specified PHY register. - * This function is called by all PHY interfaces. - *END*********************************************************************/ -uint32_t enet_mii_write(uint32_t instance, uint32_t phyAddr, uint32_t phyReg, uint32_t data) -{ - uint32_t counter; - - /* Check if the mii is enabled*/ - if (!enet_hal_is_mii_enabled(instance)) - { - return kStatus_ENET_Miiuninitialized; - } - - /* Clear the MII interrupt event*/ - enet_hal_clear_interrupt(instance, kEnetMiiInterrupt); - - /* Read command operation*/ - enet_hal_set_mii_command(instance, phyAddr, phyReg, kEnetWriteValidFrame, data); - - /* Poll for MII complete*/ - for (counter = 0; counter < kEnetMaxTimeout; counter++) - { - if (enet_hal_get_interrupt_status(instance, kEnetMiiInterrupt)) - { - break; - } - osDelay(1); - } - - /* Check for timeout*/ - if (counter == kEnetMaxTimeout) - { - return kStatus_ENET_TimeOut; - } - - /* Clear MII intrrupt event*/ - enet_hal_clear_interrupt(instance, kEnetMiiInterrupt); - - return kStatus_ENET_Success; -} -/*FUNCTION**************************************************************** - * - * Function Name: enet_mac_mii_init - * Return Value: The execution status. - * Description:Initialize the ENET Mac mii(mdc/mdio)interface. - *END*********************************************************************/ -uint32_t enet_mac_mii_init(enet_dev_if_t * enetIfPtr) -{ - uint32_t frequency; - - /* Check the input parameters*/ - if (enetIfPtr == NULL) - { - return kStatus_ENET_InvalidInput; - } - - /* Configure mii speed*/ - CLOCK_SYS_GetFreq(kSystemClock, &frequency); - enet_hal_config_mii(enetIfPtr->deviceNumber, (frequency/(2 * enetIfPtr->macCfgPtr->miiClock) + 1), - kEnetMdioHoldOneClkCycle, false); - - return kStatus_ENET_Success; -} - -/*FUNCTION**************************************************************** - * - * Function Name: enet_mac_rxbd_init - * Return Value: The execution status. - * Description:Initialize the ENET receive buffer descriptors. - * Note: If you do receive on receive interrupt handler the receive - * data buffer number can be the same as the receive descriptor numbers. - * But if you are polling receive frames please make sure the receive data - * buffers are more than buffer descriptors to guarantee a good performance. - *END*********************************************************************/ -uint32_t enet_mac_rxbd_init(enet_dev_if_t * enetIfPtr, enet_rxbd_config_t *rxbdCfg) -{ - /* Check the input parameters*/ - if ((!enetIfPtr) || (!rxbdCfg)) - { - return kStatus_ENET_InvalidInput; - } - - enetIfPtr->macContextPtr->bufferdescSize = enet_hal_get_bd_size(); - - /* Initialize the bd status*/ - enetIfPtr->macContextPtr->isRxFull = false; - - /* Initialize receive bd base address and current address*/ - enetIfPtr->macContextPtr->rxBdBasePtr = rxbdCfg->rxBdPtrAlign; - enetIfPtr->macContextPtr->rxBdCurPtr = enetIfPtr->macContextPtr->rxBdBasePtr; - enetIfPtr->macContextPtr->rxBdDirtyPtr = enetIfPtr->macContextPtr->rxBdBasePtr; - enet_hal_set_rxbd_address(enetIfPtr->deviceNumber, (uint32_t)(enetIfPtr->macContextPtr->rxBdBasePtr)); - - return kStatus_ENET_Success; -} - -/*FUNCTION**************************************************************** - * - * Function Name: enet_mac_txbd_init - * Return Value: The execution status. - * Description:Initialize the ENET transmit buffer descriptors. - * This function prepare all of the transmit buffer descriptors. - *END*********************************************************************/ -uint32_t enet_mac_txbd_init(enet_dev_if_t * enetIfPtr, enet_txbd_config_t *txbdCfg) -{ - /* Check the input parameters*/ - if ((!enetIfPtr) || (!txbdCfg)) - { - return kStatus_ENET_InvalidInput; - } - - /* Initialize the bd status*/ - enetIfPtr->macContextPtr->isTxFull = false; - - /* Initialize transmit bd base address and current address*/ - enetIfPtr->macContextPtr->txBdBasePtr = txbdCfg->txBdPtrAlign; - enetIfPtr->macContextPtr->txBdCurPtr = enetIfPtr->macContextPtr->txBdBasePtr; - enetIfPtr->macContextPtr->txBdDirtyPtr = enetIfPtr->macContextPtr->txBdBasePtr; - enet_hal_set_txbd_address(enetIfPtr->deviceNumber, (uint32_t)(enetIfPtr->macContextPtr->txBdBasePtr)); - return kStatus_ENET_Success; -} - -/*FUNCTION**************************************************************** - * - * Function Name: enet_mac_configure_fifo_accel - * Return Value: The execution status. - * Description: Configure the ENET FIFO and Accelerator. - *END*********************************************************************/ -uint32_t enet_mac_configure_fifo_accel(enet_dev_if_t * enetIfPtr) -{ - enet_config_rx_fifo_t rxFifo; - enet_config_tx_fifo_t txFifo; - - /* Check the input parameters*/ - if (!enetIfPtr) - { - return kStatus_ENET_InvalidInput; - } - - /* Initialize values that will not be initialized later on */ - rxFifo.rxEmpty = 0; - rxFifo.rxFull = 0; - txFifo.isStoreForwardEnabled = 0; - txFifo.txFifoWrite = 0; - txFifo.txEmpty = 0; - - /* Configure tx/rx accelerator*/ - if (enetIfPtr->macCfgPtr->isRxAccelEnabled) - { - enet_hal_config_rx_accelerator(enetIfPtr->deviceNumber, - (enet_config_rx_accelerator_t *)&(enetIfPtr->macCfgPtr->rxAcceler)); - if ((enetIfPtr->macCfgPtr->rxAcceler.isIpcheckEnabled) || (enetIfPtr->macCfgPtr->rxAcceler.isProtocolCheckEnabled)) - { - rxFifo.rxFull = 0; - } - } - if (enetIfPtr->macCfgPtr->isTxAccelEnabled) - { - enet_hal_config_tx_accelerator(enetIfPtr->deviceNumber, - (enet_config_tx_accelerator_t *)&(enetIfPtr->macCfgPtr->txAcceler)); - if ((enetIfPtr->macCfgPtr->txAcceler.isIpCheckEnabled) || (enetIfPtr->macCfgPtr->txAcceler.isProtocolCheckEnabled)) - { - txFifo.isStoreForwardEnabled = 1; - } - } - if (enetIfPtr->macCfgPtr->isStoreAndFwEnabled) - { - rxFifo.rxFull = 0; - txFifo.isStoreForwardEnabled = 1; - } - - - /* Set TFWR value if STRFWD is not being used */ - if (txFifo.isStoreForwardEnabled == 1) - txFifo.txFifoWrite = 0; - else - /* TFWR value is a trade-off between transmit latency and risk of transmit FIFO underrun due to contention for the system bus - TFWR = 15 means transmission will begin once 960 bytes has been written to the Tx FIFO (for frames larger than 960 bytes) - See Section 45.4.18 - Transmit FIFO Watermark Register of the K64F Reference Manual for details */ - txFifo.txFifoWrite = 15; - - /* Configure tx/rx FIFO with default value*/ - rxFifo.rxAlmostEmpty = 4; - rxFifo.rxAlmostFull = 4; - txFifo.txAlmostEmpty = 4; - txFifo.txAlmostFull = 8; - enet_hal_config_rx_fifo(enetIfPtr->deviceNumber, &rxFifo); - enet_hal_config_tx_fifo(enetIfPtr->deviceNumber, &txFifo); - - return kStatus_ENET_Success; -} - -/*FUNCTION**************************************************************** - * - * Function Name: enet_mac_configure_controller - * Return Value: The execution status. - * Description: Configure the ENET controller with the basic configuration. - *END*********************************************************************/ -uint32_t enet_mac_configure_controller(enet_dev_if_t * enetIfPtr) -{ - uint32_t macCtlCfg; - - /* Check the input parameters*/ - if (enetIfPtr == NULL) - { - return kStatus_ENET_InvalidInput; - } - - macCtlCfg = enetIfPtr->macCfgPtr->macCtlConfigure; - /* Configure rmii/mii interface*/ - enet_hal_config_rmii(enetIfPtr->deviceNumber, enetIfPtr->macCfgPtr->rmiiCfgMode, - enetIfPtr->macCfgPtr->speed, enetIfPtr->macCfgPtr->duplex, false, - (macCtlCfg & kEnetRxMiiLoopback)); - /* Configure receive buffer size*/ - if (enetIfPtr->macCfgPtr->isVlanEnabled) - { - enetIfPtr->maxFrameSize = kEnetMaxFrameVlanSize; - enet_hal_set_rx_max_size(enetIfPtr->deviceNumber, - enetIfPtr->macContextPtr->rxBufferSizeAligned, kEnetMaxFrameVlanSize); - } - else - { - enetIfPtr->maxFrameSize = kEnetMaxFrameSize; - enet_hal_set_rx_max_size(enetIfPtr->deviceNumber, - enetIfPtr->macContextPtr->rxBufferSizeAligned, kEnetMaxFrameSize); - } - - /* Set receive controller promiscuous */ - enet_hal_config_promiscuous(enetIfPtr->deviceNumber, macCtlCfg & kEnetRxPromiscuousEnable); - /* Set receive flow control*/ - enet_hal_enable_flowcontrol(enetIfPtr->deviceNumber, (macCtlCfg & kEnetRxFlowControlEnable)); - /* Set received PAUSE frames are forwarded/terminated*/ - enet_hal_enable_pauseforward(enetIfPtr->deviceNumber, (macCtlCfg & kEnetRxPauseFwdEnable)); - /* Set receive broadcast frame reject*/ - enet_hal_enable_broadcastreject(enetIfPtr->deviceNumber, (macCtlCfg & kEnetRxBcRejectEnable)); - /* Set padding is removed from the received frame*/ - enet_hal_enable_padremove(enetIfPtr->deviceNumber, (macCtlCfg & kEnetRxPadRemoveEnable)); - /* Set the crc of the received frame is stripped from the frame*/ - enet_hal_enable_rxcrcforward(enetIfPtr->deviceNumber, (macCtlCfg & kEnetRxCrcFwdEnable)); - /* Set receive payload length check*/ - enet_hal_enable_payloadcheck(enetIfPtr->deviceNumber, (macCtlCfg & kEnetPayloadlenCheckEnable)); - /* Set control sleep mode*/ - enet_hal_enable_sleep(enetIfPtr->deviceNumber, (macCtlCfg & kEnetSleepModeEnable)); - return kStatus_ENET_Success; -} - -/*FUNCTION**************************************************************** - * - * Function Name: enet_mac_init - * Return Value: The execution status. - * Description:Initialize the ENET device with the basic configuration - * When ENET is used, this function need to be called by the NET initialize - * interface. - *END*********************************************************************/ -uint32_t enet_mac_init(enet_dev_if_t * enetIfPtr, enet_rxbd_config_t *rxbdCfg, - enet_txbd_config_t *txbdCfg) -{ - uint32_t timeOut = 0; - uint32_t devNumber, result = 0; - - /* Check the input parameters*/ - if (enetIfPtr == NULL) - { - return kStatus_ENET_InvalidInput; - } - - /* Get device number and check the parameter*/ - devNumber = enetIfPtr->deviceNumber; - - /* Store the global ENET structure for ISR input parameters */ - enetIfHandle = enetIfPtr; - - /* Turn on ENET module clock gate */ - CLOCK_SYS_EnableEnetClock(0U); - - /* Reset ENET mac*/ - enet_hal_reset_ethernet(devNumber); - while ((!enet_hal_is_reset_completed(devNumber)) && (timeOut < kEnetMaxTimeout)) - { - time_delay(1); - timeOut++; - } - - /* Check out if timeout*/ - if (timeOut == kEnetMaxTimeout) - { - return kStatus_ENET_TimeOut; - } - - /* Disable all ENET mac interrupt and Clear all interrupt events*/ - enet_hal_config_interrupt(devNumber, kEnetAllInterrupt, false); - enet_hal_clear_interrupt(devNumber, kEnetAllInterrupt); - - /* Program this station's physical address*/ - enet_hal_set_mac_address(devNumber, enetIfPtr->macCfgPtr->macAddr); - - /* Clear group and individual hash register*/ - enet_hal_set_group_hashtable(devNumber, 0, kEnetSpecialAddressInit); - enet_hal_set_individual_hashtable(devNumber, 0, kEnetSpecialAddressInit); - - /* Configure mac controller*/ - result = enet_mac_configure_controller(enetIfPtr); - if(result != kStatus_ENET_Success) - { - return result; - } - /* Clear mib zero counters*/ - enet_hal_clear_mib(devNumber, true); - - /* Initialize FIFO and accelerator*/ - result = enet_mac_configure_fifo_accel(enetIfPtr); - if(result != kStatus_ENET_Success) - { - return result; - } - /* Initialize receive buffer descriptors*/ - result = enet_mac_rxbd_init(enetIfPtr, rxbdCfg); - if(result != kStatus_ENET_Success) - { - return result; - } - /* Initialize transmit buffer descriptors*/ - result = enet_mac_txbd_init(enetIfPtr, txbdCfg); - if(result != kStatus_ENET_Success) - { - return result; - } - /* Initialize rmii/mii interface*/ - result = enet_mac_mii_init(enetIfPtr); - if (result != kStatus_ENET_Success) - { - return result; - } - - return kStatus_ENET_Success; -} - -/******************************************************************************* - * EOF - ******************************************************************************/ -
--- a/arch/TARGET_Freescale/hardware_init_MK64F12.c Fri Apr 29 01:16:05 2016 +0100 +++ b/arch/TARGET_Freescale/hardware_init_MK64F12.c Tue May 03 00:16:23 2016 +0100 @@ -28,53 +28,57 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "fsl_port_hal.h" -#include "fsl_clock_manager.h" -#include "fsl_device_registers.h" -#include "fsl_sim_hal.h" +#include "fsl_port.h" /******************************************************************************* * Code ******************************************************************************/ void k64f_init_eth_hardware(void) { - uint8_t count; + port_pin_config_t configENET = {0}; + + /* Disable MPU. */ + MPU->CESR &= ~MPU_CESR_VLD_MASK; - /* Disable the mpu*/ - BW_MPU_CESR_VLD(MPU_BASE, 0); - - /* Open POTR clock gate*/ - for (count = 0; count < HW_PORT_INSTANCE_COUNT; count++) - { - CLOCK_SYS_EnablePortClock(count); - } + CLOCK_EnableClock(kCLOCK_PortC); + /* Affects PORTC_PCR16 register */ + PORT_SetPinMux(PORTC, 16u, kPORT_MuxAlt4); + /* Affects PORTC_PCR17 register */ + PORT_SetPinMux(PORTC, 17u, kPORT_MuxAlt4); + /* Affects PORTC_PCR18 register */ + PORT_SetPinMux(PORTC, 18u, kPORT_MuxAlt4); + /* Affects PORTC_PCR19 register */ + PORT_SetPinMux(PORTC, 19u, kPORT_MuxAlt4); + /* Affects PORTB_PCR1 register */ + PORT_SetPinMux(PORTB, 1u, kPORT_MuxAlt4); - /* Configure gpio*/ - PORT_HAL_SetMuxMode(PORTA_BASE, 12, kPortMuxAlt4); /*!< ENET RMII0_RXD1/MII0_RXD1*/ - PORT_HAL_SetMuxMode(PORTA_BASE, 13, kPortMuxAlt4); /*!< ENET RMII0_RXD0/MII0_RXD0*/ - PORT_HAL_SetMuxMode(PORTA_BASE, 14, kPortMuxAlt4); /*!< ENET RMII0_CRS_DV/MII0_RXDV*/ - PORT_HAL_SetMuxMode(PORTA_BASE, 15, kPortMuxAlt4); /*!< ENET RMII0_TXEN/MII0_TXEN*/ - PORT_HAL_SetMuxMode(PORTA_BASE, 16, kPortMuxAlt4); /*!< ENET RMII0_TXD0/MII0_TXD0*/ - PORT_HAL_SetMuxMode(PORTA_BASE, 17, kPortMuxAlt4); /*!< ENET RMII0_TXD01/MII0_TXD1*/ - PORT_HAL_SetMuxMode(PORTB_BASE, 0, kPortMuxAlt4); /*!< ENET RMII0_MDIO/MII0_MDIO*/ - PORT_HAL_SetOpenDrainCmd(PORTB_BASE,0, true); /*!< ENET RMII0_MDC/MII0_MDC*/ + configENET.openDrainEnable = kPORT_OpenDrainEnable; + configENET.mux = kPORT_MuxAlt4; + configENET.pullSelect = kPORT_PullUp; + /* Ungate the port clock */ + CLOCK_EnableClock(kCLOCK_PortA); + /* Affects PORTB_PCR0 register */ + PORT_SetPinConfig(PORTB, 0u, &configENET); - // Added for FRDM-K64F - PORT_HAL_SetPullMode(PORTB_BASE, 0, kPortPullUp); - PORT_HAL_SetPullCmd(PORTB_BASE, 0, true); - - PORT_HAL_SetMuxMode(PORTB_BASE, 1, kPortMuxAlt4); - -#if FSL_FEATURE_ENET_SUPPORT_PTP - PORT_HAL_SetMuxMode(PORTC_BASE, (16 + ENET_TIMER_CHANNEL_NUM), kPortMuxAlt4); /* ENET ENET0_1588_TMR0*/ - PORT_HAL_SetDriveStrengthMode(PORTC_BASE, (16 + ENET_TIMER_CHANNEL_NUM), kPortHighDriveStrength); -#endif + /* Affects PORTA_PCR13 register */ + PORT_SetPinMux(PORTA, 13u, kPORT_MuxAlt4); + /* Affects PORTA_PCR12 register */ + PORT_SetPinMux(PORTA, 12u, kPORT_MuxAlt4); + /* Affects PORTA_PCR14 register */ + PORT_SetPinMux(PORTA, 14u, kPORT_MuxAlt4); + /* Affects PORTA_PCR5 register */ + PORT_SetPinMux(PORTA, 5u, kPORT_MuxAlt4); + /* Affects PORTA_PCR16 register */ + PORT_SetPinMux(PORTA, 16u, kPORT_MuxAlt4); + /* Affects PORTA_PCR17 register */ + PORT_SetPinMux(PORTA, 17u, kPORT_MuxAlt4); + /* Affects PORTA_PCR15 register */ + PORT_SetPinMux(PORTA, 15u, kPORT_MuxAlt4); + /* Affects PORTA_PCR28 register */ + PORT_SetPinMux(PORTA, 28u, kPORT_MuxAlt4); - /* Open ENET clock gate*/ - CLOCK_SYS_EnableEnetClock( 0U); - - /* Select the ptp timer outclk*/ - CLOCK_HAL_SetSource(g_simBaseAddr[0], kClockTimeSrc, 2); + /* Select the Ethernet timestamp clock source */ + CLOCK_SetEnetTime0Clock(0x2); } /*******************************************************************************
--- a/arch/TARGET_Freescale/k64f_emac.c Fri Apr 29 01:16:05 2016 +0100 +++ b/arch/TARGET_Freescale/k64f_emac.c Tue May 03 00:16:23 2016 +0100 @@ -12,11 +12,7 @@ #include "eth_arch.h" #include "sys_arch.h" -#include "fsl_enet_driver.h" -#include "fsl_enet_hal.h" -#include "fsl_device_registers.h" -#include "fsl_phy_driver.h" -#include "fsl_interrupt_manager.h" +#include "fsl_phy.h" #include "k64f_emac_config.h" #include <ctype.h> #include <stdio.h> @@ -25,14 +21,23 @@ #include "mbed_interface.h" -extern IRQn_Type enet_irq_ids[HW_ENET_INSTANCE_COUNT][FSL_FEATURE_ENET_INTERRUPT_COUNT]; -extern uint8_t enetIntMap[kEnetIntNum]; -extern void *enetIfHandle; +enet_handle_t g_handle; +// TX Buffer descriptors +uint8_t *tx_desc_start_addr; +// RX Buffer descriptors +uint8_t *rx_desc_start_addr; +// RX packet buffer pointers +struct pbuf *rx_buff[ENET_RX_RING_LEN]; +// TX packet buffer pointers +struct pbuf *tx_buff[ENET_RX_RING_LEN]; +// RX packet payload pointers +uint32_t *rx_ptr[ENET_RX_RING_LEN]; /******************************************************************************** * Internal data ********************************************************************************/ - +#define ENET_BuffSizeAlign(n) ENET_ALIGN(n, ENET_BUFF_ALIGNMENT) +#define ENET_ALIGN(x,align) ((unsigned int)((x) + ((align)-1)) & (unsigned int)(~(unsigned int)((align)- 1))) extern void k64f_init_eth_hardware(void); /* K64F EMAC driver data structure */ @@ -42,49 +47,11 @@ sys_sem_t TxCleanSem; /**< TX cleanup thread wakeup semaphore */ sys_mutex_t TXLockMutex; /**< TX critical section mutex */ sys_sem_t xTXDCountSem; /**< TX free buffer counting semaphore */ - volatile u32_t rx_free_descs; /**< Count of free RX descriptors */ - struct pbuf *rxb[ENET_RX_RING_LEN]; /**< RX pbuf pointer list, zero-copy mode */ - uint8_t *rx_desc_start_addr; /**< RX descriptor start address */ - uint8_t *tx_desc_start_addr; /**< TX descriptor start address */ uint8_t tx_consume_index, tx_produce_index; /**< TX buffers ring */ - uint8_t rx_fill_index; /**< RX ring fill index */ - struct pbuf *txb[ENET_TX_RING_LEN]; /**< TX pbuf pointer list, zero-copy mode */ - void *txb_aligned[ENET_TX_RING_LEN]; /**< TX aligned buffers (if needed) */ }; static struct k64f_enetdata k64f_enetdata; -static enet_dev_if_t enetDevIf[HW_ENET_INSTANCE_COUNT]; -static enet_mac_config_t g_enetMacCfg[HW_ENET_INSTANCE_COUNT] = -{ - { - ENET_ETH_MAX_FLEN , /*!< enet receive buffer size*/ - ENET_RX_LARGE_BUFFER_NUM, /*!< enet large receive buffer number*/ - ENET_RX_RING_LEN, /*!< enet receive bd number*/ - ENET_TX_RING_LEN, /*!< enet transmit bd number*/ - {0}, /*!< enet mac address*/ - kEnetCfgRmii, /*!< enet rmii interface*/ - kEnetCfgSpeed100M, /*!< enet rmii 100M*/ - kEnetCfgFullDuplex, /*!< enet rmii Full- duplex*/ - /*!< enet mac control flag recommended to use enet_mac_control_flag_t - we send frame with crc so receive crc forward for data length check test*/ - kEnetRxCrcFwdEnable | kEnetRxFlowControlEnable, - true, /*!< enet txaccelerator enabled*/ - true, /*!< enet rxaccelerator enabled*/ - false, /*!< enet store and forward*/ - {false, false, true, false, true}, /*!< enet rxaccelerator config*/ - {false, false, true}, /*!< enet txaccelerator config*/ - true, /*!< vlan frame support*/ - true, /*!< phy auto discover*/ - ENET_MII_CLOCK, /*!< enet MDC clock*/ - }, -}; - -static enet_phy_config_t g_enetPhyCfg[HW_ENET_INSTANCE_COUNT] = -{ - {0, false} -}; - /** \brief Driver transmit and receive thread priorities * * Thread priorities for receive thread and TX cleanup thread. Alter @@ -95,166 +62,35 @@ #define TX_PRIORITY (osPriorityNormal) #define PHY_PRIORITY (osPriorityNormal) -/** \brief Debug output formatter lock define - * - * When using FreeRTOS and with LWIP_DEBUG enabled, enabling this - * define will allow RX debug messages to not interleave with the - * TX messages (so they are actually readable). Not enabling this - * define when the system is under load will cause the output to - * be unreadable. There is a small tradeoff in performance for this - * so use it only for debug. */ -//#define LOCK_RX_THREAD - -/** \brief Signal used for ethernet ISR to signal packet_rx() thread. - */ -#define RX_SIGNAL 1 - -// K64F-specific macros -#define RX_PBUF_AUTO_INDEX (-1) - /******************************************************************************** * Buffer management ********************************************************************************/ - -/** \brief Queues a pbuf into the RX descriptor list - * - * \param[in] k64f_enet Pointer to the drvier data structure - * \param[in] p Pointer to pbuf to queue - * \param[in] bidx Index to queue into +/* + * This function will queue a new receive buffer */ -static void k64f_rxqueue_pbuf(struct k64f_enetdata *k64f_enet, struct pbuf *p, int bidx) +static void update_read_buffer(uint8_t *buf) { - enet_bd_struct_t *start = (enet_bd_struct_t *)k64f_enet->rx_desc_start_addr; - int idx; - - /* Get next free descriptor index */ - if (bidx == RX_PBUF_AUTO_INDEX) - idx = k64f_enet->rx_fill_index; - else - idx = bidx; - - /* Setup descriptor and clear statuses */ - enet_hal_init_rxbds(start + idx, (uint8_t*)p->payload, idx == ENET_RX_RING_LEN - 1); - - /* Save pbuf pointer for push to network layer later */ - k64f_enet->rxb[idx] = p; - - /* Wrap at end of descriptor list */ - idx = (idx + 1) % ENET_RX_RING_LEN; - - /* Queue descriptor(s) */ - k64f_enet->rx_free_descs -= 1; - - if (bidx == RX_PBUF_AUTO_INDEX) - k64f_enet->rx_fill_index = idx; - - enet_hal_active_rxbd(BOARD_DEBUG_ENET_INSTANCE_ADDR); - - LWIP_DEBUGF(UDP_LPC_EMAC | LWIP_DBG_TRACE, - ("k64f_rxqueue_pbuf: pbuf packet queued: %p (free desc=%d)\n", p, - k64f_enet->rx_free_descs)); -} - -/** \brief Attempt to allocate and requeue a new pbuf for RX - * - * \param[in] netif Pointer to the netif structure - * \returns number of queued packets - */ -s32_t k64f_rx_queue(struct netif *netif, int idx) -{ - struct k64f_enetdata *k64f_enet = netif->state; - enet_dev_if_t *enetIfPtr = (enet_dev_if_t *)&enetDevIf[BOARD_DEBUG_ENET_INSTANCE]; - struct pbuf *p; - int queued = 0; - - /* Attempt to requeue as many packets as possible */ - while (k64f_enet->rx_free_descs > 0) { - /* Allocate a pbuf from the pool. We need to allocate at the - maximum size as we don't know the size of the yet to be - received packet. */ - p = pbuf_alloc(PBUF_RAW, enetIfPtr->macCfgPtr->rxBufferSize + RX_BUF_ALIGNMENT, PBUF_RAM); - if (p == NULL) { - LWIP_DEBUGF(UDP_LPC_EMAC | LWIP_DBG_TRACE, - ("k64_rx_queue: could not allocate RX pbuf (free desc=%d)\n", - k64f_enet->rx_free_descs)); - return queued; + if (buf != NULL) { + g_handle.rxBdCurrent->buffer = buf; } - /* K64F note: the next line ensures that the RX buffer is properly aligned for the K64F - RX descriptors (16 bytes alignment). However, by doing so, we're effectively changing - a data structure which is internal to lwIP. This might not prove to be a good idea - in the long run, but a better fix would probably involve modifying lwIP itself */ - p->payload = (void*)ENET_ALIGN((uint32_t)p->payload, RX_BUF_ALIGNMENT); - - /* pbufs allocated from the RAM pool should be non-chained. */ - LWIP_ASSERT("k64f_rx_queue: pbuf is not contiguous (chained)", pbuf_clen(p) <= 1); - - /* Queue packet */ - k64f_rxqueue_pbuf(k64f_enet, p, idx); - queued++; - } - - return queued; -} -/** \brief Sets up the RX descriptor ring buffers. - * - * This function sets up the descriptor list used for receive packets. - * - * \param[in] netif Pointer to driver data structure - * \returns ERR_MEM if out of memory, ERR_OK otherwise - */ -static err_t k64f_rx_setup(struct netif *netif, enet_rxbd_config_t *rxbdCfg) { - struct k64f_enetdata *k64f_enet = netif->state; - enet_dev_if_t *enetIfPtr = (enet_dev_if_t *)&enetDevIf[BOARD_DEBUG_ENET_INSTANCE]; - uint8_t *rxBdPtr; - uint32_t rxBufferSizeAligned; + /* Clears status. */ + g_handle.rxBdCurrent->control &= ENET_BUFFDESCRIPTOR_RX_WRAP_MASK; - // Allocate RX descriptors - rxBdPtr = (uint8_t *)calloc(1, enet_hal_get_bd_size() * enetIfPtr->macCfgPtr->rxBdNumber + ENET_BD_ALIGNMENT); - if(!rxBdPtr) - return ERR_MEM; - k64f_enet->rx_desc_start_addr = (uint8_t *)ENET_ALIGN((uint32_t)rxBdPtr, ENET_BD_ALIGNMENT); - k64f_enet->rx_free_descs = enetIfPtr->macCfgPtr->rxBdNumber; - k64f_enet->rx_fill_index = 0; + /* Sets the receive buffer descriptor with the empty flag. */ + g_handle.rxBdCurrent->control |= ENET_BUFFDESCRIPTOR_RX_EMPTY_MASK; - rxBufferSizeAligned = ENET_ALIGN(enetIfPtr->macCfgPtr->rxBufferSize, ENET_RX_BUFFER_ALIGNMENT); - enetIfPtr->macContextPtr->rxBufferSizeAligned = rxBufferSizeAligned; - rxbdCfg->rxBdPtrAlign = k64f_enet->rx_desc_start_addr; - rxbdCfg->rxBdNum = enetIfPtr->macCfgPtr->rxBdNumber; - rxbdCfg->rxBufferNum = enetIfPtr->macCfgPtr->rxBdNumber; - - k64f_rx_queue(netif, RX_PBUF_AUTO_INDEX); - return ERR_OK; -} + /* Increases the buffer descriptor to the next one. */ + if (g_handle.rxBdCurrent->control & ENET_BUFFDESCRIPTOR_RX_WRAP_MASK) { + g_handle.rxBdCurrent = g_handle.rxBdBase; + g_handle.rxBdDirty = g_handle.rxBdBase; + } else { + g_handle.rxBdCurrent++; + g_handle.rxBdDirty++; + } -/** \brief Sets up the TX descriptor ring buffers. - * - * This function sets up the descriptor list used for transmit packets. - * - * \param[in] netif Pointer to driver data structure - * \returns ERR_MEM if out of memory, ERR_OK otherwise - */ -static err_t k64f_tx_setup(struct netif *netif, enet_txbd_config_t *txbdCfg) { - struct k64f_enetdata *k64f_enet = netif->state; - enet_dev_if_t *enetIfPtr = (enet_dev_if_t *)&enetDevIf[BOARD_DEBUG_ENET_INSTANCE]; - uint8_t *txBdPtr; - - // Allocate TX descriptors - txBdPtr = (uint8_t *)calloc(1, enet_hal_get_bd_size() * enetIfPtr->macCfgPtr->txBdNumber + ENET_BD_ALIGNMENT); - if(!txBdPtr) - return ERR_MEM; - - k64f_enet->tx_desc_start_addr = (uint8_t *)ENET_ALIGN((uint32_t)txBdPtr, ENET_BD_ALIGNMENT); - k64f_enet->tx_consume_index = k64f_enet->tx_produce_index = 0; - - txbdCfg->txBdPtrAlign = k64f_enet->tx_desc_start_addr; - txbdCfg->txBufferNum = enetIfPtr->macCfgPtr->txBdNumber; - txbdCfg->txBufferSizeAlign = ENET_ALIGN(enetIfPtr->maxFrameSize, ENET_TX_BUFFER_ALIGNMENT); - - // Make the TX descriptor ring circular - enet_hal_init_txbds(k64f_enet->tx_desc_start_addr + enet_hal_get_bd_size() * (ENET_TX_RING_LEN - 1), 1); - - return ERR_OK; + /* Actives the receive buffer descriptor. */ + ENET->RDAR = ENET_RDAR_RDAR_MASK; } /** \brief Free TX buffers that are complete @@ -263,125 +99,141 @@ */ static void k64f_tx_reclaim(struct k64f_enetdata *k64f_enet) { - uint8_t i; - volatile enet_bd_struct_t * bdPtr = (enet_bd_struct_t *)k64f_enet->tx_desc_start_addr; + uint8_t i = 0 ; /* Get exclusive access */ sys_mutex_lock(&k64f_enet->TXLockMutex); + i = k64f_enet->tx_consume_index; // Traverse all descriptors, looking for the ones modified by the uDMA - i = k64f_enet->tx_consume_index; - while(i != k64f_enet->tx_produce_index && !(bdPtr[i].control & kEnetTxBdReady)) { - if (k64f_enet->txb_aligned[i]) { - free(k64f_enet->txb_aligned[i]); - k64f_enet->txb_aligned[i] = NULL; - } else if (k64f_enet->txb[i]) { - pbuf_free(k64f_enet->txb[i]); - k64f_enet->txb[i] = NULL; - } - osSemaphoreRelease(k64f_enet->xTXDCountSem.id); - bdPtr[i].controlExtend2 &= ~TX_DESC_UPDATED_MASK; + while((i != k64f_enet->tx_produce_index) && (!(g_handle.txBdDirty->control & ENET_BUFFDESCRIPTOR_TX_READY_MASK))) { + pbuf_free(tx_buff[i]); + if (g_handle.txBdDirty->control & ENET_BUFFDESCRIPTOR_TX_WRAP_MASK) + g_handle.txBdDirty = g_handle.txBdBase; + else + g_handle.txBdDirty++; + i = (i + 1) % ENET_TX_RING_LEN; } + k64f_enet->tx_consume_index = i; - /* Restore access */ sys_mutex_unlock(&k64f_enet->TXLockMutex); } +/** \brief Ethernet receive interrupt handler + * + * This function handles the receive interrupt of K64F. + */ +void enet_mac_rx_isr() +{ + sys_sem_signal(&k64f_enetdata.RxReadySem); +} + +void enet_mac_tx_isr() +{ + sys_sem_signal(&k64f_enetdata.TxCleanSem); +} + +void ethernet_callback(ENET_Type *base, enet_handle_t *handle, enet_event_t event, void *param) +{ + switch (event) + { + case kENET_RxEvent: + enet_mac_rx_isr(); + break; + case kENET_TxEvent: + enet_mac_tx_isr(); + break; + default: + break; + } +} + /** \brief Low level init of the MAC and PHY. * * \param[in] netif Pointer to LWIP netif structure */ static err_t low_level_init(struct netif *netif) { - enet_dev_if_t * enetIfPtr; - uint32_t device = BOARD_DEBUG_ENET_INSTANCE_ADDR; - enet_rxbd_config_t rxbdCfg; - enet_txbd_config_t txbdCfg; - enet_phy_speed_t phy_speed; - enet_phy_duplex_t phy_duplex; + struct k64f_enetdata *k64f_enet = netif->state; + uint8_t i; + uint32_t sysClock; + phy_speed_t phy_speed; + phy_duplex_t phy_duplex; + uint32_t phyAddr = 0; + bool link = false; + enet_config_t config; + + // Allocate RX descriptors + rx_desc_start_addr = (uint8_t *)calloc(1, sizeof(enet_rx_bd_struct_t) * ENET_RX_RING_LEN + ENET_BUFF_ALIGNMENT); + if(!rx_desc_start_addr) + return ERR_MEM; - k64f_init_eth_hardware(); - - /* Initialize device*/ - enetIfPtr = (enet_dev_if_t *)&enetDevIf[BOARD_DEBUG_ENET_INSTANCE]; - enetIfPtr->deviceNumber = device; - enetIfPtr->macCfgPtr = &g_enetMacCfg[BOARD_DEBUG_ENET_INSTANCE]; - enetIfPtr->phyCfgPtr = &g_enetPhyCfg[BOARD_DEBUG_ENET_INSTANCE]; - enetIfPtr->macApiPtr = &g_enetMacApi; - enetIfPtr->phyApiPtr = (void *)&g_enetPhyApi; - memcpy(enetIfPtr->macCfgPtr->macAddr, (char*)netif->hwaddr, kEnetMacAddrLen); + // Allocate TX descriptors + tx_desc_start_addr = (uint8_t *)calloc(1, sizeof(enet_tx_bd_struct_t) * ENET_TX_RING_LEN + ENET_BUFF_ALIGNMENT); + if(!tx_desc_start_addr) + return ERR_MEM; + + rx_desc_start_addr = (uint8_t *)ENET_ALIGN(rx_desc_start_addr, ENET_BUFF_ALIGNMENT); + tx_desc_start_addr = (uint8_t *)ENET_ALIGN(tx_desc_start_addr, ENET_BUFF_ALIGNMENT); - /* Allocate buffer for ENET mac context*/ - enetIfPtr->macContextPtr = (enet_mac_context_t *)calloc(1, sizeof(enet_mac_context_t)); - if (!enetIfPtr->macContextPtr) { - return ERR_BUF; + /* Create buffers for each receive BD */ + for (i = 0; i < ENET_RX_RING_LEN; i++) { + rx_buff[i] = pbuf_alloc(PBUF_RAW, ENET_ETH_MAX_FLEN + ENET_BUFF_ALIGNMENT, PBUF_RAM); + if (NULL == rx_buff[i]) + return ERR_MEM; + + /* K64F note: the next line ensures that the RX buffer is properly aligned for the K64F + RX descriptors (16 bytes alignment). However, by doing so, we're effectively changing + a data structure which is internal to lwIP. This might not prove to be a good idea + in the long run, but a better fix would probably involve modifying lwIP itself */ + rx_buff[i]->payload = (void*)ENET_ALIGN((uint32_t)rx_buff[i]->payload, ENET_BUFF_ALIGNMENT); + rx_ptr[i] = rx_buff[i]->payload; } - /* Initialize enet buffers*/ - if(k64f_rx_setup(netif, &rxbdCfg) != ERR_OK) { - return ERR_BUF; - } - /* Initialize enet buffers*/ - if(k64f_tx_setup(netif, &txbdCfg) != ERR_OK) { - return ERR_BUF; - } - /* Initialize enet module*/ - if (enet_mac_init(enetIfPtr, &rxbdCfg, &txbdCfg) == kStatus_ENET_Success) - { - /* Initialize PHY*/ - if (enetIfPtr->macCfgPtr->isPhyAutoDiscover) { - if (((enet_phy_api_t *)(enetIfPtr->phyApiPtr))->phy_auto_discover(enetIfPtr) != kStatus_PHY_Success) - return ERR_IF; - } - if (((enet_phy_api_t *)(enetIfPtr->phyApiPtr))->phy_init(enetIfPtr) != kStatus_PHY_Success) - return ERR_IF; + k64f_enet->tx_consume_index = k64f_enet->tx_produce_index = 0; + + /* prepare the buffer configuration. */ + enet_buffer_config_t buffCfg = { + ENET_RX_RING_LEN, + ENET_TX_RING_LEN, + ENET_ALIGN(ENET_ETH_MAX_FLEN, ENET_BUFF_ALIGNMENT), + 0, + (volatile enet_rx_bd_struct_t *)rx_desc_start_addr, + (volatile enet_tx_bd_struct_t *)tx_desc_start_addr, + (uint8_t *)&rx_ptr, + NULL, + }; + + k64f_init_eth_hardware(); + + sysClock = CLOCK_GetFreq(kCLOCK_CoreSysClk); - enetIfPtr->isInitialized = true; - } - else - { - // TODOETH: cleanup memory - return ERR_IF; - } + ENET_GetDefaultConfig(&config); - /* Get link information from PHY */ - phy_get_link_speed(enetIfPtr, &phy_speed); - phy_get_link_duplex(enetIfPtr, &phy_duplex); - BW_ENET_RCR_RMII_10T(enetIfPtr->deviceNumber, phy_speed == kEnetSpeed10M ? kEnetCfgSpeed10M : kEnetCfgSpeed100M); - BW_ENET_TCR_FDEN(enetIfPtr->deviceNumber, phy_duplex == kEnetFullDuplex ? kEnetCfgFullDuplex : kEnetCfgHalfDuplex); - - /* Enable Ethernet module*/ - enet_hal_config_ethernet(BOARD_DEBUG_ENET_INSTANCE_ADDR, true, true); - - /* Active Receive buffer descriptor must be done after module enable*/ - enet_hal_active_rxbd(enetIfPtr->deviceNumber); + PHY_Init(ENET, 0, sysClock); + PHY_GetLinkStatus(ENET, phyAddr, &link); + if (link) + { + /* Get link information from PHY */ + PHY_GetLinkSpeedDuplex(ENET, phyAddr, &phy_speed, &phy_duplex); + /* Change the MII speed and duplex for actual link status. */ + config.miiSpeed = (enet_mii_speed_t)phy_speed; + config.miiDuplex = (enet_mii_duplex_t)phy_duplex; + config.interrupt = kENET_RxFrameInterrupt | kENET_TxFrameInterrupt; + } + config.rxMaxFrameLen = ENET_ETH_MAX_FLEN; + config.macSpecialConfig = kENET_ControlFlowControlEnable; + config.txAccelerConfig = kENET_TxAccelIsShift16Enabled; + config.rxAccelerConfig = kENET_RxAccelisShift16Enabled | kENET_RxAccelMacCheckEnabled; + ENET_Init(ENET, &g_handle, &config, &buffCfg, netif->hwaddr, sysClock); + ENET_SetCallback(&g_handle, ethernet_callback, netif); + ENET_ActiveRead(ENET); return ERR_OK; } -/******************************************************************************** - * LWIP port - ********************************************************************************/ - -/** \brief Ethernet receive interrupt handler - * - * This function handles the receive interrupt of K64F. - */ -void enet_mac_rx_isr(void *enetIfPtr) -{ - /* Clear interrupt */ - enet_hal_clear_interrupt(((enet_dev_if_t *)enetIfPtr)->deviceNumber, kEnetRxFrameInterrupt); - sys_sem_signal(&k64f_enetdata.RxReadySem); -} - -void enet_mac_tx_isr(void *enetIfPtr) -{ - /*Clear interrupt*/ - enet_hal_clear_interrupt(((enet_dev_if_t *)enetIfPtr)->deviceNumber, kEnetTxFrameInterrupt); - sys_sem_signal(&k64f_enetdata.TxCleanSem); -} /** * This function is the ethernet packet send function. It calls @@ -409,11 +261,13 @@ */ static struct pbuf *k64f_low_level_input(struct netif *netif, int idx) { - struct k64f_enetdata *k64f_enet = netif->state; - enet_bd_struct_t * bdPtr = (enet_bd_struct_t*)k64f_enet->rx_desc_start_addr; + volatile enet_rx_bd_struct_t *bdPtr = g_handle.rxBdCurrent; struct pbuf *p = NULL; - u32_t length = 0, orig_length; - const u16_t err_mask = kEnetRxBdTrunc | kEnetRxBdCrc | kEnetRxBdNoOctet | kEnetRxBdLengthViolation; + struct pbuf *temp_rxbuf = NULL; + u32_t length = 0; + const u16_t err_mask = ENET_BUFFDESCRIPTOR_RX_TRUNC_MASK | ENET_BUFFDESCRIPTOR_RX_CRC_MASK | + ENET_BUFFDESCRIPTOR_RX_NOOCTET_MASK | ENET_BUFFDESCRIPTOR_RX_LENVLIOLATE_MASK; + #ifdef LOCK_RX_THREAD /* Get exclusive access */ @@ -421,42 +275,32 @@ #endif /* Determine if a frame has been received */ - if ((bdPtr[idx].control & err_mask) != 0) { + if ((bdPtr->control & err_mask) != 0) { #if LINK_STATS - if ((bdPtr[idx].control & kEnetRxBdLengthViolation) != 0) + if ((bdPtr->control & ENET_BUFFDESCRIPTOR_RX_LENVLIOLATE_MASK) != 0) LINK_STATS_INC(link.lenerr); else LINK_STATS_INC(link.chkerr); #endif LINK_STATS_INC(link.drop); - - /* Re-queue the same buffer */ - k64f_enet->rx_free_descs++; - p = k64f_enet->rxb[idx]; - k64f_enet->rxb[idx] = NULL; - k64f_rxqueue_pbuf(k64f_enet, p, idx); - p = NULL; + /* Re-use the same buffer in case of error */ + update_read_buffer(NULL); } else { /* A packet is waiting, get length */ - length = enet_hal_get_bd_length(bdPtr + idx); + length = bdPtr->length; /* Zero-copy */ - p = k64f_enet->rxb[idx]; - orig_length = p->len; - p->len = (u16_t) length; + p = rx_buff[idx]; + p->len = length; - /* Free pbuf from descriptor */ - k64f_enet->rxb[idx] = NULL; - k64f_enet->rx_free_descs++; - /* Attempt to queue new buffer */ - if (k64f_rx_queue(netif, idx) == 0) { + temp_rxbuf = pbuf_alloc(PBUF_RAW, ENET_ETH_MAX_FLEN + ENET_BUFF_ALIGNMENT, PBUF_RAM); + if (NULL == temp_rxbuf) { /* Drop frame (out of memory) */ LINK_STATS_INC(link.drop); /* Re-queue the same buffer */ - p->len = orig_length; - k64f_rxqueue_pbuf(k64f_enet, p, idx); + update_read_buffer(NULL); LWIP_DEBUGF(UDP_LPC_EMAC | LWIP_DBG_TRACE, ("k64f_low_level_input: Packet index %d dropped for OOM\n", @@ -468,6 +312,15 @@ return NULL; } + rx_buff[idx] = temp_rxbuf; + /* K64F note: the next line ensures that the RX buffer is properly aligned for the K64F + RX descriptors (16 bytes alignment). However, by doing so, we're effectively changing + a data structure which is internal to lwIP. This might not prove to be a good idea + in the long run, but a better fix would probably involve modifying lwIP itself */ + rx_buff[idx]->payload = (void*)ENET_ALIGN((uint32_t)rx_buff[idx]->payload, ENET_BUFF_ALIGNMENT); + rx_ptr[idx] = rx_buff[idx]->payload; + + update_read_buffer(rx_buff[idx]->payload); LWIP_DEBUGF(UDP_LPC_EMAC | LWIP_DBG_TRACE, ("k64f_low_level_input: Packet received: %p, size %d (index=%d)\n", p, length, idx)); @@ -533,14 +386,13 @@ */ static void packet_rx(void* pvParameters) { struct k64f_enetdata *k64f_enet = pvParameters; - volatile enet_bd_struct_t * bdPtr = (enet_bd_struct_t*)k64f_enet->rx_desc_start_addr; int idx = 0; while (1) { /* Wait for receive task to wakeup */ sys_arch_sem_wait(&k64f_enet->RxReadySem, 0); - while ((bdPtr[idx].control & kEnetRxBdEmpty) == 0) { + while ((g_handle.rxBdCurrent->control & ENET_BUFFDESCRIPTOR_RX_EMPTY_MASK) == 0) { k64f_enetif_input(k64f_enet->netif, idx); idx = (idx + 1) % ENET_RX_RING_LEN; } @@ -561,58 +413,10 @@ while (1) { /* Wait for transmit cleanup task to wakeup */ sys_arch_sem_wait(&k64f_enet->TxCleanSem, 0); - // TODOETH: handle TX underrun? k64f_tx_reclaim(k64f_enet); } } - /** \brief Polls if an available TX descriptor is ready. Can be used to - * determine if the low level transmit function will block. - * - * \param[in] netif the lwip network interface structure - * \return 0 if no descriptors are read, or >0 - */ -s32_t k64f_tx_ready(struct netif *netif) -{ - struct k64f_enetdata *k64f_enet = netif->state; - s32_t fb; - u32_t idx, cidx; - - cidx = k64f_enet->tx_consume_index; - idx = k64f_enet->tx_produce_index; - - /* Determine number of free buffers */ - if (idx == cidx) - fb = ENET_TX_RING_LEN; - else if (cidx > idx) - fb = (ENET_TX_RING_LEN - 1) - - ((idx + ENET_TX_RING_LEN) - cidx); - else - fb = (ENET_TX_RING_LEN - 1) - (cidx - idx); - - return fb; -} - -/*FUNCTION**************************************************************** - * - * Function Name: enet_hal_update_txbds - * Description: Update ENET transmit buffer descriptors. - *END*********************************************************************/ -void k64f_update_txbds(struct k64f_enetdata *k64f_enet, int idx, uint8_t *buffer, uint16_t length, bool isLast) -{ - volatile enet_bd_struct_t * bdPtr = (enet_bd_struct_t *)(k64f_enet->tx_desc_start_addr + idx * enet_hal_get_bd_size()); - - bdPtr->length = HTONS(length); /* Set data length*/ - bdPtr->buffer = (uint8_t *)HTONL((uint32_t)buffer); /* Set data buffer*/ - if (isLast) - bdPtr->control |= kEnetTxBdLast; - else - bdPtr->control &= ~kEnetTxBdLast; - bdPtr->controlExtend1 |= kEnetTxBdTxInterrupt; - bdPtr->controlExtend2 &= ~TX_DESC_UPDATED_MASK; // descriptor not updated by DMA - bdPtr->control |= kEnetTxBdTransmitCrc | kEnetTxBdReady; -} - /** \brief Low level output of a packet. Never call this from an * interrupt context, as it may block until TX descriptors * become available. @@ -625,83 +429,51 @@ { struct k64f_enetdata *k64f_enet = netif->state; struct pbuf *q; - u32_t idx; - s32_t dn; + struct pbuf *temp_pbuf; uint8_t *psend = NULL, *dst; - /* Get free TX buffer index */ - idx = k64f_enet->tx_produce_index; - - /* Check the pbuf chain for payloads that are not 8-byte aligned. - If found, a new properly aligned buffer needs to be allocated - and the data copied there */ - for (q = p; q != NULL; q = q->next) - if (((u32_t)q->payload & (TX_BUF_ALIGNMENT - 1)) != 0) - break; - if (q != NULL) { - // Allocate properly aligned buffer - psend = (uint8_t*)malloc(p->tot_len); - if (NULL == psend) - return ERR_MEM; - LWIP_ASSERT("k64f_low_level_output: buffer not properly aligned", ((u32_t)psend & (TX_BUF_ALIGNMENT - 1)) == 0); - for (q = p, dst = psend; q != NULL; q = q->next) { - MEMCPY(dst, q->payload, q->len); - dst += q->len; - } - k64f_enet->txb_aligned[idx] = psend; - dn = 1; - } else { - k64f_enet->txb_aligned[idx] = NULL; - dn = (s32_t) pbuf_clen(p); - pbuf_ref(p); + + temp_pbuf = pbuf_alloc(PBUF_RAW, p->tot_len + ENET_BUFF_ALIGNMENT, PBUF_RAM); + if (NULL == temp_pbuf) + return ERR_MEM; + + /* K64F note: the next line ensures that the RX buffer is properly aligned for the K64F + RX descriptors (16 bytes alignment). However, by doing so, we're effectively changing + a data structure which is internal to lwIP. This might not prove to be a good idea + in the long run, but a better fix would probably involve modifying lwIP itself */ + psend = (uint8_t *)ENET_ALIGN((uint32_t)temp_pbuf->payload, ENET_BUFF_ALIGNMENT); + + for (q = p, dst = psend; q != NULL; q = q->next) { + MEMCPY(dst, q->payload, q->len); + dst += q->len; } - /* Wait until enough descriptors are available for the transfer. */ - /* THIS WILL BLOCK UNTIL THERE ARE ENOUGH DESCRIPTORS AVAILABLE */ - while (dn > k64f_tx_ready(netif)) + /* Wait until a descriptor is available for the transfer. */ + /* THIS WILL BLOCK UNTIL THERE ARE A DESCRIPTOR AVAILABLE */ + while (g_handle.txBdCurrent->control & ENET_BUFFDESCRIPTOR_TX_READY_MASK) osSemaphoreWait(k64f_enet->xTXDCountSem.id, osWaitForever); /* Get exclusive access */ sys_mutex_lock(&k64f_enet->TXLockMutex); + /* Save the buffer so that it can be freed when transmit is done */ + tx_buff[k64f_enet->tx_produce_index] = temp_pbuf; + k64f_enet->tx_produce_index = (k64f_enet->tx_produce_index + 1) % ENET_TX_RING_LEN; + /* Setup transfers */ - q = p; - while (dn > 0) { - dn--; - if (psend != NULL) { - k64f_update_txbds(k64f_enet, idx, psend, p->tot_len, 1); - k64f_enet->txb[idx] = NULL; - - LWIP_DEBUGF(UDP_LPC_EMAC | LWIP_DBG_TRACE, - ("k64f_low_level_output: aligned packet(%p) sent" - " size = %d (index=%d)\n", psend, p->tot_len, idx)); - } else { - LWIP_ASSERT("k64f_low_level_output: buffer not properly aligned", ((u32_t)q->payload & 0x07) == 0); + g_handle.txBdCurrent->buffer = psend; + g_handle.txBdCurrent->length = p->tot_len; + g_handle.txBdCurrent->control |= (ENET_BUFFDESCRIPTOR_TX_READY_MASK | ENET_BUFFDESCRIPTOR_TX_LAST_MASK); - /* Only save pointer to free on last descriptor */ - if (dn == 0) { - /* Save size of packet and signal it's ready */ - k64f_update_txbds(k64f_enet, idx, q->payload, q->len, 1); - k64f_enet->txb[idx] = p; - } - else { - /* Save size of packet, descriptor is not last */ - k64f_update_txbds(k64f_enet, idx, q->payload, q->len, 0); - k64f_enet->txb[idx] = NULL; - } - - LWIP_DEBUGF(UDP_LPC_EMAC | LWIP_DBG_TRACE, - ("k64f_low_level_output: pbuf packet(%p) sent, chain#=%d," - " size = %d (index=%d)\n", q->payload, dn, q->len, idx)); - } + /* Increase the buffer descriptor address. */ + if (g_handle.txBdCurrent->control & ENET_BUFFDESCRIPTOR_TX_WRAP_MASK) + g_handle.txBdCurrent = g_handle.txBdBase; + else + g_handle.txBdCurrent++; - q = q->next; + /* Active the transmit buffer descriptor. */ + ENET->TDAR = ENET_TDAR_TDAR_MASK; - idx = (idx + 1) % ENET_TX_RING_LEN; - } - - k64f_enet->tx_produce_index = idx; - enet_hal_active_txbd(BOARD_DEBUG_ENET_INSTANCE_ADDR); LINK_STATS_INC(link.xmit); /* Restore access */ @@ -719,31 +491,33 @@ typedef struct { int connected; - enet_phy_speed_t speed; - enet_phy_duplex_t duplex; + phy_speed_t speed; + phy_duplex_t duplex; } PHY_STATE; int phy_link_status() { bool connection_status; - enet_dev_if_t * enetIfPtr = (enet_dev_if_t*)&enetDevIf[BOARD_DEBUG_ENET_INSTANCE]; - phy_get_link_status(enetIfPtr, &connection_status); + uint32_t phyAddr = 0; + + PHY_GetLinkStatus(ENET, phyAddr, &connection_status); return (int)connection_status; } static void k64f_phy_task(void *data) { struct netif *netif = (struct netif*)data; bool connection_status; - enet_dev_if_t * enetIfPtr = (enet_dev_if_t*)&enetDevIf[BOARD_DEBUG_ENET_INSTANCE]; - PHY_STATE crt_state = {STATE_UNKNOWN, (enet_phy_speed_t)STATE_UNKNOWN, (enet_phy_duplex_t)STATE_UNKNOWN}; + PHY_STATE crt_state = {STATE_UNKNOWN, (phy_speed_t)STATE_UNKNOWN, (phy_duplex_t)STATE_UNKNOWN}; PHY_STATE prev_state; + uint32_t phyAddr = 0; + uint32_t rcr = 0; prev_state = crt_state; while (true) { // Get current status - phy_get_link_status(enetIfPtr, &connection_status); + PHY_GetLinkStatus(ENET, phyAddr, &connection_status); crt_state.connected = connection_status ? 1 : 0; - phy_get_link_speed(enetIfPtr, &crt_state.speed); - phy_get_link_duplex(enetIfPtr, &crt_state.duplex); + // Get the actual PHY link speed + PHY_GetLinkSpeedDuplex(ENET, phyAddr, &crt_state.speed, &crt_state.duplex); // Compare with previous state if (crt_state.connected != prev_state.connected) { @@ -753,10 +527,12 @@ tcpip_callback_with_block((tcpip_callback_fn)netif_set_link_down, (void*) netif, 1); } - if (crt_state.speed != prev_state.speed) - BW_ENET_RCR_RMII_10T(enetIfPtr->deviceNumber, crt_state.speed == kEnetSpeed10M ? kEnetCfgSpeed10M : kEnetCfgSpeed100M); - - // TODO: duplex change requires disable/enable of Ethernet interface, to be implemented + if (crt_state.speed != prev_state.speed) { + rcr = ENET->RCR; + rcr &= ~ENET_RCR_RMII_10T_MASK; + rcr |= ENET_RCR_RMII_10T(!crt_state.speed); + ENET->RCR = rcr; + } prev_state = crt_state; osDelay(PHY_TASK_PERIOD_MS); @@ -851,32 +627,14 @@ } void eth_arch_enable_interrupts(void) { - enet_hal_config_interrupt(BOARD_DEBUG_ENET_INSTANCE_ADDR, (kEnetTxFrameInterrupt | kEnetRxFrameInterrupt), true); - INT_SYS_EnableIRQ(enet_irq_ids[BOARD_DEBUG_ENET_INSTANCE][enetIntMap[kEnetRxfInt]]); - INT_SYS_EnableIRQ(enet_irq_ids[BOARD_DEBUG_ENET_INSTANCE][enetIntMap[kEnetTxfInt]]); + //NVIC_SetPriority(ENET_Receive_IRQn, 6U); + //NVIC_SetPriority(ENET_Transmit_IRQn, 6U); } void eth_arch_disable_interrupts(void) { - INT_SYS_DisableIRQ(enet_irq_ids[BOARD_DEBUG_ENET_INSTANCE][enetIntMap[kEnetRxfInt]]); - INT_SYS_DisableIRQ(enet_irq_ids[BOARD_DEBUG_ENET_INSTANCE][enetIntMap[kEnetTxfInt]]); -} -void ENET_Transmit_IRQHandler(void) -{ - enet_mac_tx_isr(enetIfHandle); } -void ENET_Receive_IRQHandler(void) -{ - enet_mac_rx_isr(enetIfHandle); -} - -#if FSL_FEATURE_ENET_SUPPORT_PTP -void ENET_1588_Timer_IRQHandler(void) -{ - enet_mac_ts_isr(enetIfHandle); -} -#endif /** * @} */
--- a/arch/TARGET_Freescale/k64f_emac_config.h Fri Apr 29 01:16:05 2016 +0100 +++ b/arch/TARGET_Freescale/k64f_emac_config.h Tue May 03 00:16:23 2016 +0100 @@ -29,18 +29,11 @@ */ #ifndef K64F_EMAC_CONFIG_H__ #define K64F_EMAC_CONFIG_H__ - + +#include "fsl_enet.h" + #define ENET_RX_RING_LEN (16) #define ENET_TX_RING_LEN (8) -#define ENET_RX_LARGE_BUFFER_NUM (0) -#define ENET_RX_BUFFER_ALIGNMENT (16) -#define ENET_TX_BUFFER_ALIGNMENT (16) -#define ENET_BD_ALIGNMENT (16) -#define ENET_MII_CLOCK (2500000L) -#define RX_BUF_ALIGNMENT (16) -#define TX_BUF_ALIGNMENT (8) -#define BOARD_DEBUG_ENET_INSTANCE (0) -#define BOARD_DEBUG_ENET_INSTANCE_ADDR (ENET_BASE) #define ENET_ETH_MAX_FLEN (1522) // recommended size for a VLAN frame
--- a/arch/TARGET_Freescale/lwipopts_conf.h Fri Apr 29 01:16:05 2016 +0100 +++ b/arch/TARGET_Freescale/lwipopts_conf.h Tue May 03 00:16:23 2016 +0100 @@ -24,6 +24,6 @@ #define LWIP_TRANSPORT_ETHERNET 1 #define ETH_PAD_SIZE 2 -#define MEM_SIZE (ENET_RX_RING_LEN * (ENET_ETH_MAX_FLEN + RX_BUF_ALIGNMENT) + ENET_TX_RING_LEN * ENET_ETH_MAX_FLEN) +#define MEM_SIZE (ENET_RX_RING_LEN * (ENET_ETH_MAX_FLEN + ENET_BUFF_ALIGNMENT) + ENET_TX_RING_LEN * ENET_ETH_MAX_FLEN) #endif