www.pudn.com > ethercat-slave-source-code.rar > ecataoe.c, change:2013-05-03,size:20813b


/** 
\addtogroup AoE ADS over EtherCAT 
@{ 
*/ 
 
/** 
\file ecataoe.c 
\author EthercatSSC@beckhoff.com 
\brief Implementation 
This file contains the AoE mailbox interface. 
 
\version 5.10 
 
<br>Changes to version V5.01:<br> 
V5.10 AOE1: Reset AoE response result<br> 
V5.10 ESC1: Update address register offset for 32Bit ESC access<br> 
V5.10 ESC5: Add missing swapping<br> 
<br>Changes to version V5.0:<br> 
V5.01 ESC2: Add missed value swapping<br> 
<br>Changes to version V4.40:<br> 
V5.0 ESC1: ESC 32Bit Access added.<br> 
<br>Changes to version V4.20:<br> 
V4.40 HW0: Generic hardware access functions<br> 
<br>Changes to version V4.11:<br> 
V4.20 MBX 1:    Free Mailbox queue buffer<br> 
V4.11 AOE 1: Additional return "No memory available"<br> 
<br>Changes to version V4.08:<br> 
V4.10 AOE 1: The mailbox header of the AoE fragments was not initialized<br> 
V4.10 AOE 2: The function AOEAPPL_AmsInd was not called 
*/ 
 
/*--------------------------------------------------------------------------------------- 
------ 
------    Includes 
------ 
---------------------------------------------------------------------------------------*/ 
 
#include "ecat_def.h" 
 
#if AOE_SUPPORTED 
 
#include "esc.h" 
#include "ecatslv.h" 
 
#define    _ECATAOE_    1 
#include "aoeappl.h" 
#undef      _ECATAOE_ 
#define    _ECATAOE_    0 
 
/*--------------------------------------------------------------------------------------- 
------ 
------    Internal Types and Defines 
------ 
---------------------------------------------------------------------------------------*/ 
#define    MAX_ADS_QUEUE_SIZE       10 /**< \brief ADS queue*/ 
#define    MAX_NO_AMS_CMD_IN_WORK   10 /**< \brief Max parallel AMS commands*/ 
#define    AMS_FRAGMENT_TIMEOUT     10000 /**< \brief AMS fragment timeout*/ 
 
 
/** 
 * \brief ADS queue  
 */ 
typedef struct 
{ 
    UINT16    firstInQueue; /**< \brief First in queue*/ 
    UINT16    lastInQueue; /**< \brief last in queue*/ 
    UINT16    maxQueueSize; /**< \brief Queue size*/ 
    AmsCmd MBXMEM * queue[MAX_ADS_QUEUE_SIZE+1]; /**< \brief Queue buffer*/ 
} TADSQUEUE; 
 
 
/** 
 * \brief AMS fragment table 
 */ 
typedef struct 
{ 
    UINT16                packetNo; /**< \brief Packet number*/ 
    UINT16                fragmentNo; /**< \brief Fragment number*/ 
    AmsAddr                sender; /**< \brief Sender AMS address*/ 
    UINT32                actOffs; /**< \brief Actual offset*/ 
    UINT16                ttl; /**< \brief Ttl*/ 
    AmsCmd MBXMEM *    pFullCmd; /**< \brief AMS command pointer*/ 
} TAmsFragTable, *PTAmsFragTable; 
 
 
/*--------------------------------------------------------------------------------------- 
------ 
------    Local Variables 
------ 
---------------------------------------------------------------------------------------*/ 
TADSQUEUE MBXMEM        sAdsIndQueue; /**< \brief ADS queue variable*/ 
UINT16                    u16AmsFragAck; /**< \brief AMS fragment acknowledge*/ 
UINT16                    u16AmsPacketNo; /**< \brief AMS Packet number*/ 
UINT16                    u16AmsCmdsInWork; /**< \brief AMS command in work*/ 
TAmsFragTable HUGE    asAmsCmdsInWork[MAX_NO_AMS_CMD_IN_WORK]; /**< \brief Pending AMS commands*/ 
 
 
/*--------------------------------------------------------------------------------------- 
------ 
------    Local Functions 
------ 
---------------------------------------------------------------------------------------*/ 
UINT16 AmsFragRes(AmsCmd MBXMEM *pCmd, UINT16 dataLen); 
UINT16 InsertInFragTable(AmsCmd MBXMEM *pCmd); 
AmsCmd MBXMEM * RemoveFromFragTable(UINT16 index); 
AmsCmd MBXMEM * AoeGetOutOfQueue(void); 
UINT16 AoePutInQueue(AmsCmd MBXMEM *pCmd); 
 
 
/*--------------------------------------------------------------------------------------- 
------ 
------    functions 
------ 
---------------------------------------------------------------------------------------*/ 
 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 
 \brief    This function intialize the AoE Interface. 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
