ln-dotnet
/
SOES
mirror of https://github.com/OpenEtherCATsociety/SOES.git
3
0
Fork 0
Code Issues Projects Releases Wiki Activity
SOES/soes/esc_coe.c

910 lines
26 KiB
C
Raw Blame History

/*
* SOES Simple Open EtherCAT Slave
*
* File : esc_coe.c
* Version : 0.9.2
* Date : 22-02-2010
* Copyright (C) 2007-2013 Arthur Ketels
*
* SOES is free software; you can redistribute it and/or modify it under
* the terms of the GNU General Public License version 2 as published by the Free
* Software Foundation.
*
* SOES is distributed in the hope that it will be useful, but WITHOUT ANY
* WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* As a special exception, if other files instantiate templates or use macros
* or inline functions from this file, or you compile this file and link it
* with other works to produce a work based on this file, this file does not
* by itself cause the resulting work to be covered by the GNU General Public
* License. However the source code for this file must still be made available
* in accordance with section (3) of the GNU General Public License.
*
* This exception does not invalidate any other reasons why a work based on
* this file might be covered by the GNU General Public License.
*
* The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual
* property of, and protected by Beckhoff Automation GmbH.
*/
/** \file
* \brief
* CAN over EtherCAT (CoE) module.
*
* SDO read / write and SDO service functions
*/
#include <string.h>
#include <cc.h>
#include "esc.h"
#include "esc_coe.h"
#define BITS2BYTES(b) ((b + 7) >> 3)
extern uint8 txpdoitems;
extern uint8 rxpdoitems;
/** Search for an object index matching the wanted value in the Object List.
*
* @param[in] index = value on index of object we want to locate
* @return local array index if we succeed, -1 if we didn't find the index.
*/
int32 SDO_findobject (uint16 index)
{
int32 n = 0;
while (SDOobjects[n].index < index)
{
n++;
}
if (SDOobjects[n].index != index)
{
return -1;
}
return n;
}
/** Calculate the size in Bytes of TxPDOs by adding the objects in SyncManager
* SDO 1C13.
*
* @return size of TxPDOs in Bytes.
*/
uint16 sizeTXPDO (void)
{
uint8 c, l, si, sic;
uint16 size = 0, hobj;
int16 nidx;
_objd FLASHSTORE *objd;
if (SDO1C13[0].data)
{
si = *((uint8 *) SDO1C13[0].data);
}
else
{
si = (uint8) SDO1C13[0].value;
}
if (si)
{
for (sic = 1; sic <= si; sic++)
{
if (SDO1C13[sic].data)
{
hobj = *((uint16 *) SDO1C13[sic].data);
}
else
{
hobj = (uint16) SDO1C13[sic].value;
}
nidx = SDO_findobject (hobj);
if (nidx > 0)
{
objd = SDOobjects[nidx].objdesc;
l = (uint8) objd->value;
for (c = 1; c <= l; c++)
{
size += ((objd + c)->value & 0xff);
}
}
}
}
return BITS2BYTES (size);
}
/** Calculate the size in Bytes of RxPDOs by adding the objects in SyncManager
* SDO 1C12.
*
* @return size of RxPDOs in Bytes.
*/
uint16 sizeRXPDO (void)
{
uint8 c, l, si, sic;
uint16 size = 0, hobj;
int16 nidx;
_objd FLASHSTORE *objd;
if (SDO1C12[0].data)
{
si = *((uint8 *) SDO1C12[0].data);
}
else
{
si = (uint8) SDO1C12[0].value;
}if (si)
{
for (sic = 1; sic <= si; sic++)
{
if (SDO1C12[sic].data)
{
hobj = *((uint16 *) SDO1C12[sic].data);
}
else
{
hobj = (uint16) SDO1C12[sic].value;
}
nidx = SDO_findobject (hobj);
if (nidx > 0)
{
objd = SDOobjects[nidx].objdesc;
l = (uint8) objd->value;
for (c = 1; c <= l; c++)
{
size += ((objd + c)->value & 0xff);
}
}
}
}
return BITS2BYTES (size);
}
/** Search for an object sub-index.
*
* @param[in] nidx = local array index of object we want to find sub-index to
* @param[in] subindex = value on sub-index of object we want to locate
* @return local array index if we succeed, -1 if we didn't find the index.
*/
int16 SDO_findsubindex (int16 nidx, uint8 subindex)
{
_objd FLASHSTORE *objd;
int16 n = 0;
uint8 maxsub;
objd = SDOobjects[nidx].objdesc;
maxsub = SDOobjects[nidx].maxsub;
while (((objd + n)->subindex < subindex) && (n < maxsub))
{
n++;
}
if ((objd + n)->subindex != subindex)
{
return -1;
}
return n;
}
/** Copy to mailbox.
*
* @param[in] source = pointer to source
* @param[in] dest = pointer to destination
* @param[in] size = Size to copy
*/
void copy2mbx (void *source, void *dest, uint16 size)
{
memcpy (dest, source, size);
}
/** Function for sending an SDO Abort reply.
*
* @param[in] index = index of object causing abort reply
* @param[in] sub-index = sub-index of object causing abort reply
* @param[in] abortcode = abort code to send in reply
*/
void SDO_abort (uint16 index, uint8 subindex, uint32 abortcode)
{
uint8 MBXout;
_COEsdo *coeres;
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEsdo *) &MBX[MBXout];
coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOREQUEST << 12));
coeres->index = htoes (index);
coeres->subindex = subindex;
coeres->command = COE_COMMAND_SDOABORT;
coeres->size = htoel (abortcode);
MBXcontrol[MBXout].state = MBXstate_outreq;
}
}
/** Function for responding on requested SDO Upload, sending the content
* requested in a free Mailbox buffer. Depending of size of data expedited,
* normal or segmented transfer is used. On error an SDO Abort will be sent.
*/
void SDO_upload (void)
{
_COEsdo *coesdo, *coeres;
uint16 index;
uint8 subindex;
int16 nidx, nsub;
uint8 MBXout;
uint32 size;
uint8 dss;
_objd FLASHSTORE *objd;
coesdo = (_COEsdo *) &MBX[0];
index = etohs (coesdo->index);
subindex = coesdo->subindex;
nidx = SDO_findobject (index);
if (nidx >= 0)
{
nsub = SDO_findsubindex (nidx, subindex);
if (nsub >= 0)
{
objd = SDOobjects[nidx].objdesc;
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEsdo *) &MBX[MBXout];
coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDORESPONSE << 12));
size = (objd + nsub)->bitlength;
/* expedited bits used calculation */
dss = 0x0c;
if (size > 8)
{
dss = 0x08;
}
if (size > 16)
{
dss = 0x04;
}
if (size > 24)
{
dss = 0x00;
}
coeres->index = htoes (index);
coeres->subindex = subindex;
if (size <= 32)
{
/* expedited response i.e. length<=4 bytes */
coeres->command = COE_COMMAND_UPLOADRESPONSE +
COE_SIZE_INDICATOR + COE_EXPEDITED_INDICATOR + dss;
if ((objd + nsub)->data == nil)
{
/* use constant value */
coeres->size = htoel ((objd + nsub)->value);
}
else
{
/* convert bits to bytes */
size = (size + 7) >> 3;
/* use dynamic data */
copy2mbx ((objd + nsub)->data, &(coeres->size), size);
}
}
else
{
/* normal response i.e. length>4 bytes */
coeres->command = COE_COMMAND_UPLOADRESPONSE +
COE_SIZE_INDICATOR;
/* convert bits to bytes */
size = (size + 7) >> 3;
coeres->size = htoel (size);
if ((size + COE_HEADERSIZE) > MBXDSIZE)
{
/* segmented transfer needed */
/* set total size in bytes */
ESCvar.frags = size;
/* limit to mailbox size */
size = MBXDSIZE - COE_HEADERSIZE;
/* number of bytes done */
ESCvar.fragsleft = size;
/* signal segmented transfer */
ESCvar.segmented = MBXSEU;
ESCvar.data = (objd + nsub)->data;
}
else
{
ESCvar.segmented = 0;
}
coeres->mbxheader.length = htoes (COE_HEADERSIZE + size);
/* use dynamic data */
copy2mbx ((objd + nsub)->data, (&(coeres->size)) + 1, size);
}
MBXcontrol[MBXout].state = MBXstate_outreq;
}
}
else
{
SDO_abort (index, subindex, ABORT_NOSUBINDEX);
}
}
else
{
SDO_abort (index, subindex, ABORT_NOOBJECT);
}
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
}
/** Function for handling the following SDO Upload if previous SDOUpload
* response was flagged it needed to be segmented.
*
*/
void SDO_uploadsegment (void)
{
_COEsdo *coesdo, *coeres;
uint8 MBXout;
uint32 size, offset;
coesdo = (_COEsdo *) &MBX[0];
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEsdo *) &MBX[MBXout];
offset = ESCvar.fragsleft;
size = ESCvar.frags - ESCvar.fragsleft;
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDORESPONSE << 12));
coeres->command = COE_COMMAND_UPLOADSEGMENT + (coesdo->command & COE_TOGGLEBIT); // copy toggle bit
if ((size + COE_SEGMENTHEADERSIZE) > MBXDSIZE)
{
/* more segmented transfer needed */
/* limit to mailbox size */
size = MBXDSIZE - COE_SEGMENTHEADERSIZE;
/* number of bytes done */
ESCvar.fragsleft += size;
coeres->mbxheader.length = htoes (COE_SEGMENTHEADERSIZE + size);
}
else
{
/* last segment */
ESCvar.segmented = 0;
ESCvar.frags = 0;
ESCvar.fragsleft = 0;
coeres->command += COE_COMMAND_LASTSEGMENTBIT;
if (size >= 7)
{
coeres->mbxheader.length = htoes (COE_SEGMENTHEADERSIZE + size);
}
else
{
coeres->command += (7 - size) << 1;
coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
}
}
copy2mbx ((uint8 *) ESCvar.data + offset, (&(coeres->command)) + 1, size); //copy to mailbox
MBXcontrol[MBXout].state = MBXstate_outreq;
}
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
}
/** Function to pre-qualify the incoming SDO download.
*
* @param[in] index = index of SDO download request to check
* @param[in] sub-index = sub-index of SDO download request to check
* @return 1 if the SDO Download is correct. 0 If not correct.
*/
int ESC_pre_objecthandler (uint16 index, uint8 subindex)
{
if ((index == 0x1c12) && (subindex > 0) && (rxpdoitems != 0))
{
SDO_abort (index, subindex, ABORT_READONLY);
return 0;
}
if ((index == 0x1c13) && (subindex > 0) && (txpdoitems != 0))
{
SDO_abort (index, subindex, ABORT_READONLY);
return 0;
}
return 1;
}
/** Function for handling incoming requested SDO Download, validating the
* request and sending an response. On error an SDO Abort will be sent.
*/
void SDO_download (void)
{
_COEsdo *coesdo, *coeres;
uint16 index;
uint8 subindex;
int16 nidx, nsub;
uint8 MBXout;
uint16 size, actsize;
_objd FLASHSTORE *objd;
uint32 *mbxdata;
coesdo = (_COEsdo *) &MBX[0];
index = etohs (coesdo->index);
subindex = coesdo->subindex;
nidx = SDO_findobject (index);
if (nidx >= 0)
{
nsub = SDO_findsubindex (nidx, subindex);
if (nsub >= 0)
{
objd = SDOobjects[nidx].objdesc;
if (((objd + nsub)->access == ATYPE_RW) ||
(((objd + nsub)->access == ATYPE_RWpre)
&& ((ESCvar.ALstatus & 0x0f) == ESCpreop)))
{
/* expedited? */
if (coesdo->command & COE_EXPEDITED_INDICATOR)
{
size = 4 - ((coesdo->command & 0x0c) >> 2);
mbxdata = &(coesdo->size);
}
else
{
/* normal upload */
size = (etohs (coesdo->size) & 0xffff);
mbxdata = (&(coesdo->size)) + 1;
}
actsize = ((objd + nsub)->bitlength + 7) >> 3;
if (actsize == size)
{
copy2mbx (mbxdata, (objd + nsub)->data, size);
if (ESC_pre_objecthandler (index, subindex))
{
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEsdo *) &MBX[MBXout];
coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDORESPONSE << 12));
coeres->index = htoes (index);
coeres->subindex = subindex;
coeres->command = COE_COMMAND_DOWNLOADRESPONSE;
coeres->size = htoel (0);
MBXcontrol[MBXout].state = MBXstate_outreq;
}
}
/* external object write handler */
ESC_objecthandler (index, subindex);
}
else
{
SDO_abort (index, subindex, ABORT_TYPEMISMATCH);
}
}
else
{
if ((objd + nsub)->access == ATYPE_RWpre)
{
SDO_abort (index, subindex, ABORT_NOTINTHISSTATE);
}
else
{
SDO_abort (index, subindex, ABORT_READONLY);
}
}
}
else
{
SDO_abort (index, subindex, ABORT_NOSUBINDEX);
}
}
else
{
SDO_abort (index, subindex, ABORT_NOOBJECT);
}
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
}
/** Function for sending an SDO Info Error reply.
*
* @param[in] abortcode = = abort code to send in reply
*/
void SDO_infoerror (uint32 abortcode)
{
uint8 MBXout;
_COEobjdesc *coeres;
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coeres = (_COEobjdesc *) &MBX[MBXout];
coeres->mbxheader.length = htoes ((uint16) 0x0a);
coeres->mbxheader.mbxtype = MBXCOE;
coeres->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
/* SDO info error request */
coeres->infoheader.opcode = COE_INFOERROR;
coeres->infoheader.incomplete = 0;
coeres->infoheader.reserved = 0x00;
coeres->infoheader.fragmentsleft = 0;
coeres->index = htoel (abortcode);
MBXcontrol[MBXout].state = MBXstate_outreq;
}
}
#define ODLISTSIZE ((MBX1_sml - MBXHSIZE - sizeof(_COEh) - sizeof(_INFOh) - 2) & 0xfffe)
/** Function for handling incoming requested SDO Get OD List, validating the
* request and sending an response. On error an SDO Info Error will be sent.
*/
void SDO_getodlist (void)
{
uint16 frags;
uint8 MBXout = 0;
uint16 entries = 0;
uint16 i, n;
uint16 *p;
_COEobjdesc *coel, *coer;
while (SDOobjects[entries].index != 0xffff)
{
entries++;
}
ESCvar.entries = entries;
frags = ((entries << 1) + ODLISTSIZE - 1);
frags /= ODLISTSIZE;
coer = (_COEobjdesc *) &MBX[0];
/* check for unsupported opcodes */
if (etohs (coer->index) > 0x01)
{
SDO_infoerror (ABORT_UNSUPPORTED);
}
else
{
MBXout = ESC_claimbuffer ();
}
if (MBXout)
{
coel = (_COEobjdesc *) &MBX[MBXout];
coel->mbxheader.mbxtype = MBXCOE;
coel->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
coel->infoheader.opcode = COE_GETODLISTRESPONSE;
/* number of objects request */
if (etohs (coer->index) == 0x00)
{
coel->index = htoes ((uint16) 0x00);
coel->infoheader.incomplete = 0;
coel->infoheader.reserved = 0x00;
coel->infoheader.fragmentsleft = htoes ((uint16) 0);
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
ESCvar.frags = frags;
ESCvar.fragsleft = frags - 1;
p = &(coel->datatype);
*p = htoes (entries);
p++;
*p = 0;
p++;
*p = 0;
p++;
*p = 0;
p++;
*p = 0;
coel->mbxheader.length = htoes (0x08 + (5 << 1));
}
/* only return all objects */
if (etohs (coer->index) == 0x01)
{
if (frags > 1)
{
coel->infoheader.incomplete = 1;
ESCvar.xoe = MBXCOE + MBXODL;
n = ODLISTSIZE >> 1;
}
else
{
coel->infoheader.incomplete = 0;
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
n = entries;
}
coel->infoheader.reserved = 0x00;
ESCvar.frags = frags;
ESCvar.fragsleft = frags - 1;
coel->infoheader.fragmentsleft = htoes (ESCvar.fragsleft);
coel->index = htoes ((uint16) 0x01);
p = &(coel->datatype);
for (i = 0; i < n; i++)
{
*p = htoes (SDOobjects[i].index);
p++;
}
coel->mbxheader.length = htoes (0x08 + (n << 1));
}
MBXcontrol[MBXout].state = MBXstate_outreq;
}
}
/** Function for continuing sending left overs from previous requested
* SDO Get OD List, validating the request and sending an response.
*
*/
void SDO_getodlistcont (void)
{
uint8 MBXout;
uint16 i, n, s;
uint16 *p;
_COEobjdesc *coel;
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coel = (_COEobjdesc *) &MBX[MBXout];
coel->mbxheader.mbxtype = MBXCOE;
coel->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
coel->infoheader.opcode = COE_GETODLISTRESPONSE;
s = (ESCvar.frags - ESCvar.fragsleft) * (ODLISTSIZE >> 1);
if (ESCvar.fragsleft > 1)
{
coel->infoheader.incomplete = 1;
n = s + (ODLISTSIZE >> 1);
}
else
{
coel->infoheader.incomplete = 0;
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
n = ESCvar.entries;
}
coel->infoheader.reserved = 0x00;
ESCvar.fragsleft--;
coel->infoheader.fragmentsleft = htoes (ESCvar.fragsleft);
/* pointer 2 bytes back to exclude index */
p = &(coel->index);
for (i = s; i < n; i++)
{
*p = htoes (SDOobjects[i].index);
p++;
}
coel->mbxheader.length = htoes (0x06 + ((n - s) << 1));
MBXcontrol[MBXout].state = MBXstate_outreq;
}
}
/** Function for handling incoming requested SDO Get Object Description,
* validating the request and sending an response. On error an
* SDO Info Error will be sent.
*/
void SDO_getod (void)
{
uint8 MBXout;
uint16 index;
int32 nidx;
uint8 *d;
uint8 FLASHSTORE *s;
uint8 n = 0;
_COEobjdesc *coer, *coel;
coer = (_COEobjdesc *) &MBX[0];
index = etohs (coer->index);
nidx = SDO_findobject (index);
if (nidx >= 0)
{
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coel = (_COEobjdesc *) &MBX[MBXout];
coel->mbxheader.mbxtype = MBXCOE;
coel->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
coel->infoheader.opcode = COE_GETODRESPONSE;
coel->infoheader.incomplete = 0;
coel->infoheader.reserved = 0x00;
coel->infoheader.fragmentsleft = htoes (0);
coel->index = htoes (index);
if (SDOobjects[nidx].objtype == OTYPE_VAR)
{
int32 nsub = SDO_findsubindex (nidx, 0);
_objd FLASHSTORE *objd = SDOobjects[nidx].objdesc;
coel->datatype = htoes ((objd + nsub)->datatype);
}
else
{
coel->datatype = htoes (0);
}
coel->maxsub = SDOobjects[nidx].maxsub;
coel->objectcode = SDOobjects[nidx].objtype;
s = (uint8 *) SDOobjects[nidx].name;
d = (uint8 *) &(coel->name);
while (*s && (n < (MBXDSIZE - 0x0c)))
{
*d = *s;
n++;
s++;
d++;
}
*d = *s;
coel->mbxheader.length = htoes ((uint16) 0x0c + n);
MBXcontrol[MBXout].state = MBXstate_outreq;
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
}
}
else
{
SDO_infoerror (ABORT_NOOBJECT);
}
}
/** Function for handling incoming requested SDO Get Entry Description,
* validating the request and sending an response. On error an
* SDO Info Error will be sent.
*/
void SDO_geted (void)
{
uint8 MBXout;
uint16 index;
int32 nidx, nsub;
uint8 subindex;
uint8 *d;
uint8 FLASHSTORE *s;
_objd FLASHSTORE *objd;
uint8 n = 0;
_COEentdesc *coer, *coel;
coer = (_COEentdesc *) &MBX[0];
index = etohs (coer->index);
subindex = coer->subindex;
nidx = SDO_findobject (index);
if (nidx >= 0)
{
nsub = SDO_findsubindex (nidx, subindex);
if (nsub >= 0)
{
objd = SDOobjects[nidx].objdesc;
MBXout = ESC_claimbuffer ();
if (MBXout)
{
coel = (_COEentdesc *) &MBX[MBXout];
coel->mbxheader.mbxtype = MBXCOE;
coel->coeheader.numberservice =
htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
coel->infoheader.opcode = COE_ENTRYDESCRIPTIONRESPONSE;
coel->infoheader.incomplete = 0;
coel->infoheader.reserved = 0x00;
coel->infoheader.fragmentsleft = htoes ((uint16) 0);
coel->index = htoes (index);
coel->subindex = subindex;
coel->valueinfo = COE_VALUEINFO_ACCESS +
COE_VALUEINFO_OBJECT + COE_VALUEINFO_MAPPABLE;
coel->datatype = htoes ((objd + nsub)->datatype);
coel->bitlength = htoes ((objd + nsub)->bitlength);
coel->access = htoes ((objd + nsub)->access);
s = (uint8 *) (objd + nsub)->name;
d = (uint8 *) &(coel->name);
while (*s && (n < (MBXDSIZE - 0x10)))
{
*d = *s;
n++;
s++;
d++;
}
*d = *s;
coel->mbxheader.length = htoes ((uint16) 0x10 + n);
MBXcontrol[MBXout].state = MBXstate_outreq;
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
}
}
else
{
SDO_infoerror (ABORT_NOSUBINDEX);
}
}
else
{
SDO_infoerror (ABORT_NOOBJECT);
}
}
/** Main CoE function checking the status on current mailbox buffers carrying
* data, distributing the mailboxes to appropriate CoE functions.
* On Error an MBX_error or SDO Abort will be sent depending on error cause.
*/
void ESC_coeprocess (void)
{
_MBXh *mbh;
_COEsdo *coesdo;
_COEobjdesc *coeobjdesc;
uint8 service;
if (!MBXrun)
{
return;
}
if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
{
mbh = (_MBXh *) &MBX[0];
if (mbh->mbxtype == MBXCOE)
{
if (mbh->length < 8)
{
MBX_error (MBXERR_INVALIDSIZE);
}
else
ESCvar.xoe = MBXCOE;
}
}
if ((ESCvar.xoe == (MBXCOE + MBXODL)) && (!ESCvar.mbxoutpost))
{
/* continue get OD list */
SDO_getodlistcont ();
}
if (ESCvar.xoe == MBXCOE)
{
coesdo = (_COEsdo *) &MBX[0];
coeobjdesc = (_COEobjdesc *) &MBX[0];
service = etohs (coesdo->coeheader.numberservice) >> 12;
/* initiate SDO upload request */
if ((service == COE_SDOREQUEST)
&& (coesdo->command == COE_COMMAND_UPLOADREQUEST)
&& (etohs (coesdo->mbxheader.length) == 0x0a))
{
SDO_upload ();
}
/* SDO upload segment request */
if ((service == COE_SDOREQUEST)
&& ((coesdo->command & 0xef) == COE_COMMAND_UPLOADSEGREQ)
&& (etohs (coesdo->mbxheader.length) == 0x0a)
&& (ESCvar.segmented == MBXSEU))
{
SDO_uploadsegment ();
}
/* initiate SDO download request */
else
{
if ((service == COE_SDOREQUEST) && ((coesdo->command & 0xf1) == 0x21))
{
SDO_download ();
}
/* initiate SDO get OD list */
else
{
if ((service == COE_SDOINFORMATION)
&& (coeobjdesc->infoheader.opcode == 0x01))
{
SDO_getodlist ();
}
/* initiate SDO get OD */
else
{
if ((service == COE_SDOINFORMATION)
&& (coeobjdesc->infoheader.opcode == 0x03))
{
SDO_getod ();
}
/* initiate SDO get ED */
else
{
if ((service == COE_SDOINFORMATION)
&& (coeobjdesc->infoheader.opcode == 0x05))
{
SDO_geted ();
}
else
{
/* COE not recognised above */
if (ESCvar.xoe == MBXCOE)
{
if (service == 0)
{
MBX_error (MBXERR_INVALIDHEADER);
}
else
{
SDO_abort (etohs (coesdo->index), coesdo->subindex, ABORT_UNSUPPORTED);
}
MBXcontrol[0].state = MBXstate_idle;
ESCvar.xoe = 0;
}
}
}
}
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink