ethercateoe.c

00001 /*
00002  * Licensed under the GNU General Public License version 2 with exceptions. See
00003  * LICENSE file in the project root for full license information
00004  */
00005 
00006 /** \file
00007  * \brief
00008  * Ethernet over EtherCAT (EoE) module.
00009  *
00010  * Set / Get IP functions
00011  * Blocking send/receive Ethernet Frame
00012  * Read incoming EoE fragment to Ethernet Frame
00013  */
00014 
00015 #include <stdio.h>
00016 #include <string.h>
00017 #include "osal.h"
00018 #include "oshw.h"
00019 #include "ethercat.h"
00020 
00021  /** EoE utility function to convert uint32 to eoe ip bytes.
00022  * @param[in] ip       = ip in uint32
00023  * @param[out] byte_ip = eoe ip 4th octet, 3ed octet, 2nd octet, 1st octet
00024  */
00025 static void EOE_ip_uint32_to_byte(eoe_ip4_addr_t * ip, uint8_t * byte_ip)
00026 {
00027    byte_ip[3] = eoe_ip4_addr1(ip); /* 1st octet */
00028    byte_ip[2] = eoe_ip4_addr2(ip); /* 2nd octet */
00029    byte_ip[1] = eoe_ip4_addr3(ip); /* 3ed octet */
00030    byte_ip[0] = eoe_ip4_addr4(ip); /* 4th octet */
00031 }
00032 
00033 /** EoE utility function to convert eoe ip bytes to uint32.
00034 * @param[in] byte_ip = eoe ip 4th octet, 3ed octet, 2nd octet, 1st octet
00035 * @param[out] ip     = ip in uint32
00036 */
00037 static void EOE_ip_byte_to_uint32(uint8_t * byte_ip, eoe_ip4_addr_t * ip)
00038 {
00039    EOE_IP4_ADDR_TO_U32(ip,
00040       byte_ip[3],  /* 1st octet */
00041       byte_ip[2],  /* 2nd octet */
00042       byte_ip[1],  /* 3ed octet */
00043       byte_ip[0]); /* 4th octet */
00044 }
00045 
00046 /** EoE fragment data handler hook. Should not block.
00047 *
00048 * @param[in]  context = context struct
00049 * @param[in]  hook    = Pointer to hook function.
00050 * @return 1
00051 */
00052 int ecx_EOEdefinehook(ecx_contextt *context, void *hook)
00053 {
00054    context->EOEhook = hook;
00055    return 1;
00056 }
00057 
00058 /** EoE EOE set IP, blocking. Waits for response from the slave.
00059 *
00060 * @param[in]  context    = Context struct
00061 * @param[in]  slave      = Slave number
00062 * @param[in]  port       = Port number on slave if applicable
00063 * @param[in]  ipparam    = IP parameter data to be sent
00064 * @param[in]  Timeout    = Timeout in us, standard is EC_TIMEOUTRXM
00065 * @return Workcounter from last slave response or returned result code 
00066 */
00067 int ecx_EOEsetIp(ecx_contextt *context, uint16 slave, uint8 port, eoe_param_t * ipparam, int timeout)
00068 {
00069    ec_EOEt *EOEp, *aEOEp;  
00070    ec_mbxbuft MbxIn, MbxOut;  
00071    uint16 frameinfo1, result;
00072    uint8 cnt, data_offset;
00073    uint8 flags = 0;
00074    int wkc;
00075 
00076    ec_clearmbx(&MbxIn);
00077    /* Empty slave out mailbox if something is in. Timout set to 0 */
00078    wkc = ecx_mbxreceive(context,  slave, (ec_mbxbuft *)&MbxIn, 0);
00079    ec_clearmbx(&MbxOut);
00080    aEOEp = (ec_EOEt *)&MbxIn;
00081    EOEp = (ec_EOEt *)&MbxOut;  
00082    EOEp->mbxheader.address = htoes(0x0000);
00083    EOEp->mbxheader.priority = 0x00;
00084    data_offset = EOE_PARAM_OFFSET;
00085    
00086    /* get new mailbox count value, used as session handle */
00087    cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00088    context->slavelist[slave].mbx_cnt = cnt;
00089 
00090    EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + (cnt << 4); /* EoE */
00091 
00092    EOEp->frameinfo1 = htoes(EOE_HDR_FRAME_TYPE_SET(EOE_INIT_REQ) | 
00093       EOE_HDR_FRAME_PORT_SET(port) |
00094       EOE_HDR_LAST_FRAGMENT);
00095    EOEp->frameinfo2 = 0;
00096   
00097    /* The EoE frame will include "empty" IP/DNS entries, makes wireshark happy.
00098     * Specification say they are optional, TwinCAT include empty entries.
00099     */
00100    if (ipparam->mac_set)
00101    {
00102       flags |= EOE_PARAM_MAC_INCLUDE;
00103       memcpy(&EOEp->data[data_offset], ipparam->mac.addr, EOE_ETHADDR_LENGTH);
00104    }
00105    data_offset += EOE_ETHADDR_LENGTH;
00106    if (ipparam->ip_set)
00107    {
00108       flags |= EOE_PARAM_IP_INCLUDE;
00109       EOE_ip_uint32_to_byte(&ipparam->ip, &EOEp->data[data_offset]);
00110    }
00111    data_offset += 4;
00112    if (ipparam->subnet_set)
00113    {
00114       flags |= EOE_PARAM_SUBNET_IP_INCLUDE;
00115       EOE_ip_uint32_to_byte(&ipparam->subnet, &EOEp->data[data_offset]);
00116    }
00117    data_offset += 4;
00118    if (ipparam->default_gateway_set)
00119    {
00120       flags |= EOE_PARAM_DEFAULT_GATEWAY_INCLUDE;
00121       EOE_ip_uint32_to_byte(&ipparam->default_gateway, &EOEp->data[data_offset]);
00122    }
00123    data_offset += 4;
00124    if (ipparam->dns_ip_set)
00125    {
00126       flags |= EOE_PARAM_DNS_IP_INCLUDE;
00127       EOE_ip_uint32_to_byte(&ipparam->dns_ip, &EOEp->data[data_offset]);
00128    }
00129    data_offset += 4;
00130    if (ipparam->dns_name_set)
00131    {
00132       flags |= EOE_PARAM_DNS_NAME_INCLUDE;
00133       memcpy(&EOEp->data[data_offset], (void *)ipparam->dns_name, EOE_DNS_NAME_LENGTH);
00134    }
00135    data_offset += EOE_DNS_NAME_LENGTH;
00136 
00137    EOEp->mbxheader.length = htoes(EOE_PARAM_OFFSET + data_offset);
00138    EOEp->data[0] = flags;
00139 
00140    /* send EoE request to slave */
00141    wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00142 
00143    if (wkc > 0) /* succeeded to place mailbox in slave ? */
00144    {
00145       /* clean mailboxbuffer */
00146       ec_clearmbx(&MbxIn);
00147       /* read slave response */
00148       wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00149       if (wkc > 0) /* succeeded to read slave response ? */
00150       {
00151          /* slave response should be FoE */
00152          if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE)
00153          {
00154             frameinfo1 = etohs(aEOEp->frameinfo1);
00155             result = etohs(aEOEp->result);
00156             if ((EOE_HDR_FRAME_TYPE_GET(frameinfo1) != EOE_INIT_RESP) ||
00157                 (result != EOE_RESULT_SUCCESS))
00158             {
00159                wkc = -result;
00160             }
00161          }
00162          else
00163          {
00164             /* unexpected mailbox received */
00165             wkc = -EC_ERR_TYPE_PACKET_ERROR;
00166          }
00167       }
00168    }
00169    return wkc;
00170 }
00171 
00172 /** EoE EOE get IP, blocking. Waits for response from the slave.
00173 *
00174 * @param[in]  context    = Context struct
00175 * @param[in]  slave      = Slave number
00176 * @param[in]  port       = Port number on slave if applicable
00177 * @param[out] ipparam    = IP parameter data retrived from slave
00178 * @param[in]  Timeout    = Timeout in us, standard is EC_TIMEOUTRXM
00179 * @return Workcounter from last slave response or returned result code
00180 */
00181 int ecx_EOEgetIp(ecx_contextt *context, uint16 slave, uint8 port, eoe_param_t * ipparam, int timeout)
00182 {
00183    ec_EOEt *EOEp, *aEOEp;
00184    ec_mbxbuft MbxIn, MbxOut;
00185    uint16 frameinfo1, eoedatasize;
00186    uint8 cnt, data_offset;
00187    uint8 flags = 0;
00188    int wkc;
00189 
00190    /* Empty slave out mailbox if something is in. Timout set to 0 */
00191    wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
00192    ec_clearmbx(&MbxOut);
00193    aEOEp = (ec_EOEt *)&MbxIn;
00194    EOEp = (ec_EOEt *)&MbxOut;
00195    EOEp->mbxheader.address = htoes(0x0000);
00196    EOEp->mbxheader.priority = 0x00;
00197    data_offset = EOE_PARAM_OFFSET;
00198 
00199    /* get new mailbox count value, used as session handle */
00200    cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00201    context->slavelist[slave].mbx_cnt = cnt;
00202 
00203    EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + (cnt << 4); /* EoE */
00204 
00205    EOEp->frameinfo1 = htoes(EOE_HDR_FRAME_TYPE_SET(EOE_GET_IP_PARAM_REQ) | 
00206       EOE_HDR_FRAME_PORT_SET(port) |
00207       EOE_HDR_LAST_FRAGMENT);
00208    EOEp->frameinfo2 = 0;
00209 
00210    EOEp->mbxheader.length = htoes(0x0004); 
00211    EOEp->data[0] = flags;
00212 
00213    /* send EoE request to slave */
00214    wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00215    if (wkc > 0) /* succeeded to place mailbox in slave ? */
00216    {
00217       /* clean mailboxbuffer */
00218       ec_clearmbx(&MbxIn);
00219       /* read slave response */
00220       wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00221       if (wkc > 0) /* succeeded to read slave response ? */
00222       {
00223          /* slave response should be FoE */
00224          if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE)
00225          {
00226             frameinfo1 = etohs(aEOEp->frameinfo1);
00227             eoedatasize = etohs(aEOEp->mbxheader.length) - 0x0004;
00228             if (EOE_HDR_FRAME_TYPE_GET(frameinfo1) != EOE_GET_IP_PARAM_RESP)
00229             {
00230                wkc = -EOE_RESULT_UNSUPPORTED_FRAME_TYPE;
00231             }
00232             else
00233             {
00234                /* The EoE frame will include "empty" IP/DNS entries,  makes 
00235                 * wireshark happy. Specification say they are optional, TwinCAT 
00236                 * include empty entries.
00237                 */
00238                flags = aEOEp->data[0];
00239                if (flags & EOE_PARAM_MAC_INCLUDE)
00240                {
00241                   memcpy(ipparam->mac.addr, 
00242                      &aEOEp->data[data_offset], 
00243                      EOE_ETHADDR_LENGTH);
00244                   ipparam->mac_set = 1;
00245                }
00246                data_offset += EOE_ETHADDR_LENGTH;
00247                if (flags & EOE_PARAM_IP_INCLUDE)
00248                {
00249                   EOE_ip_byte_to_uint32(&aEOEp->data[data_offset],
00250                      &ipparam->ip);
00251                   ipparam->ip_set = 1;
00252                }
00253                data_offset += 4;
00254                if (flags & EOE_PARAM_SUBNET_IP_INCLUDE)
00255                {
00256                   EOE_ip_byte_to_uint32(&aEOEp->data[data_offset],
00257                      &ipparam->subnet);
00258                   ipparam->subnet_set = 1;
00259                }
00260                data_offset += 4;
00261                if (flags & EOE_PARAM_DEFAULT_GATEWAY_INCLUDE)
00262                {
00263                   EOE_ip_byte_to_uint32(&aEOEp->data[data_offset],
00264                      &ipparam->default_gateway);
00265                   ipparam->default_gateway_set = 1;
00266                }
00267                data_offset += 4;
00268                if (flags & EOE_PARAM_DNS_IP_INCLUDE)
00269                {
00270                   EOE_ip_byte_to_uint32(&aEOEp->data[data_offset],
00271                      &ipparam->dns_ip);
00272                   ipparam->dns_ip_set = 1;
00273                }
00274                data_offset += 4;
00275                if (flags & EOE_PARAM_DNS_NAME_INCLUDE)
00276                {
00277                   uint16_t dns_len;
00278                   if ((eoedatasize - data_offset) < EOE_DNS_NAME_LENGTH)
00279                   {
00280                      dns_len = (eoedatasize - data_offset);
00281                   }
00282                   else
00283                   {
00284                      dns_len = EOE_DNS_NAME_LENGTH;
00285                   }     
00286                   /* Assume ZERO terminated string */
00287                   memcpy(ipparam->dns_name, &aEOEp->data[data_offset], dns_len);
00288                   ipparam->dns_name_set = 1;
00289                }
00290                data_offset += EOE_DNS_NAME_LENGTH;
00291                /* Something os not correct, flag the error */
00292                if(data_offset > eoedatasize)
00293                {
00294                   wkc = -EC_ERR_TYPE_MBX_ERROR;
00295                }
00296             }
00297          }
00298          else
00299          {
00300             /* unexpected mailbox received */
00301             wkc = -EC_ERR_TYPE_PACKET_ERROR;
00302          }
00303       }
00304    }
00305    return wkc;
00306 }
00307 
00308 /** EoE ethernet buffer write, blocking. 
00309 *
00310 * If the buffer is larger than the mailbox size then the buffer is sent in 
00311 * several fragments. The function will split the buf data in fragments and
00312 * send them to the slave one by one.
00313 *
00314 * @param[in]  context    = context struct
00315 * @param[in]  slave      = Slave number
00316 * @param[in]  port       = Port number on slave if applicable
00317 * @param[in]  psize      = Size in bytes of parameter buffer.
00318 * @param[in]  p          = Pointer to parameter buffer
00319 * @param[in]  Timeout    = Timeout in us, standard is EC_TIMEOUTRXM
00320 * @return Workcounter from last slave transmission
00321 */
00322 int ecx_EOEsend(ecx_contextt *context, uint16 slave, uint8 port, int psize, void *p, int timeout)
00323 {
00324    ec_EOEt *EOEp;
00325    ec_mbxbuft MbxOut;
00326    uint16 frameinfo1, frameinfo2;
00327    uint16 txframesize, txframeoffset;
00328    uint8 cnt, txfragmentno;  
00329    boolean  NotLast;
00330    int wkc, maxdata;
00331    const uint8 * buf = p;
00332    static uint8_t txframeno = 0;
00333 
00334    ec_clearmbx(&MbxOut);
00335    EOEp = (ec_EOEt *)&MbxOut;
00336    EOEp->mbxheader.address = htoes(0x0000);
00337    EOEp->mbxheader.priority = 0x00;
00338    /* data section=mailbox size - 6 mbx - 4 EoEh */
00339    maxdata = context->slavelist[slave].mbx_l - 0x0A; 
00340    txframesize = psize;
00341    txfragmentno = 0;
00342    txframeoffset = 0;
00343    NotLast = TRUE;
00344 
00345    do
00346    {
00347       txframesize = psize - txframeoffset;
00348       if (txframesize > maxdata)
00349       {
00350          /* Adjust to even 32-octect blocks */
00351          txframesize = ((maxdata >> 5) << 5);
00352       }
00353 
00354       if (txframesize == (psize - txframeoffset))
00355       {
00356          frameinfo1 = (EOE_HDR_LAST_FRAGMENT_SET(1) | EOE_HDR_FRAME_PORT_SET(port));
00357          NotLast = FALSE;
00358       }
00359       else
00360       {
00361          frameinfo1 = EOE_HDR_FRAME_PORT_SET(port);
00362       }
00363 
00364       frameinfo2 = EOE_HDR_FRAG_NO_SET(txfragmentno);
00365       if (txfragmentno > 0)
00366       {
00367          frameinfo2 = frameinfo2 | (EOE_HDR_FRAME_OFFSET_SET((txframeoffset >> 5)));
00368       }
00369       else
00370       {
00371          frameinfo2 = frameinfo2 | (EOE_HDR_FRAME_OFFSET_SET(((psize + 31) >> 5)));
00372          txframeno++;
00373       }
00374       frameinfo2 = frameinfo2 | EOE_HDR_FRAME_NO_SET(txframeno);
00375 
00376       /* get new mailbox count value, used as session handle */
00377       cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00378       context->slavelist[slave].mbx_cnt = cnt;
00379 
00380       EOEp->mbxheader.length = htoes(4 + txframesize); /* no timestamp */
00381       EOEp->mbxheader.mbxtype = ECT_MBXT_EOE + (cnt << 4); /* EoE */
00382 
00383       EOEp->frameinfo1 = htoes(frameinfo1);
00384       EOEp->frameinfo2 = htoes(frameinfo2);
00385 
00386       memcpy(EOEp->data, &buf[txframeoffset], txframesize);
00387 
00388       /* send EoE request to slave */
00389       wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, timeout);
00390       if ((NotLast == TRUE)  && (wkc > 0))
00391       {
00392          txframeoffset += txframesize;
00393          txfragmentno++;
00394       }
00395    } while ((NotLast == TRUE) && (wkc > 0));
00396    
00397    return wkc;
00398 }
00399 
00400 
00401 /** EoE ethernet buffer read, blocking.
00402 *
00403 * If the buffer is larger than the mailbox size then the buffer is received 
00404 * by several fragments. The function will assamble the fragments into
00405 * a complete Ethernet buffer.
00406 *
00407 * @param[in]     context = context struct
00408 * @param[in]     slave   = Slave number
00409 * @param[in]     port    = Port number on slave if applicable
00410 * @param[in/out] psize   = Size in bytes of parameter buffer.
00411 * @param[in]     p       = Pointer to parameter buffer
00412 * @param[in]     timeout = Timeout in us, standard is EC_TIMEOUTRXM
00413 * @return Workcounter from last slave response or error code
00414 */
00415 int ecx_EOErecv(ecx_contextt *context, uint16 slave, uint8 port, int * psize, void *p, int timeout)
00416 {
00417    ec_EOEt *aEOEp;
00418    ec_mbxbuft MbxIn;
00419    uint16 frameinfo1, frameinfo2, rxframesize, rxframeoffset, eoedatasize;
00420    uint8 rxfragmentno, rxframeno;
00421    boolean NotLast;
00422    int wkc, buffersize;
00423    uint8 * buf = p;
00424    
00425    ec_clearmbx(&MbxIn);
00426    aEOEp = (ec_EOEt *)&MbxIn;
00427    NotLast = TRUE;
00428    buffersize = *psize;
00429    rxfragmentno = 0;
00430    
00431    /* Hang for a while if nothing is in */
00432    wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00433 
00434    while ((wkc > 0) && (NotLast == TRUE))
00435    {
00436       /* slave response should be FoE */
00437       if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE)
00438       {
00439         
00440          eoedatasize = etohs(aEOEp->mbxheader.length) - 0x00004;
00441          frameinfo1 = etohs(aEOEp->frameinfo1);
00442          frameinfo2 = etohs(aEOEp->frameinfo2);
00443 
00444          if (rxfragmentno != EOE_HDR_FRAG_NO_GET(frameinfo2))
00445          {
00446             if (EOE_HDR_FRAG_NO_GET(frameinfo2) > 0)
00447             {
00448                wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
00449                /* Exit here*/
00450                break;
00451             }
00452          }
00453 
00454          if (rxfragmentno == 0)
00455          {
00456             rxframeoffset = 0;
00457             rxframeno = EOE_HDR_FRAME_NO_GET(frameinfo2);
00458             rxframesize = (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5);
00459             if (rxframesize > buffersize)
00460             {
00461                wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
00462                /* Exit here*/
00463                break;
00464             }
00465             if (port != EOE_HDR_FRAME_PORT_GET(frameinfo1))
00466             {
00467                wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
00468                /* Exit here*/
00469                break;
00470             }
00471          }
00472          else
00473          {
00474             if (rxframeno != EOE_HDR_FRAME_NO_GET(frameinfo2))
00475             {
00476                wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
00477                /* Exit here*/
00478                break;
00479             }
00480             else if (rxframeoffset != (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5))
00481             {
00482                wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
00483                /* Exit here*/
00484                break;
00485             }
00486          }
00487 
00488          if ((rxframeoffset + eoedatasize) <= buffersize)
00489          {
00490             memcpy(&buf[rxframeoffset], aEOEp->data, eoedatasize);
00491             rxframeoffset += eoedatasize;
00492             rxfragmentno++;
00493          }
00494 
00495          if (EOE_HDR_LAST_FRAGMENT_GET(frameinfo1))
00496          {
00497             /* Remove timestamp */
00498             if (EOE_HDR_TIME_APPEND_GET(frameinfo1))
00499             {
00500                rxframeoffset -= 4;
00501             }
00502             NotLast = FALSE;
00503             *psize = rxframeoffset;
00504          }
00505          else
00506          {
00507             /* Hang for a while if nothing is in */
00508             wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00509          }
00510       }
00511       else
00512       {
00513          /* unexpected mailbox received */
00514          wkc = -EC_ERR_TYPE_PACKET_ERROR;
00515       }
00516    }
00517    return wkc;
00518 }
00519 
00520 /** EoE mailbox fragment read
00521 *
00522 * Will take the data in incoming mailbox buffer and copy to destination 
00523 * Ethernet frame buffer at given offset and update current fragment variables
00524 *
00525 * @param[in] MbxIn             = Received mailbox containing fragment data
00526 * @param[in/out] rxfragmentno  = Fragment number
00527 * @param[in/out] rxframesize   = Frame size
00528 * @param[in/out] rxframeoffset = Frame offset
00529 * @param[in/out] rxframeno     = Frame number
00530 * @param[in/out] psize         = Size in bytes of frame buffer.
00531 * @param[out] p                = Pointer to frame buffer
00532 * @return 0= if fragment OK, >0 if last fragment, <0 on error
00533 */
00534 int ecx_EOEreadfragment(
00535    ec_mbxbuft * MbxIn,
00536    uint8 * rxfragmentno,
00537    uint16 * rxframesize, 
00538    uint16 * rxframeoffset, 
00539    uint16 * rxframeno,
00540    int * psize, 
00541    void *p)
00542 {  
00543    uint16 frameinfo1, frameinfo2,  eoedatasize;
00544    int wkc;
00545    ec_EOEt * aEOEp;
00546    uint8 * buf;
00547    
00548    aEOEp = (ec_EOEt *)MbxIn;
00549    buf = p;
00550    wkc = 0;
00551 
00552    /* slave response should be EoE */
00553    if ((aEOEp->mbxheader.mbxtype & 0x0f) == ECT_MBXT_EOE)
00554    {
00555       eoedatasize = etohs(aEOEp->mbxheader.length) - 0x00004;
00556       frameinfo1 = etohs(aEOEp->frameinfo1);
00557       frameinfo2 = etohs(aEOEp->frameinfo2);
00558 
00559       /* Retrive fragment number, is it what we expect? */
00560       if (*rxfragmentno != EOE_HDR_FRAG_NO_GET(frameinfo2))
00561       {
00562          /* If expected fragment number is not 0, reset working variables */
00563          if (*rxfragmentno != 0)
00564          {
00565             *rxfragmentno = 0;
00566             *rxframesize = 0;
00567             *rxframeoffset = 0;
00568             *rxframeno = 0;
00569          }
00570 
00571          /* If incoming fragment number is not 0 we can't recover, exit */
00572          if (EOE_HDR_FRAG_NO_GET(frameinfo2) > 0)
00573          {
00574             wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
00575             return wkc;
00576          }
00577       }
00578 
00579       /* Is it a new frame?*/
00580       if (*rxfragmentno == 0)
00581       {
00582          *rxframesize = (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5);
00583          *rxframeoffset = 0;
00584          *rxframeno = EOE_HDR_FRAME_NO_GET(frameinfo2);
00585       }
00586       else
00587       {
00588          /* If we're inside a frame, make sure it is the same */
00589          if (*rxframeno != EOE_HDR_FRAME_NO_GET(frameinfo2))
00590          {
00591             *rxfragmentno = 0;
00592             *rxframesize = 0;
00593             *rxframeoffset = 0;
00594             *rxframeno = 0;
00595             wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
00596             return wkc;
00597          }
00598          else if (*rxframeoffset != (EOE_HDR_FRAME_OFFSET_GET(frameinfo2) << 5))
00599          {
00600             *rxfragmentno = 0;
00601             *rxframesize = 0;
00602             *rxframeoffset = 0;
00603             *rxframeno = 0;
00604             wkc = -EC_ERR_TYPE_EOE_INVALID_RX_DATA;
00605             return wkc;
00606          }
00607       }
00608 
00609       /* Make sure we're inside expected frame size */
00610       if (((*rxframeoffset + eoedatasize) <= *rxframesize) && 
00611          ((*rxframeoffset + eoedatasize) <= *psize))
00612       {
00613          memcpy(&buf[*rxframeoffset], aEOEp->data, eoedatasize);
00614          *rxframeoffset += eoedatasize;
00615          *rxfragmentno += 1;
00616       }
00617 
00618       /* Is it the last fragment */
00619       if (EOE_HDR_LAST_FRAGMENT_GET(frameinfo1))
00620       {
00621          /* Remove timestamp */
00622          if (EOE_HDR_TIME_APPEND_GET(frameinfo1))
00623          {
00624             *rxframeoffset -= 4;
00625          }
00626          *psize = *rxframeoffset;
00627          *rxfragmentno = 0;
00628          *rxframesize = 0;
00629          *rxframeoffset = 0;
00630          *rxframeno = 0;
00631          wkc = 1;
00632       }
00633    }
00634    else
00635    {
00636       /* unexpected mailbox received */
00637       wkc = -EC_ERR_TYPE_PACKET_ERROR;
00638    }
00639    return wkc;
00640 }