ethercat_com.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 #include <stdio.h>
37 #include <errno.h>
38 
39 EthercatDirectCom::EthercatDirectCom(EtherCAT_DataLinkLayer *dll) :
40  dll_(dll)
41 {
42  assert(dll_ != NULL);
43 }
44 
46 {
47  dll_ = NULL;
48 }
49 
50 bool EthercatDirectCom::txandrx_once(struct EtherCAT_Frame * frame)
51 {
52  assert(frame!=NULL);
53  int handle = dll_->tx(frame);
54  if (handle < 0)
55  return false;
56  return dll_->rx(frame, handle);
57 }
58 
59 bool EthercatDirectCom::txandrx(struct EtherCAT_Frame * frame)
60 {
61  return dll_->txandrx(frame);
62 }
63 
64 EthercatOobCom::EthercatOobCom(struct netif *ni) :
65  ni_(ni),
66  state_(IDLE),
67  frame_(NULL),
68  handle_(-1),
69  line_(0)
70 {
71  assert(ni_!=NULL);
72 
73  pthread_mutexattr_t mutex_attr;
74  int error = pthread_mutexattr_init(&mutex_attr);
75  if (error != 0) {
76  fprintf(stderr,"%s : Initializing mutex attr failed : %d\n", __func__, error);
77  return;
78  }
79  error = pthread_mutexattr_settype(&mutex_attr, PTHREAD_MUTEX_ERRORCHECK_NP);
80  if (error != 0) {
81  fprintf(stderr,"%s : Setting type of mutex attr failed : %d\n", __func__, error);
82  return;
83  }
84  error = pthread_mutex_init(&mutex_, &mutex_attr);
85  if (error != 0) {
86  fprintf(stderr,"%s : Initializing mutex failed : %d\n", __func__, error);
87  return;
88  }
89  error = pthread_cond_init(&share_cond_,NULL);
90  if (error != 0) {
91  fprintf(stderr,"%s : Initializing share condition failed : %d\n", __func__, error);
92  return;
93  }
94  error = pthread_cond_init(&busy_cond_,NULL);
95  if (error != 0) {
96  fprintf(stderr,"%s : Initializing busy condition failed : %d\n", __func__, error);
97  }
98  return;
99 }
100 
102 {
103  ni_=NULL;
104 }
105 
106 
107 bool EthercatOobCom::lock(unsigned line)
108 {
109  int error;
110  if (0 != (error = pthread_mutex_lock(&mutex_))) {
111  fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
112  return false;
113  }
114  line_ = line;
115  return true;
116 }
117 
118 bool EthercatOobCom::trylock(unsigned line)
119 {
120  int error;
121  if (0 != (error = pthread_mutex_trylock(&mutex_))) {
122  if (error != EBUSY) {
123  fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
124  }
125  return false;
126  }
127  line_ = line;
128  return true;
129 }
130 
131 
132 bool EthercatOobCom::unlock(unsigned line)
133 {
134  int error;
135  if (0 != (error = pthread_mutex_unlock(&mutex_))) {
136  fprintf(stderr, "%s : unlock %d at %d\n", __func__, error, line);
137  return false;
138  }
139  line_ = 0;
140  return true;
141 }
142 
143 
144 // OOB replacement for netif->txandrx()
145 // Returns true for success, false for dropped packet
146 bool EthercatOobCom::txandrx_once(struct EtherCAT_Frame * frame)
147 {
148  assert(frame != NULL);
149 
150  if (!lock(__LINE__))
151  return false;
152 
153  // Wait for an opening to send frame
154  while (state_ != IDLE) {
155  pthread_cond_wait(&share_cond_,&mutex_);
156  }
157  frame_ = frame;
159 
160  // RT control loop will send frame
161  do {
162  pthread_cond_wait(&busy_cond_,&mutex_);
163  } while (state_ != WAITING_TO_RECV);
164 
165  // Packet has been sent, wait for recv
166  bool success = false;
167  if (handle_ >= 0) {
168  success = ni_->rx(frame_, ni_, handle_);
169  }
170  handle_=-1;
171 
172  // Allow other threads to send data
173  assert(frame_ == frame);
174  state_ = IDLE;
175  pthread_cond_signal(&share_cond_);
176 
177  unlock(__LINE__);
178 
179  return success;
180 }
181 
182 bool EthercatOobCom::txandrx(struct EtherCAT_Frame * frame)
183 {
184  static const unsigned MAX_TRIES=10;
185  for (unsigned tries=0; tries<MAX_TRIES; ++tries) {
186  if (this->txandrx_once(frame)) {
187  return true;
188  }
189  }
190  return false;
191 }
192 
193 // Called by RT control loop to send oob data
195 {
196  if (!trylock(__LINE__))
197  return;
198 
199  if (state_ == READY_TO_SEND) {
200  // Packet is in need of being sent
201  assert(frame_!=NULL);
202  handle_ = ni_->tx(frame_, ni_);
204  pthread_cond_signal(&busy_cond_);
205  }
206 
207  unlock(__LINE__);
208 }
209 
unsigned line_
Definition: ethercat_com.h:92
bool trylock(unsigned line)
EthercatDirectCom(EtherCAT_DataLinkLayer *dll)
struct netif * ni_
Definition: ethercat_com.h:85
bool txandrx_once(struct EtherCAT_Frame *frame)
EtherCAT_DataLinkLayer * dll_
Definition: ethercat_com.h:67
bool txandrx(struct EtherCAT_Frame *frame)
pthread_mutex_t mutex_
Definition: ethercat_com.h:86
bool lock(unsigned line)
pthread_cond_t busy_cond_
Definition: ethercat_com.h:88
EthercatOobCom(struct netif *ni)
pthread_cond_t share_cond_
Definition: ethercat_com.h:87
enum EthercatOobCom::@1 state_
bool unlock(unsigned line)
EtherCAT_Frame * frame_
Definition: ethercat_com.h:90
bool txandrx(struct EtherCAT_Frame *frame)
bool txandrx_once(struct EtherCAT_Frame *frame)


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Mar 4 2021 03:10:21