ethercatsoe.c
Go to the documentation of this file.
1 /*
2  * Simple Open EtherCAT Master Library
3  *
4  * File : ethercatsoe.c
5  * Version : 1.3.0
6  * Date : 24-02-2013
7  * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f.
8  * Copyright (C) 2005-2013 Arthur Ketels
9  * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven
10  * Thanks to Hidde Verhoef for testing and improving the SoE module
11  *
12  * SOEM is free software; you can redistribute it and/or modify it under
13  * the terms of the GNU General Public License version 2 as published by the Free
14  * Software Foundation.
15  *
16  * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY
17  * WARRANTY; without even the implied warranty of MERCHANTABILITY or
18  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
19  * for more details.
20  *
21  * As a special exception, if other files instantiate templates or use macros
22  * or inline functions from this file, or you compile this file and link it
23  * with other works to produce a work based on this file, this file does not
24  * by itself cause the resulting work to be covered by the GNU General Public
25  * License. However the source code for this file must still be made available
26  * in accordance with section (3) of the GNU General Public License.
27  *
28  * This exception does not invalidate any other reasons why a work based on
29  * this file might be covered by the GNU General Public License.
30  *
31  * The EtherCAT Technology, the trade name and logo EtherCAT are the intellectual
32  * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for
33  * the sole purpose of creating, using and/or selling or otherwise distributing
34  * an EtherCAT network master provided that an EtherCAT Master License is obtained
35  * from Beckhoff Automation GmbH.
36  *
37  * In case you did not receive a copy of the EtherCAT Master License along with
38  * SOEM write to Beckhoff Automation GmbH, Eiserstra├če 5, D-33415 Verl, Germany
39  * (www.beckhoff.com).
40  */
41 
47 #include <stdio.h>
48 #include <string.h>
49 #include "osal.h"
50 #include "oshw.h"
51 #include "ethercattype.h"
52 #include "ethercatbase.h"
53 #include "ethercatmain.h"
54 #include "ethercatsoe.h"
55 
56 
58 PACKED_BEGIN
59 typedef struct PACKED
60 {
67  union
68  {
69  uint16 idn;
71  };
72 } ec_SoEt;
73 PACKED_END
74 
82 void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error)
83 {
84  ec_errort Ec;
85 
86  Ec.Time = osal_current_time();
87  Ec.Slave = Slave;
88  Ec.Index = idn;
89  Ec.SubIdx = 0;
90  *(context->ecaterror) = TRUE;
92  Ec.ErrorCode = Error;
93  ecx_pusherror(context, &Ec);
94 }
95 
112 int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
113 {
114  ec_SoEt *SoEp, *aSoEp;
115  uint16 totalsize, framedatasize;
116  int wkc;
117  uint8 *bp;
118  uint8 *mp;
119  uint16 *errorcode;
120  ec_mbxbuft MbxIn, MbxOut;
121  uint8 cnt;
122  boolean NotLast;
123 
124  ec_clearmbx(&MbxIn);
125  /* Empty slave out mailbox if something is in. Timeout set to 0 */
126  wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
127  ec_clearmbx(&MbxOut);
128  aSoEp = (ec_SoEt *)&MbxIn;
129  SoEp = (ec_SoEt *)&MbxOut;
130  SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert));
131  SoEp->MbxHeader.address = htoes(0x0000);
132  SoEp->MbxHeader.priority = 0x00;
133  /* get new mailbox count value, used as session handle */
134  cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
135  context->slavelist[slave].mbx_cnt = cnt;
136  SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
137  SoEp->opCode = ECT_SOE_READREQ;
138  SoEp->incomplete = 0;
139  SoEp->error = 0;
140  SoEp->driveNo = driveNo;
141  SoEp->elementflags = elementflags;
142  SoEp->idn = htoes(idn);
143  totalsize = 0;
144  bp = p;
145  mp = (uint8 *)&MbxIn + sizeof(ec_SoEt);
146  NotLast = TRUE;
147  /* send SoE request to slave */
148  wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
149  if (wkc > 0) /* succeeded to place mailbox in slave ? */
150  {
151  while (NotLast)
152  {
153  /* clean mailboxbuffer */
154  ec_clearmbx(&MbxIn);
155  /* read slave response */
156  wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
157  if (wkc > 0) /* succeeded to read slave response ? */
158  {
159  /* slave response should be SoE, ReadRes */
160  if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
161  (aSoEp->opCode == ECT_SOE_READRES) &&
162  (aSoEp->error == 0) &&
163  (aSoEp->driveNo == driveNo) &&
164  (aSoEp->elementflags == elementflags))
165  {
166  framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt) + sizeof(ec_mbxheadert);
167  totalsize += framedatasize;
168  /* Does parameter fit in parameter buffer ? */
169  if (totalsize <= *psize)
170  {
171  /* copy parameter data in parameter buffer */
172  memcpy(bp, mp, framedatasize);
173  /* increment buffer pointer */
174  bp += framedatasize;
175  }
176  else
177  {
178  framedatasize -= totalsize - *psize;
179  totalsize = *psize;
180  /* copy parameter data in parameter buffer */
181  if (framedatasize > 0) memcpy(bp, mp, framedatasize);
182  }
183 
184  if (!aSoEp->incomplete)
185  {
186  NotLast = FALSE;
187  *psize = totalsize;
188  }
189  }
190  /* other slave response */
191  else
192  {
193  NotLast = FALSE;
194  if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
195  (aSoEp->opCode == ECT_SOE_READRES) &&
196  (aSoEp->error == 1))
197  {
198  mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
199  errorcode = (uint16 *)mp;
200  ecx_SoEerror(context, slave, idn, *errorcode);
201  }
202  else
203  {
204  ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
205  }
206  wkc = 0;
207  }
208  }
209  else
210  {
211  NotLast = FALSE;
212  ecx_packeterror(context, slave, idn, 0, 4); /* no response */
213  }
214  }
215  }
216  return wkc;
217 }
218 
234 int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
235 {
236  ec_SoEt *SoEp, *aSoEp;
237  uint16 framedatasize, maxdata;
238  int wkc;
239  uint8 *mp;
240  uint8 *hp;
241  uint16 *errorcode;
242  ec_mbxbuft MbxIn, MbxOut;
243  uint8 cnt;
244  boolean NotLast;
245 
246  ec_clearmbx(&MbxIn);
247  /* Empty slave out mailbox if something is in. Timeout set to 0 */
248  wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
249  ec_clearmbx(&MbxOut);
250  aSoEp = (ec_SoEt *)&MbxIn;
251  SoEp = (ec_SoEt *)&MbxOut;
252  SoEp->MbxHeader.address = htoes(0x0000);
253  SoEp->MbxHeader.priority = 0x00;
254  SoEp->opCode = ECT_SOE_WRITEREQ;
255  SoEp->error = 0;
256  SoEp->driveNo = driveNo;
257  SoEp->elementflags = elementflags;
258  hp = p;
259  mp = (uint8 *)&MbxOut + sizeof(ec_SoEt);
260  maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt);
261  NotLast = TRUE;
262  while (NotLast)
263  {
264  framedatasize = psize;
265  NotLast = FALSE;
266  SoEp->idn = htoes(idn);
267  SoEp->incomplete = 0;
268  if (framedatasize > maxdata)
269  {
270  framedatasize = maxdata; /* segmented transfer needed */
271  NotLast = TRUE;
272  SoEp->incomplete = 1;
273  SoEp->fragmentsleft = psize / maxdata;
274  }
275  SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize);
276  /* get new mailbox counter, used for session handle */
277  cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
278  context->slavelist[slave].mbx_cnt = cnt;
279  SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
280  /* copy parameter data to mailbox */
281  memcpy(mp, hp, framedatasize);
282  hp += framedatasize;
283  psize -= framedatasize;
284  /* send SoE request to slave */
285  wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
286  if (wkc > 0) /* succeeded to place mailbox in slave ? */
287  {
288  if (!NotLast || !ecx_mbxempty(context, slave, timeout))
289  {
290  /* clean mailboxbuffer */
291  ec_clearmbx(&MbxIn);
292  /* read slave response */
293  wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
294  if (wkc > 0) /* succeeded to read slave response ? */
295  {
296  NotLast = FALSE;
297  /* slave response should be SoE, WriteRes */
298  if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
299  (aSoEp->opCode == ECT_SOE_WRITERES) &&
300  (aSoEp->error == 0) &&
301  (aSoEp->driveNo == driveNo) &&
302  (aSoEp->elementflags == elementflags))
303  {
304  /* SoE write succeeded */
305  }
306  /* other slave response */
307  else
308  {
309  if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
310  (aSoEp->opCode == ECT_SOE_READRES) &&
311  (aSoEp->error == 1))
312  {
313  mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
314  errorcode = (uint16 *)mp;
315  ecx_SoEerror(context, slave, idn, *errorcode);
316  }
317  else
318  {
319  ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
320  }
321  wkc = 0;
322  }
323  }
324  else
325  {
326  ecx_packeterror(context, slave, idn, 0, 4); /* no response */
327  }
328  }
329  }
330  }
331  return wkc;
332 }
333 
346 int ecx_readIDNmap(ecx_contextt *context, uint16 slave, int *Osize, int *Isize)
347 {
348  int retVal = 0;
349  int wkc;
350  int psize;
351  uint16 entries, itemcount;
352  ec_SoEmappingt SoEmapping;
353  ec_SoEattributet SoEattribute;
354 
355  *Isize = 0;
356  *Osize = 0;
357  psize = sizeof(SoEmapping);
358  /* read output mapping via SoE */
359  wkc = ecx_SoEread(context, slave, 0, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
360  if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
361  {
362  /* command word (uint16) is always mapped but not in list */
363  *Osize = 16;
364  for (itemcount = 0 ; itemcount < entries ; itemcount++)
365  {
366  psize = sizeof(SoEattribute);
367  /* read attribute of each IDN in mapping list */
368  wkc = ecx_SoEread(context, slave, 0, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
369  if ((wkc > 0) && (!SoEattribute.list))
370  {
371  /* length : 0 = 8bit, 1 = 16bit .... */
372  *Osize += (int)8 << SoEattribute.length;
373  }
374  }
375  }
376  psize = sizeof(SoEmapping);
377  /* read input mapping via SoE */
378  wkc = ecx_SoEread(context, slave, 0, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
379  if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
380  {
381  /* status word (uint16) is always mapped but not in list */
382  *Isize = 16;
383  for (itemcount = 0 ; itemcount < entries ; itemcount++)
384  {
385  psize = sizeof(SoEattribute);
386  /* read attribute of each IDN in mapping list */
387  wkc = ecx_SoEread(context, slave, 0, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
388  if ((wkc > 0) && (!SoEattribute.list))
389  {
390  /* length : 0 = 8bit, 1 = 16bit .... */
391  *Isize += (int)8 << SoEattribute.length;
392  }
393  }
394  }
395 
396  /* found some I/O bits ? */
397  if ((*Isize > 0) || (*Osize > 0))
398  {
399  retVal = 1;
400  }
401  return retVal;
402 }
403 
404 #ifdef EC_VER1
405 int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
406 {
407  return ecx_SoEread(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
408 }
409 
410 int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
411 {
412  return ecx_SoEwrite(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
413 }
414 
415 int ec_readIDNmap(uint16 slave, int *Osize, int *Isize)
416 {
417  return ecx_readIDNmap(&ecx_context, slave, Osize, Isize);
418 }
419 #endif
#define EC_SOE_MAXMAPPING
Definition: ethercatsoe.h:64
#define EC_SOE_VALUE_B
Definition: ethercatsoe.h:60
ec_slavet * slavelist
Definition: ethercatmain.h:418
int ecx_readIDNmap(ecx_contextt *context, uint16 slave, int *Osize, int *Isize)
Definition: ethercatsoe.c:346
uint8 incomplete
Definition: ethercatsoe.c:63
uint8_t uint8
Definition: osal.h:33
PACKED_BEGIN struct PACKED ec_SoEattributet
uint16 mbx_l
Definition: ethercatmain.h:188
void ecx_pusherror(ecx_contextt *context, const ec_errort *Ec)
Definition: ethercatmain.c:188
void ec_clearmbx(ec_mbxbuft *Mbx)
Definition: ethercatmain.c:832
uint8 SubIdx
Definition: ethercattype.h:503
PACKED_BEGIN struct PACKED ec_SoEt
void ecx_packeterror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, uint16 ErrorCode)
Definition: ethercatmain.c:253
uint16_t uint16
Definition: osal.h:34
uint8 driveNo
Definition: ethercatsoe.c:65
#define EC_TIMEOUTRXM
Definition: ethercattype.h:100
General typedefs and defines for EtherCAT.
uint8 opCode
Definition: ethercatsoe.c:62
#define etohs(A)
Definition: ethercattype.h:550
#define TRUE
Definition: osal.h:28
ec_err_type Etype
Definition: ethercattype.h:505
#define EC_SOE_ATTRIBUTE_B
Definition: ethercatsoe.h:56
ec_timet osal_current_time(void)
Definition: linux/osal.c:52
boolean * ecaterror
Definition: ethercatmain.h:438
#define EC_IDN_MDTCONFIG
Definition: ethercatsoe.h:66
int ecx_mbxempty(ecx_contextt *context, uint16 slave, int timeout)
Definition: ethercatmain.c:843
PACKED_BEGIN struct PACKED ec_mbxheadert
#define EC_TIMEOUTTXM
Definition: ethercattype.h:98
uint16 Slave
Definition: ethercattype.h:499
ec_mbxheadert MbxHeader
Definition: ethercatcoe.c:61
#define FALSE
Definition: osal.h:29
Headerfile for ethercatsoe.c.
uint8 ec_nextmbxcnt(uint8 cnt)
Definition: ethercatmain.c:818
uint8 mbx_cnt
Definition: ethercatmain.h:198
int ecx_mbxsend(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int timeout)
Definition: ethercatmain.c:878
int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
Definition: ethercatsoe.c:112
uint8 elementflags
Definition: ethercatsoe.c:66
PACKED_END PACKED_BEGIN struct PACKED ec_SoEmappingt
ec_timet Time
Definition: ethercattype.h:495
PACKED_END void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error)
Definition: ethercatsoe.c:82
uint16 ErrorCode
Definition: ethercattype.h:513
Headerfile for ethercatbase.c.
#define htoes(A)
Definition: ethercattype.h:547
uint16 Index
Definition: ethercattype.h:501
Headerfile for ethercatmain.c.
int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
Definition: ethercatsoe.c:234
uint8 error
Definition: ethercatsoe.c:64
uint16 fragmentsleft
Definition: ethercatsoe.c:70
int ecx_mbxreceive(ecx_contextt *context, uint16 slave, ec_mbxbuft *mbx, int timeout)
Definition: ethercatmain.c:911
uint16 idn[EC_SOE_MAXMAPPING]
Definition: ethercatsoe.h:107
#define EC_IDN_ATCONFIG
Definition: ethercatsoe.h:67
uint8 ec_mbxbuft[EC_MAXMBX+1]
Definition: ethercatmain.h:341


youbot_driver
Author(s): Jan Paulus
autogenerated on Mon Jun 10 2019 15:46:24