void AOE_Init(void) 
{ 
    bAoeWaitForNetId = 1; 
    bAoeRelativeNetIds = 0; 
} 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pCmd        Pointer to the AMS commands 
 \param    dataLen    length of the AMS command to be sent 
 
 \brief    This function is called when an AMS response shall be sent 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
UINT16 AmsFragRes(AmsCmd MBXMEM *pCmd, UINT16 dataLen) 
{ 
    UINT16 result = ERR_NOERROR; 
    UINT16 maxDataLen = u16AmsFragSize; 
 
    if (maxDataLen < dataLen) 
    { 
        /* command has to fragmented because it doesn't fit in the mailbox */ 
        UINT8        bLoop = 1; 
        UINT16    offset = 2 * SIZEOF(AmsAddr); 
        UINT16    fragNo = 0; 
 
        dataLen += (SIZEOF(AmsHead) - 2 * SIZEOF(AmsAddr)); 
        while (bLoop) 
        { 
            TMBX MBXMEM *pMbx; 
            AmsCmd MBXMEM *pFragCmd; 
            UINT16 size; 
 
            if (dataLen > maxDataLen) 
                size = maxDataLen; 
            else 
            { 
                size = dataLen; 
                bLoop = 0; 
            } 
 
            pMbx = (TMBX MBXMEM *) APPL_AllocMailboxBuffer(size + SIZEOF(AmsHead) + MBX_HEADER_SIZE); 
            if (pMbx) 
            { 
                /* the AMS command begins after the mailbox header */ 
                pFragCmd = (AmsCmd MBXMEM *) pMbx->Data; 
                HMEMCPY((UINT8 MBXMEM *) pFragCmd, (UINT8 MBXMEM *) pCmd, 2*SIZEOF(AmsAddr)); 
                pFragCmd->head.cmdId = ROUTERCMD_FRAGMENT; 
                pFragCmd->head.stateFlags = u16AmsFragAck; 
                pFragCmd->head.cbData = size; 
                pFragCmd->head.errCode = ERR_NOERROR; 
                pFragCmd->head.hUser.frag.packetNo = u16AmsPacketNo; 
                pFragCmd->head.hUser.frag.fragmentNo = fragNo++; 
                HMEMCPY(&pFragCmd[1], &((UINT8 MBXMEM *) pCmd)[offset], size); 
                offset += size; 
                dataLen -= size; 
 
                if (MBX_MailboxSendReq(pMbx, AOE_SERVICE) != 0) 
                { 
                    bLoop = FALSE; 
                    result = ADSERR_DEVICE_NOMEMORY; 
#if MAILBOX_QUEUE 
                    if (pMbx != NULL) 
                        APPL_FreeMailboxBuffer(pMbx); 
#endif 
                } 
            } 
            else 
            { 
                bLoop = FALSE; 
                result = ADSERR_DEVICE_NOMEMORY; 
            } 
        } 
 
        u16AmsPacketNo++; 
 
        FREEADSMEM(pCmd); 
    } 
    else 
    { 
        TMBX MBXMEM *pMbx; 
        AmsCmd MBXMEM *pFragCmd; 
 
        pMbx = (TMBX MBXMEM *) APPL_AllocMailboxBuffer(dataLen + SIZEOF(AmsHead) + MBX_HEADER_SIZE); 
        if (pMbx) 
        { 
            /* the AMS command begins after the mailbox header */ 
            pFragCmd = (AmsCmd MBXMEM *) pMbx->Data; 
            HMEMCPY(pFragCmd, pCmd, dataLen + SIZEOF(AmsHead)); 
            FREEADSMEM(pCmd); 
            pMbx->MbxHeader.Length    = dataLen + SIZEOF(AmsHead); 
            pMbx->MbxHeader.Address    = 0; 
            pMbx->MbxHeader.Flags[MBX_OFFS_TYPE] = MBX_TYPE_AOE<<MBX_SHIFT_TYPE; 
            if (MBX_MailboxSendReq(pMbx, AOE_SERVICE) != 0) 
            { 
                result = ADSERR_DEVICE_NOMEMORY; 
                APPL_FreeMailboxBuffer(pMbx); 
            } 
        } 
        else 
            result = ADSERR_DEVICE_NOMEMORY; 
    } 
 
    return result; 
} 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pCmd        Pointer to the AMS commands 
 
 \brief    This function is called when a fragmented AMS command is received 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
