ethercatbase.c
Go to the documentation of this file.
1 /*
2  * Simple Open EtherCAT Master Library
3  *
4  * File : ethercatbase.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  *
11  * SOEM is free software; you can redistribute it and/or modify it under
12  * the terms of the GNU General Public License version 2 as published by the Free
13  * Software Foundation.
14  *
15  * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY
16  * WARRANTY; without even the implied warranty of MERCHANTABILITY or
17  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
18  * for more details.
19  *
20  * As a special exception, if other files instantiate templates or use macros
21  * or inline functions from this file, or you compile this file and link it
22  * with other works to produce a work based on this file, this file does not
23  * by itself cause the resulting work to be covered by the GNU General Public
24  * License. However the source code for this file must still be made available
25  * in accordance with section (3) of the GNU General Public License.
26  *
27  * This exception does not invalidate any other reasons why a work based on
28  * this file might be covered by the GNU General Public License.
29  *
30  * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual
31  * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for
32  * the sole purpose of creating, using and/or selling or otherwise distributing
33  * an EtherCAT network master provided that an EtherCAT Master License is obtained
34  * from Beckhoff Automation GmbH.
35  *
36  * In case you did not receive a copy of the EtherCAT Master License along with
37  * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany
38  * (www.beckhoff.com).
39  */
40 
52 #include <stdio.h>
53 #include <string.h>
54 #include "oshw.h"
55 #include "osal.h"
56 #include "ethercattype.h"
57 #include "ethercatbase.h"
58 
59 
72 int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data)
73 {
74  ec_comt *datagramP;
75  uint8 *frameP;
76 
77  frameP = frame;
78  /* Ethernet header is preset and fixed in frame buffers
79  EtherCAT header needs to be added after that */
80  datagramP = (ec_comt*)&frameP[ETH_HEADERSIZE];
81  datagramP->elength = htoes(EC_ECATTYPE + EC_HEADERSIZE + length);
82  datagramP->command = com;
83  datagramP->index = idx;
84  datagramP->ADP = htoes(ADP);
85  datagramP->ADO = htoes(ADO);
86  datagramP->dlength = htoes(length);
87  if (length > 0)
88  {
89  memcpy(&frameP[ETH_HEADERSIZE + EC_HEADERSIZE], data, length);
90  }
91  /* set WKC to zero */
92  frameP[ETH_HEADERSIZE + EC_HEADERSIZE + length] = 0x00;
93  frameP[ETH_HEADERSIZE + EC_HEADERSIZE + length + 1] = 0x00;
94  /* set size of frame in buffer array */
95  port->txbuflength[idx] = ETH_HEADERSIZE + EC_HEADERSIZE + EC_WKCSIZE + length;
96 
97  return 0;
98 }
99 
113 int ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data)
114 {
115  ec_comt *datagramP;
116  uint8 *frameP;
117  uint16 prevlength;
118 
119  frameP = frame;
120  /* copy previous frame size */
121  prevlength = port->txbuflength[idx];
122  datagramP = (ec_comt*)&frameP[ETH_HEADERSIZE];
123  /* add new datagram to ethernet frame size */
124  datagramP->elength = htoes( etohs(datagramP->elength) + EC_HEADERSIZE + length );
125  /* add "datagram follows" flag to previous subframe dlength */
126  datagramP->dlength = htoes( etohs(datagramP->dlength) | EC_DATAGRAMFOLLOWS );
127  /* set new EtherCAT header position */
128  datagramP = (ec_comt*)&frameP[prevlength - EC_ELENGTHSIZE];
129  datagramP->command = com;
130  datagramP->index = idx;
131  datagramP->ADP = htoes(ADP);
132  datagramP->ADO = htoes(ADO);
133  if (more)
134  {
135  /* this is not the last datagram to add */
136  datagramP->dlength = htoes(length | EC_DATAGRAMFOLLOWS);
137  }
138  else
139  {
140  /* this is the last datagram in the frame */
141  datagramP->dlength = htoes(length);
142  }
143  if (length > 0)
144  {
145  memcpy(&frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE], data, length);
146  }
147  /* set WKC to zero */
148  frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + length] = 0x00;
149  frameP[prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + length + 1] = 0x00;
150  /* set size of frame in buffer array */
151  port->txbuflength[idx] = prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE + EC_WKCSIZE + length;
152 
153  /* return offset to data in rx frame
154  14 bytes smaller than tx frame due to stripping of ethernet header */
155  return prevlength + EC_HEADERSIZE - EC_ELENGTHSIZE - ETH_HEADERSIZE;
156 }
157 
168 int ecx_BWR (ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
169 {
170  uint8 idx;
171  int wkc;
172 
173  /* get fresh index */
174  idx = ecx_getindex (port);
175  /* setup datagram */
176  ecx_setupdatagram (port, &(port->txbuf[idx]), EC_CMD_BWR, idx, ADP, ADO, length, data);
177  /* send data and wait for answer */
178  wkc = ecx_srconfirm (port, idx, timeout);
179  /* clear buffer status */
180  ecx_setbufstat (port, idx, EC_BUF_EMPTY);
181 
182  return wkc;
183 }
184 
195 int ecx_BRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
196 {
197  uint8 idx;
198  int wkc;
199 
200  /* get fresh index */
201  idx = ecx_getindex(port);
202  /* setup datagram */
203  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_BRD, idx, ADP, ADO, length, data);
204  /* send data and wait for answer */
205  wkc = ecx_srconfirm (port, idx, timeout);
206  if (wkc > 0)
207  {
208  /* copy datagram to data buffer */
209  memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
210  }
211  /* clear buffer status */
212  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
213 
214  return wkc;
215 }
216 
227 int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
228 {
229  int wkc;
230  uint8 idx;
231 
232  idx = ecx_getindex(port);
233  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_APRD, idx, ADP, ADO, length, data);
234  wkc = ecx_srconfirm(port, idx, timeout);
235  if (wkc > 0)
236  {
237  memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
238  }
239  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
240 
241  return wkc;
242 }
243 
255 int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
256 {
257  int wkc;
258  uint8 idx;
259 
260  idx = ecx_getindex(port);
261  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_ARMW, idx, ADP, ADO, length, data);
262  wkc = ecx_srconfirm(port, idx, timeout);
263  if (wkc > 0)
264  {
265  memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
266  }
267  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
268 
269  return wkc;
270 }
271 
283 int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
284 {
285  int wkc;
286  uint8 idx;
287 
288  idx = ecx_getindex(port);
289  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FRMW, idx, ADP, ADO, length, data);
290  wkc = ecx_srconfirm(port, idx, timeout);
291  if (wkc > 0)
292  {
293  memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
294  }
295  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
296 
297  return wkc;
298 }
299 
308 uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout)
309 {
310  uint16 w;
311 
312  w = 0;
313  ecx_APRD(port, ADP, ADO, sizeof(w), &w, timeout);
314 
315  return w;
316 }
317 
328 int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
329 {
330  int wkc;
331  uint8 idx;
332 
333  idx = ecx_getindex(port);
334  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FPRD, idx, ADP, ADO, length, data);
335  wkc = ecx_srconfirm(port, idx, timeout);
336  if (wkc > 0)
337  {
338  memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
339  }
340  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
341 
342  return wkc;
343 }
344 
353 uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout)
354 {
355  uint16 w;
356 
357  w = 0;
358  ecx_FPRD(port, ADP, ADO, sizeof(w), &w, timeout);
359  return w;
360 }
361 
372 int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
373 {
374  uint8 idx;
375  int wkc;
376 
377  idx = ecx_getindex(port);
378  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_APWR, idx, ADP, ADO, length, data);
379  wkc = ecx_srconfirm(port, idx, timeout);
380  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
381 
382  return wkc;
383 }
384 
394 int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout)
395 {
396  return ecx_APWR(port, ADP, ADO, sizeof(data), &data, timeout);
397 }
398 
409 int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
410 {
411  int wkc;
412  uint8 idx;
413 
414  idx = ecx_getindex(port);
415  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_FPWR, idx, ADP, ADO, length, data);
416  wkc = ecx_srconfirm(port, idx, timeout);
417  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
418 
419  return wkc;
420 }
421 
431 int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout)
432 {
433  return ecx_FPWR(port, ADP, ADO, sizeof(data), &data, timeout);
434 }
435 
445 int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout)
446 {
447  uint8 idx;
448  int wkc;
449 
450  idx = ecx_getindex(port);
451  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LRW, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data);
452  wkc = ecx_srconfirm(port, idx, timeout);
453  if ((wkc > 0) && (port->rxbuf[idx][EC_CMDOFFSET] == EC_CMD_LRW))
454  {
455  memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
456  }
457  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
458 
459  return wkc;
460 }
461 
471 int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout)
472 {
473  uint8 idx;
474  int wkc;
475 
476  idx = ecx_getindex(port);
477  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LRD, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data);
478  wkc = ecx_srconfirm(port, idx, timeout);
479  if ((wkc > 0) && (port->rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRD))
480  {
481  memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
482  }
483  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
484 
485  return wkc;
486 }
487 
497 int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout)
498 {
499  uint8 idx;
500  int wkc;
501 
502  idx = ecx_getindex(port);
503  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LWR, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data);
504  wkc = ecx_srconfirm(port, idx, timeout);
505  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
506 
507  return wkc;
508 }
509 
522 int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout)
523 {
524  uint16 DCtO;
525  uint8 idx;
526  int wkc;
527  uint64 DCtE;
528 
529  idx = ecx_getindex(port);
530  /* LRW in first datagram */
531  ecx_setupdatagram(port, &(port->txbuf[idx]), EC_CMD_LRW, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data);
532  /* FPRMW in second datagram */
533  DCtE = htoell(*DCtime);
534  DCtO = ecx_adddatagram(port, &(port->txbuf[idx]), EC_CMD_FRMW, idx, FALSE, DCrs, ECT_REG_DCSYSTIME, sizeof(DCtime), &DCtE);
535  wkc = ecx_srconfirm(port, idx, timeout);
536  if ((wkc > 0) && (port->rxbuf[idx][EC_CMDOFFSET] == EC_CMD_LRW))
537  {
538  memcpy(data, &(port->rxbuf[idx][EC_HEADERSIZE]), length);
539  memcpy(&wkc, &(port->rxbuf[idx][EC_HEADERSIZE + length]), EC_WKCSIZE);
540  memcpy(&DCtE, &(port->rxbuf[idx][DCtO]), sizeof(*DCtime));
541  *DCtime = etohll(DCtE);
542  }
543  ecx_setbufstat(port, idx, EC_BUF_EMPTY);
544 
545  return wkc;
546 }
547 
548 #ifdef EC_VER1
549 int ec_setupdatagram(void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data)
550 {
551  return ecx_setupdatagram (&ecx_port, frame, com, idx, ADP, ADO, length, data);
552 }
553 
554 int ec_adddatagram (void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data)
555 {
556  return ecx_adddatagram (&ecx_port, frame, com, idx, more, ADP, ADO, length, data);
557 }
558 
559 int ec_BWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
560 {
561  return ecx_BWR (&ecx_port, ADP, ADO, length, data, timeout);
562 }
563 
564 int ec_BRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
565 {
566  return ecx_BRD(&ecx_port, ADP, ADO, length, data, timeout);
567 }
568 
569 int ec_APRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
570 {
571  return ecx_APRD(&ecx_port, ADP, ADO, length, data, timeout);
572 }
573 
574 int ec_ARMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
575 {
576  return ecx_ARMW(&ecx_port, ADP, ADO, length, data, timeout);
577 }
578 
579 int ec_FRMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
580 {
581  return ecx_FRMW(&ecx_port, ADP, ADO, length, data, timeout);
582 }
583 
584 uint16 ec_APRDw(uint16 ADP, uint16 ADO, int timeout)
585 {
586  uint16 w;
587 
588  w = 0;
589  ec_APRD(ADP, ADO, sizeof(w), &w, timeout);
590 
591  return w;
592 }
593 
594 int ec_FPRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
595 {
596  return ecx_FPRD(&ecx_port, ADP, ADO, length, data, timeout);
597 }
598 
599 uint16 ec_FPRDw(uint16 ADP, uint16 ADO, int timeout)
600 {
601  uint16 w;
602 
603  w = 0;
604  ec_FPRD(ADP, ADO, sizeof(w), &w, timeout);
605  return w;
606 }
607 
608 int ec_APWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
609 {
610  return ecx_APWR(&ecx_port, ADP, ADO, length, data, timeout);
611 }
612 
613 int ec_APWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout)
614 {
615  return ec_APWR(ADP, ADO, sizeof(data), &data, timeout);
616 }
617 
618 int ec_FPWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
619 {
620  return ecx_FPWR(&ecx_port, ADP, ADO, length, data, timeout);
621 }
622 
623 int ec_FPWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout)
624 {
625  return ec_FPWR(ADP, ADO, sizeof(data), &data, timeout);
626 }
627 
628 int ec_LRW(uint32 LogAdr, uint16 length, void *data, int timeout)
629 {
630  return ecx_LRW(&ecx_port, LogAdr, length, data, timeout);
631 }
632 
633 int ec_LRD(uint32 LogAdr, uint16 length, void *data, int timeout)
634 {
635  return ecx_LRD(&ecx_port, LogAdr, length, data, timeout);
636 }
637 
638 int ec_LWR(uint32 LogAdr, uint16 length, void *data, int timeout)
639 {
640  return ecx_LWR(&ecx_port, LogAdr, length, data, timeout);
641 }
642 
643 int ec_LRWDC(uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout)
644 {
645  return ecx_LRWDC(&ecx_port, LogAdr, length, data, DCrs, DCtime, timeout);
646 }
647 #endif
int ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data)
Definition: ethercatbase.c:113
int ecx_srconfirm(ecx_portt *port, int idx, int timeout)
Definition: linux/nicdrv.c:592
ec_bufT txbuf[EC_MAXBUF]
Definition: linux/nicdrv.h:106
void ecx_setbufstat(ecx_portt *port, int idx, int bufstat)
Definition: linux/nicdrv.c:278
int txbuflength[EC_MAXBUF]
Definition: linux/nicdrv.h:108
int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:497
int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout)
Definition: ethercatbase.c:522
uint8_t uint8
Definition: osal.h:33
int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout)
Definition: ethercatbase.c:394
int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout)
Definition: ethercatbase.c:431
int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:409
#define EC_DATAGRAMFOLLOWS
Definition: ethercattype.h:159
uint16_t uint16
Definition: osal.h:34
int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data)
Definition: ethercatbase.c:72
int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:372
General typedefs and defines for EtherCAT.
#define etohs(A)
Definition: ethercattype.h:550
#define ETH_HEADERSIZE
Definition: ethercattype.h:127
int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:328
ec_bufT rxbuf[EC_MAXBUF]
Definition: linux/nicdrv.h:96
int ecx_BRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:195
#define EC_HEADERSIZE
Definition: ethercattype.h:151
int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:283
int64_t int64
Definition: osal.h:36
#define FALSE
Definition: osal.h:29
#define EC_ELENGTHSIZE
Definition: ethercattype.h:153
PACKED_BEGIN struct PACKED ec_comt
int ecx_BWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:168
#define htoell(A)
Definition: ethercattype.h:549
#define EC_CMDOFFSET
Definition: ethercattype.h:155
int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:227
uint64_t uint64
Definition: osal.h:37
#define HI_WORD(l)
Definition: ethercattype.h:534
#define LO_WORD(l)
Definition: ethercattype.h:532
int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:471
Headerfile for ethercatbase.c.
#define htoes(A)
Definition: ethercattype.h:547
#define etohll(A)
Definition: ethercattype.h:552
uint32_t uint32
Definition: osal.h:35
uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout)
Definition: ethercatbase.c:353
uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout)
Definition: ethercatbase.c:308
int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:445
#define EC_ECATTYPE
Definition: ethercattype.h:86
#define EC_WKCSIZE
Definition: ethercattype.h:157
int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
Definition: ethercatbase.c:255
int ecx_getindex(ecx_portt *port)
Definition: linux/nicdrv.c:239


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