296 lines
9.5 KiB
C
296 lines
9.5 KiB
C
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#include <ethercat.h>
|
|
#include "ecmbind.h"
|
|
|
|
/** SDO service structure */
|
|
PACKED_BEGIN
|
|
typedef struct PACKED
|
|
{
|
|
ec_mbxheadert MbxHeader;
|
|
uint16 CANOpen;
|
|
uint8 Opcode;
|
|
uint8 Reserved;
|
|
uint16 Fragments;
|
|
union
|
|
{
|
|
uint8 bdata[0x200]; /* variants for easy data access */
|
|
uint16 wdata[0x100];
|
|
uint32 ldata[0x80];
|
|
};
|
|
} ec_SDOservicet;
|
|
PACKED_END
|
|
|
|
|
|
|
|
static void ecx_SDOinfoerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
|
|
{
|
|
ec_errort Ec;
|
|
|
|
memset(&Ec, 0, sizeof(Ec));
|
|
Ec.Slave = Slave;
|
|
Ec.Index = Index;
|
|
Ec.SubIdx = SubIdx;
|
|
*(context->ecaterror) = TRUE;
|
|
Ec.Etype = EC_ERR_TYPE_SDOINFO_ERROR;
|
|
Ec.AbortCode = AbortCode;
|
|
ecx_pusherror(context, &Ec);
|
|
}
|
|
|
|
|
|
int ecmbind_enumerate_servicedescriptors(int Slave, cb_enum_indeces_t cb)
|
|
{
|
|
ecx_contextt *context = &ecx_context;
|
|
ec_SDOservicet *SDOp, *aSDOp;
|
|
ec_mbxbuft MbxIn, MbxOut;
|
|
int wkc;
|
|
uint16 x, n, i, sp, offset;
|
|
boolean stop;
|
|
uint8 cnt;
|
|
boolean First;
|
|
|
|
if (cb == NULL)
|
|
return -1;
|
|
|
|
ec_clearmbx(&MbxIn);
|
|
/* clear pending out mailbox in slave if available. Timeout is set to 0 */
|
|
wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
|
|
ec_clearmbx(&MbxOut);
|
|
aSDOp = (ec_SDOservicet*)&MbxIn;
|
|
SDOp = (ec_SDOservicet*)&MbxOut;
|
|
SDOp->MbxHeader.length = htoes(0x0008);
|
|
SDOp->MbxHeader.address = htoes(0x0000);
|
|
SDOp->MbxHeader.priority = 0x00;
|
|
/* Get new mailbox counter value */
|
|
cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
|
|
context->slavelist[Slave].mbx_cnt = cnt;
|
|
SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
|
|
SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */
|
|
SDOp->Opcode = ECT_GET_ODLIST_REQ; /* get object description list request */
|
|
SDOp->Reserved = 0;
|
|
SDOp->Fragments = 0; /* fragments left */
|
|
SDOp->wdata[0] = htoes(0x01); /* all objects */
|
|
/* send get object description list request to slave */
|
|
wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
|
|
/* mailbox placed in slave ? */
|
|
if (wkc > 0)
|
|
{
|
|
x = 0;
|
|
sp = 0;
|
|
First = TRUE;
|
|
offset = 1; /* offset to skip info header in first frame, otherwise set to 0 */
|
|
do
|
|
{
|
|
stop = TRUE; /* assume this is last iteration */
|
|
ec_clearmbx(&MbxIn);
|
|
/* read slave response */
|
|
wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
|
|
/* got response ? */
|
|
if (wkc > 0)
|
|
{
|
|
/* response should be CoE and "get object description list response" */
|
|
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
|
|
((aSDOp->Opcode & 0x7f) == ECT_GET_ODLIST_RES))
|
|
{
|
|
if (First)
|
|
{
|
|
/* extract number of indexes from mailbox data size */
|
|
n = (etohs(aSDOp->MbxHeader.length) - (6 + 2)) / 2;
|
|
}
|
|
else
|
|
{
|
|
/* extract number of indexes from mailbox data size */
|
|
n = (etohs(aSDOp->MbxHeader.length) - 6) / 2;
|
|
}
|
|
|
|
/* extract indexes one by one */
|
|
for (i = 0; i < n; i++)
|
|
cb(Slave, etohs(aSDOp->wdata[i + offset]));
|
|
|
|
sp += n;
|
|
/* check if more fragments will follow */
|
|
if (aSDOp->Fragments > 0)
|
|
{
|
|
stop = FALSE;
|
|
}
|
|
First = FALSE;
|
|
offset = 0;
|
|
}
|
|
/* got unexpected response from slave */
|
|
else
|
|
{
|
|
if ((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR) /* SDO info error received */
|
|
{
|
|
ecx_SDOinfoerror(context, Slave, 0, 0, etohl(aSDOp->ldata[0]));
|
|
stop = TRUE;
|
|
}
|
|
else
|
|
{
|
|
ecx_packeterror(context, Slave, 0, 0, 1); /* Unexpected frame returned */
|
|
}
|
|
wkc = 0;
|
|
x += 20;
|
|
}
|
|
}
|
|
x++;
|
|
}
|
|
while ((x <= 128) && !stop);
|
|
}
|
|
return wkc;
|
|
}
|
|
|
|
int ecmbind_read_objectdescription(int Slave, int index, cb_enum_sdo_descriptors_t cb)
|
|
{
|
|
ecx_contextt *context = &ecx_context;
|
|
ec_SDOservicet *SDOp, *aSDOp;
|
|
int wkc;
|
|
uint16 n;
|
|
ec_mbxbuft MbxIn, MbxOut;
|
|
uint8 cnt;
|
|
char temp[128];
|
|
|
|
ec_clearmbx(&MbxIn);
|
|
/* clear pending out mailbox in slave if available. Timeout is set to 0 */
|
|
wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0);
|
|
ec_clearmbx(&MbxOut);
|
|
aSDOp = (ec_SDOservicet*)&MbxIn;
|
|
SDOp = (ec_SDOservicet*)&MbxOut;
|
|
SDOp->MbxHeader.length = htoes(0x0008);
|
|
SDOp->MbxHeader.address = htoes(0x0000);
|
|
SDOp->MbxHeader.priority = 0x00;
|
|
/* Get new mailbox counter value */
|
|
cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
|
|
context->slavelist[Slave].mbx_cnt = cnt;
|
|
SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
|
|
SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */
|
|
SDOp->Opcode = ECT_GET_OD_REQ; /* get object description request */
|
|
SDOp->Reserved = 0;
|
|
SDOp->Fragments = 0; /* fragments left */
|
|
SDOp->wdata[0] = htoes(index); /* Data of Index */
|
|
/* send get object description request to slave */
|
|
wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
|
|
/* mailbox placed in slave ? */
|
|
if (wkc > 0)
|
|
{
|
|
ec_clearmbx(&MbxIn);
|
|
/* read slave response */
|
|
wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM);
|
|
/* got response ? */
|
|
if (wkc > 0)
|
|
{
|
|
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
|
|
((aSDOp->Opcode & 0x7f) == ECT_GET_OD_RES))
|
|
{
|
|
n = (etohs(aSDOp->MbxHeader.length) - 12); /* length of string(name of object) */
|
|
if (n > sizeof(temp)-1)
|
|
n = sizeof(temp)-1;
|
|
strncpy(temp , (char *)&aSDOp->bdata[6], n);
|
|
temp[n] = 0x00;
|
|
|
|
cb(Slave, index, etohs(aSDOp->wdata[1]), aSDOp->bdata[5], aSDOp->bdata[4], temp);
|
|
}
|
|
/* got unexpected response from slave */
|
|
else
|
|
{
|
|
if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */
|
|
{
|
|
ecx_SDOinfoerror(context, Slave, index, 0, etohl(aSDOp->ldata[0]));
|
|
}
|
|
else
|
|
{
|
|
ecx_packeterror(context, Slave, index, 0, 1); /* Unexpected frame returned */
|
|
}
|
|
wkc = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
return wkc;
|
|
}
|
|
|
|
int ecmbind_read_objectdescription_entry(uint16_t slave, uint16_t index, uint16_t sub, cb_enum_sdo_descriptors_t cb)
|
|
{
|
|
ecx_contextt *context = &ecx_context;
|
|
ec_SDOservicet *SDOp, *aSDOp;
|
|
int wkc;
|
|
int16 n;
|
|
ec_mbxbuft MbxIn, MbxOut;
|
|
uint8 cnt;
|
|
char temp[128];
|
|
|
|
wkc = 0;
|
|
ec_clearmbx(&MbxIn);
|
|
/* clear pending out mailbox in slave if available. Timeout is set to 0 */
|
|
wkc = ecx_mbxreceive(context, slave, &MbxIn, 0);
|
|
ec_clearmbx(&MbxOut);
|
|
aSDOp = (ec_SDOservicet*)&MbxIn;
|
|
SDOp = (ec_SDOservicet*)&MbxOut;
|
|
SDOp->MbxHeader.length = htoes(0x000a);
|
|
SDOp->MbxHeader.address = htoes(0x0000);
|
|
SDOp->MbxHeader.priority = 0x00;
|
|
/* Get new mailbox counter value */
|
|
cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
|
|
context->slavelist[slave].mbx_cnt = cnt;
|
|
SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
|
|
SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */
|
|
SDOp->Opcode = ECT_GET_OE_REQ; /* get object entry description request */
|
|
SDOp->Reserved = 0;
|
|
SDOp->Fragments = 0; /* fragments left */
|
|
SDOp->wdata[0] = htoes(index); /* index */
|
|
SDOp->bdata[2] = sub; /* Subindex */
|
|
SDOp->bdata[3] = 1 + 2 + 4; /* get access rights, object category, PDO */
|
|
/* send get object entry description request to slave */
|
|
wkc = ecx_mbxsend(context, slave, &MbxOut, EC_TIMEOUTTXM);
|
|
/* mailbox placed in slave ? */
|
|
if (wkc > 0)
|
|
{
|
|
ec_clearmbx(&MbxIn);
|
|
/* read slave response */
|
|
wkc = ecx_mbxreceive(context, slave, &MbxIn, EC_TIMEOUTRXM);
|
|
/* got response ? */
|
|
if (wkc > 0)
|
|
{
|
|
if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
|
|
((aSDOp->Opcode & 0x7f) == ECT_GET_OE_RES))
|
|
{
|
|
n = (etohs(aSDOp->MbxHeader.length) - 16); /* length of string(name of object) */
|
|
if (n > sizeof(temp)-1)
|
|
n = sizeof(temp)-1;
|
|
|
|
if (n < 0 )
|
|
n = 0;
|
|
|
|
strncpy(temp , (char *)&aSDOp->wdata[5], n);
|
|
temp[n] = 0x00; /* string terminator */
|
|
|
|
cb(slave, index, (ushort)etohs(aSDOp->wdata[2]), 0, sub, temp);
|
|
|
|
/*
|
|
pOElist->ValueInfo[sub] = aSDOp->bdata[3];
|
|
pOElist->DataType[sub] = etohs(aSDOp->wdata[2]);
|
|
pOElist->BitLength[sub] = etohs(aSDOp->wdata[3]);
|
|
pOElist->ObjAccess[sub] = etohs(aSDOp->wdata[4]);
|
|
*/
|
|
}
|
|
|
|
/* got unexpected response from slave */
|
|
else
|
|
{
|
|
if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */
|
|
{
|
|
ecx_SDOinfoerror(context, slave, index, sub, etohl(aSDOp->ldata[0]));
|
|
}
|
|
else
|
|
{
|
|
ecx_packeterror(context, slave, index, sub, 1); /* Unexpected frame returned */
|
|
}
|
|
wkc = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
return wkc;
|
|
}
|