UINT16 InsertInFragTable(AmsCmd MBXMEM *pCmd) 
{ 
    UINT16 result = ERR_NOERROR; 
 
    if (pCmd->head.hUser.frag.fragmentNo == 0) 
    { 
        if (pCmd->head.cbData >= SIZEOF(AmsFirstFragmentHead)) 
        { 
            AmsFirstFragmentHead MBXMEM *pHead = (AmsFirstFragmentHead MBXMEM *) pCmd; 
 
            if (u16AmsCmdsInWork < MAX_NO_AMS_CMD_IN_WORK) 
            { 
                TAmsFragTable MBXMEM *pEntry = &asAmsCmdsInWork[u16AmsCmdsInWork]; 
                AmsCmd MBXMEM *pFullCmd = (AmsCmd MBXMEM *) ALLOCADSMEM(pHead->org.cbData + SIZEOF(AmsHead)); 
 
                if (pFullCmd) 
                { 
                    HMEMCPY((UINT8 MBXMEM *) pFullCmd, (UINT8 MBXMEM *) pCmd, 2*SIZEOF(AmsAddr)); 
                    HMEMCPY((UINT8 MBXMEM *) &pFullCmd->head.cmdId, (UINT8 MBXMEM *) &pHead->org, pCmd->head.cbData); 
                    HMEMCPY((UINT8 MBXMEM *) &pEntry->sender, (UINT8 MBXMEM *) &pCmd->head.sender, SIZEOF(AmsAddr)); 
 
                    pEntry->packetNo         = pCmd->head.hUser.frag.packetNo; 
                    pEntry->fragmentNo     = 1; 
                    pEntry->actOffs         = (pCmd->head.cbData + 2*SIZEOF(AmsAddr)); 
                    pEntry->pFullCmd         = pFullCmd; 
                    pEntry->ttl                = AMS_FRAGMENT_TIMEOUT; 
                    u16AmsCmdsInWork++; 
                } 
                else 
                { 
                    result = ADSERR_DEVICE_NOMEMORY; 
                } 
            } 
        } 
        else 
        { 
            result = ADSERR_DEVICE_ERROR; 
        } 
    } 
    else 
    { 
        result = ADSERR_DEVICE_ERROR; 
    } 
 
    return result; 
} 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     index        Index of the requested AMS command 
 
 \brief    This function is called when a fragmented AMS command is searched 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
AmsCmd MBXMEM * RemoveFromFragTable(UINT16 index) 
{ 
    UINT16 i; 
    AmsCmd MBXMEM * pCmd = asAmsCmdsInWork[index].pFullCmd; 
 
    if (index < u16AmsCmdsInWork) 
    { 
        for (i = index; i < (u16AmsCmdsInWork-1); i++) 
            HMEMCPY((UINT8 MBXMEM *) &asAmsCmdsInWork[i], (UINT8 MBXMEM *) &asAmsCmdsInWork[i+1], SIZEOF(TAmsFragTable)); 
        u16AmsCmdsInWork--; 
    } 
 
    return pCmd; 
} 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pCmd        Pointer to the AMS commands 
 
 \brief    This function is called when a fragmented AMS command is received 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
AmsCmd MBXMEM * AOE_FragmentedCmdInd(AmsCmd MBXMEM *pCmd) 
{ 
    UINT16 i; 
    UINT16 result = ERR_NOERROR; 
    UINT8 bFound = 0; 
    UINT8 bAck; 
    AmsCmd MBXMEM * pAck = 0; 
 
    if (pCmd->head.stateFlags & AMSCMDSF_RESPONSE) 
    { 
        FREEMEM(pCmd); 
        return NULL; 
    } 
 
    if ( (pCmd->head.stateFlags & AMSCMDSF_NORETURN) == 0 ) 
        bAck = 1; 
    else 
        bAck = 0; 
 
    for (i = 0; (i < u16AmsCmdsInWork)&&(!bFound); i++) 
    { 
        TAmsFragTable MBXMEM *pEntry = &asAmsCmdsInWork[i]; 
        if ((pCmd->head.hUser.frag.packetNo == pEntry->packetNo) 
          &&(AOE_AmsNetIdsEqual(&pCmd->head.sender.netId, &pEntry->sender.netId)) 
          &&(pCmd->head.sender.port == pEntry->sender.port) 
            ) 
        { 
            /* AMS command found */ 
            if (pCmd->head.hUser.frag.fragmentNo == pEntry->fragmentNo) 
            { 
                AmsCmd MBXMEM * pFullCmd = pEntry->pFullCmd; 
                if ( (pEntry->actOffs + pCmd->head.cbData) <= (pFullCmd->head.cbData + SIZEOF(AmsHead)) ) 
                { 
                    bFound = 1; 
                    HMEMCPY(&((UINT8 MBXMEM *)pFullCmd)[pEntry->actOffs], &pCmd[1], pCmd->head.cbData); 
                    pEntry->actOffs += pCmd->head.cbData; 
                    pEntry->fragmentNo++; 
                    pEntry->ttl    = AMS_FRAGMENT_TIMEOUT; 
                    if ( pEntry->actOffs == (pFullCmd->head.cbData + SIZEOF(AmsHead)) ) 
                    { 
                        /* last fragment */ 
                        FREEADSMEM(pCmd); 
                        pCmd = RemoveFromFragTable(i); 
                    } 
                    else 
                    { 
                        /* middle Fragment */ 
                        if (bAck) 
                            pAck = pCmd; 
                        else 
                            FREEMEM(pCmd); 
                        pCmd = NULL; 
                    } 
                } 
                else 
                { 
                    result = ADSERR_DEVICE_ERROR; 
                    bFound = 1; 
                } 
            } 
            else 
            { 
                result = ADSERR_DEVICE_ERROR; 
                bFound = 1; 
            } 
        } 
    } 
 
    if (bFound) 
    { 
        if (result != ERR_NOERROR) 
        { 
            /* fault in Fragment, delete from list */ 
            AmsCmd MBXMEM *p = RemoveFromFragTable(i); 
 
            if (p) 
                FREEADSMEM(p); 
 
            FREEADSMEM(pCmd); 
            return NULL; 
        } 
    } 
    else 
    { 
        /* new AMS command */ 
        result = InsertInFragTable(pCmd); 
        if (result == ERR_NOERROR) 
        { 
            if (bAck) 
                pAck = pCmd; 
            else 
                FREEADSMEM(pCmd); 
            pCmd = NULL; 
        } 
        else 
        { 
            FREEADSMEM(pCmd); 
            return NULL; 
        } 
    } 
 
    if (bAck) 
    { 
        /* fragment has to be acknowledged */ 
        AOE_AmsRes(pAck, ERR_NOERROR, 0); 
    } 
 
    return(pCmd); 
} 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pNetId1,pNetId2            Net-IDs to be compared 
 
 \return 0 = Net-IDs different, 1 = Net-IDs equal 
 
 \brief    This function is called when an AMS response shall be sent 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
UINT8 AOE_AmsNetIdsEqual(AmsNetId MBXMEM *pNetId1, AmsNetId MBXMEM *pNetId2) 
{ 
    int i = 0; 
 
    for (i = 0; i < 6; i++) 
    { 
        if (pNetId1->b[i] != pNetId2->b[i]) 
            return 0; 
    } 
 
    return 1; 
} 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pCmd            Pointer to the AMS commands 
 \param    amsErrCode    AMS Error Code 
 \param    dataLen        AMS Data Length 
 
 \brief    This function is called when an AMS response shall be sent 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
UINT16 AOE_AmsRes(AmsCmd MBXMEM *pCmd, UINT16 amsErrCode, UINT16 dataLen) 
{ 
    AmsAddr amsAddr; 
 
    /* change Sender and Target */ 
    HMEMCPY((UINT8 MBXMEM *) &amsAddr, (UINT8 MBXMEM *) &pCmd->head.sender, SIZEOF(AmsAddr)); 
    HMEMCPY((UINT8 MBXMEM *) &pCmd->head.sender, (UINT8 MBXMEM *) &pCmd->head.target, SIZEOF(AmsAddr)); 
    HMEMCPY((UINT8 MBXMEM *) &pCmd->head.target, (UINT8 MBXMEM *) &amsAddr, SIZEOF(AmsAddr)); 
 
    pCmd->head.errCode         = amsErrCode; 
    pCmd->head.cbData         = dataLen; 
    pCmd->head.stateFlags    |= AMSCMDSF_RESPONSE; 
 
    return AmsFragRes(pCmd, dataLen); 
} 
 
#if MAILBOX_QUEUE 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pCmd      Pointer to the AMS commands 
 
 \brief    This function is called when a AMS indication is received 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
UINT16 AoePutInQueue(AmsCmd MBXMEM *pCmd) 
{ 
    UINT16 lastInQueue; 
 
    ENTER_MBX_CRITICAL; 
    lastInQueue = sAdsIndQueue.lastInQueue+1; 
    if (lastInQueue == sAdsIndQueue.maxQueueSize) 
    { 
        lastInQueue = 0; 
    } 
 
    if (sAdsIndQueue.firstInQueue == lastInQueue) 
    { 
        /* Queue overflow */ 
        LEAVE_MBX_CRITICAL; 
        return ADSERR_DEVICE_NOMEMORY; 
    } 
 
    sAdsIndQueue.queue[sAdsIndQueue.lastInQueue] = pCmd; 
    sAdsIndQueue.lastInQueue = lastInQueue; 
 
    LEAVE_MBX_CRITICAL; 
    return ERR_NOERROR; 
} 
 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \return     pCmd      Pointer to the AMS commands 
 
 \brief    This function is gets the oldest ADS indication from the queue 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
AmsCmd MBXMEM * AoeGetOutOfQueue(void) 
{ 
    AmsCmd MBXMEM *pCmd = NULL; 
 
    ENTER_MBX_CRITICAL; 
    if (sAdsIndQueue.firstInQueue != sAdsIndQueue.lastInQueue) 
    { 
        UINT16 firstInQueue = sAdsIndQueue.firstInQueue; 
        pCmd = sAdsIndQueue.queue[firstInQueue++]; 
        sAdsIndQueue.firstInQueue = firstInQueue; 
        if (sAdsIndQueue.firstInQueue == sAdsIndQueue.maxQueueSize) 
        { 
            sAdsIndQueue.firstInQueue = 0; 
        } 
    } 
    else 
        pCmd = 0; 
    LEAVE_MBX_CRITICAL; 
 
    return pCmd; 
} 
 
#endif 
///////////////////////////////////////////////////////////////////////////////////////// 
/** 
 \param     pMbx      Pointer to the received mailbox data from the master. 
 
 \return    result of the operation (0 (success) or mailbox error code (MBXERR_.... defined in 
            mailbox.h)) 
 
 \brief    This function is called when a AoE (ADS over EtherCAT) service is received from 
             the master. 
*//////////////////////////////////////////////////////////////////////////////////////// 
 
UINT8 AOE_ServiceInd(TMBX MBXMEM *pMbx) 
{ 
/* ECATCHANGE_START(V5.10) ESC5*/ 
    UINT16 mbxSize = SWAPWORD(pMbx->MbxHeader.Length); 
/* ECATCHANGE_END(V5.10) ESC5*/ 
 
    if (mbxSize) 
    { 
        AmsCmd MBXMEM * pCmd = (AmsCmd MBXMEM *) ALLOCADSMEM(mbxSize); 
 
        TAdsReadRes MBXMEM *pRes = (TAdsReadRes MBXMEM *)pCmd; 
 
        if (pCmd && mbxSize >= (SIZEOF(AmsHead))) 
        { 
            HMEMCPY((UINT8 MBXMEM *) pCmd, pMbx->Data, (mbxSize+1) & 0xFFFE); 
#if MAILBOX_QUEUE 
            APPL_FreeMailboxBuffer(pMbx); 
#endif 
            if ( pCmd->head.cmdId == ADSSRVID_WRITE && pCmd->head.cbData >= (SIZEOF(TAdsWriteReq)-SIZEOF(AmsHead)+SIZEOF(AmsNetId)) ) 
            { 
                TAdsWriteReq MBXMEM * pReq = (TAdsWriteReq MBXMEM *) pCmd; 
                UINT16 ecatAddr; 
#if ESC_32BIT_ACCESS 
/* ECATCHANGE_START(V5.10) ESC1*/ 
                UINT32 TmpVal = 0; 
                HW_EscReadDWord(TmpVal, ESC_SLAVE_ADDRESS_OFFSET); 
                ecatAddr = (UINT16) ((SWAPDWORD(TmpVal) & ESC_SLAVE_ADDRESS_MASK)); 
/* ECATCHANGE_END(V5.10) ESC1*/ 
#else 
                HW_EscReadWord(ecatAddr, ESC_SLAVE_ADDRESS_OFFSET); 
                ecatAddr = SWAPWORD(ecatAddr); 
#endif 
                if ( pReq->indexGroup == 1 
                  && pReq->indexOffset == 3 
                  && pReq->cbLength == SIZEOF(AmsNetId) 
                  && pCmd->head.target.port == ecatAddr 
                  && pCmd->head.sender.port == ecatAddr 
                   ) 
                { 
                    // HBu 30.09.06: mit dem richtigen Kommando wird die Net-ID immer ueberschrieben 
                    bAoeWaitForNetId = 0; 
                    HMEMCPY(&sRouterNetId, &pReq[1], SIZEOF(AmsNetId)); 
                    if (((UINT32 MBXMEM *) &pReq[1])[0] == 0) 
                    { 
                        // relative Net-ID 
                        HMEMCPY(&sRouterNetId, pCmd->head.target.netId.b, 4); 
                        bAoeRelativeNetIds = TRUE; 
                    } 
                    else 
                    { 
                        bAoeRelativeNetIds = FALSE; 
                    } 
 
                    pCmd->head.stateFlags |= AMSCMDSF_RESPONSE; 
/* ECATCHANGE_START(V5.10) AOE1*/ 
                    pRes->result = 0; 
/* ECATCHANGE_END(V5.10) AOE1*/ 
 
                    AOE_AmsRes( pCmd, 0, 4); 
                    return 0; 
                } 
            } 
 
            if (bAoeWaitForNetId) 
            { 
                // Hbu 30.09.06: aus Kompatibilitaetsgruenden wird die Net-ID aus dem ersten Kommando genommen 
                HMEMCPY(&sRouterNetId, &pCmd->head.target.netId.b[0], SIZEOF(AmsNetId));        // take the first rdx NET ID 
                bAoeWaitForNetId = 0; 
            } 
 
            AOEAPPL_AmsInd(pCmd); 
        } 
        else 
        { 
            return MBXERR_NOMOREMEMORY; 
        } 
    } 
 
    return 0; 
} 
 
/** @} */ 
 
#endif /* AOE_SUPPORTED */