Lely core libraries  2.2.5
sdo.cpp
Go to the documentation of this file.
1 
24 #include "coapp.hpp"
25 
26 #include <lely/coapp/sdo.hpp>
27 #include <lely/co/csdo.hpp>
28 
29 #include <limits>
30 #include <memory>
31 #include <string>
32 #include <utility>
33 #include <vector>
34 
35 #include <cassert>
36 
37 namespace lely {
38 
39 namespace canopen {
40 
42 struct Sdo::Impl_ {
43  Impl_(CANNet* net, CODev* dev, uint8_t num);
44  Impl_(COCSDO* sdo, int timeout);
45  Impl_(const Impl_&) = delete;
46  Impl_& operator=(const Impl_&) = delete;
47  ~Impl_();
48 
49  void Submit(detail::SdoRequestBase& req);
50  ::std::size_t Cancel(detail::SdoRequestBase* req, SdoErrc ac);
51  ::std::size_t Abort(detail::SdoRequestBase* req);
52 
53  bool Pop(detail::SdoRequestBase* req, sllist& queue);
54 
55  template <class T>
56  void OnDownload(detail::SdoDownloadRequestBase<T>& req) noexcept;
57  template <class T>
58  void OnUpload(detail::SdoUploadRequestBase<T>& req) noexcept;
59 
60  void OnDnCon(COCSDO*, uint16_t idx, uint8_t subidx, uint32_t ac) noexcept;
61  template <class T>
62  void OnUpCon(COCSDO*, uint16_t idx, uint8_t subidx, uint32_t ac,
63  T value) noexcept;
64 
65  void OnCompletion(detail::SdoRequestBase& req) noexcept;
66 
67  ::std::shared_ptr<COCSDO> sdo;
68 
69  sllist queue;
70 };
71 
72 namespace detail {
73 
74 template <class T>
75 void
76 SdoDownloadRequestWrapper<T>::operator()() noexcept {
77  auto id = this->id;
78  auto idx = this->idx;
79  auto subidx = this->subidx;
80  auto ec = this->ec;
81  ::std::function<Signature> con;
82  con.swap(this->con_);
83  delete this;
84  if (con) con(id, idx, subidx, ec);
85 }
86 
87 template <class T>
88 void
89 SdoDownloadRequestWrapper<T>::OnRequest(void* data) noexcept {
90  static_cast<Sdo::Impl_*>(data)->OnDownload(*this);
91 }
92 
93 template <class T>
94 void
95 SdoUploadRequestWrapper<T>::operator()() noexcept {
96  auto id = this->id;
97  auto idx = this->idx;
98  auto subidx = this->subidx;
99  auto ec = this->ec;
100  T value = ::std::move(this->value);
101  ::std::function<Signature> con;
102  con.swap(this->con_);
103  delete this;
104  if (con) con(id, idx, subidx, ec, ::std::move(value));
105 }
106 
107 template <class T>
108 void
109 SdoUploadRequestWrapper<T>::OnRequest(void* data) noexcept {
110  static_cast<Sdo::Impl_*>(data)->OnUpload(*this);
111 }
112 
113 #ifndef DOXYGEN_SHOULD_SKIP_THIS
114 
115 // BOOLEAN
116 template class SdoDownloadRequestWrapper<bool>;
117 template class SdoUploadRequestWrapper<bool>;
118 
119 // INTEGER8
120 template class SdoDownloadRequestWrapper<int8_t>;
121 template class SdoUploadRequestWrapper<int8_t>;
122 
123 // INTEGER16
124 template class SdoDownloadRequestWrapper<int16_t>;
125 template class SdoUploadRequestWrapper<int16_t>;
126 
127 // INTEGER32
128 template class SdoDownloadRequestWrapper<int32_t>;
129 template class SdoUploadRequestWrapper<int32_t>;
130 
131 // UNSIGNED8
132 template class SdoDownloadRequestWrapper<uint8_t>;
133 template class SdoUploadRequestWrapper<uint8_t>;
134 
135 // UNSIGNED16
136 template class SdoDownloadRequestWrapper<uint16_t>;
137 template class SdoUploadRequestWrapper<uint16_t>;
138 
139 // UNSIGNED32
140 template class SdoDownloadRequestWrapper<uint32_t>;
141 template class SdoUploadRequestWrapper<uint32_t>;
142 
143 // REAL32
144 template class SdoDownloadRequestWrapper<float>;
145 template class SdoUploadRequestWrapper<float>;
146 
147 // VISIBLE_STRING
148 template class SdoDownloadRequestWrapper<::std::string>;
149 template class SdoUploadRequestWrapper<::std::string>;
150 
151 // OCTET_STRING
152 template class SdoDownloadRequestWrapper<::std::vector<uint8_t>>;
153 template class SdoUploadRequestWrapper<::std::vector<uint8_t>>;
154 
155 // UNICODE_STRING
156 template class SdoDownloadRequestWrapper<::std::basic_string<char16_t>>;
157 template class SdoUploadRequestWrapper<::std::basic_string<char16_t>>;
158 
159 // TIME_OF_DAY
160 // TIME_DIFFERENCE
161 // DOMAIN
162 // INTEGER24
163 
164 // REAL64
165 template class SdoDownloadRequestWrapper<double>;
166 template class SdoUploadRequestWrapper<double>;
167 
168 // INTEGER40
169 // INTEGER48
170 // INTEGER56
171 
172 // INTEGER64
173 template class SdoDownloadRequestWrapper<int64_t>;
174 template class SdoUploadRequestWrapper<int64_t>;
175 
176 // UNSIGNED24
177 // UNSIGNED40
178 // UNSIGNED48
179 // UNSIGNED56
180 
181 // UNSIGNED64
182 template class SdoDownloadRequestWrapper<uint64_t>;
183 template class SdoUploadRequestWrapper<uint64_t>;
184 
185 #endif // !DOXYGEN_SHOULD_SKIP_THIS
186 
187 } // namespace detail
188 
189 template <class T>
190 void
191 SdoDownloadRequest<T>::operator()() noexcept {
192  if (con_) con_(this->id, this->idx, this->subidx, this->ec);
193 }
194 
195 template <class T>
196 void
197 SdoDownloadRequest<T>::OnRequest(void* data) noexcept {
198  static_cast<Sdo::Impl_*>(data)->OnDownload(*this);
199 }
200 
201 template <class T>
202 void
203 SdoUploadRequest<T>::operator()() noexcept {
204  if (con_)
205  con_(this->id, this->idx, this->subidx, this->ec, ::std::move(this->value));
206 }
207 
208 template <class T>
209 void
210 SdoUploadRequest<T>::OnRequest(void* data) noexcept {
211  static_cast<Sdo::Impl_*>(data)->OnUpload(*this);
212 }
213 
214 #ifndef DOXYGEN_SHOULD_SKIP_THIS
215 
216 // BOOLEAN
217 template class SdoDownloadRequest<bool>;
218 template class SdoUploadRequest<bool>;
219 
220 // INTEGER8
221 template class SdoDownloadRequest<int8_t>;
222 template class SdoUploadRequest<int8_t>;
223 
224 // INTEGER16
225 template class SdoDownloadRequest<int16_t>;
226 template class SdoUploadRequest<int16_t>;
227 
228 // INTEGER32
229 template class SdoDownloadRequest<int32_t>;
230 template class SdoUploadRequest<int32_t>;
231 
232 // UNSIGNED8
233 template class SdoDownloadRequest<uint8_t>;
234 template class SdoUploadRequest<uint8_t>;
235 
236 // UNSIGNED16
237 template class SdoDownloadRequest<uint16_t>;
238 template class SdoUploadRequest<uint16_t>;
239 
240 // UNSIGNED32
241 template class SdoDownloadRequest<uint32_t>;
242 template class SdoUploadRequest<uint32_t>;
243 
244 // REAL32
245 template class SdoDownloadRequest<float>;
246 template class SdoUploadRequest<float>;
247 
248 // VISIBLE_STRING
249 template class SdoDownloadRequest<::std::string>;
250 template class SdoUploadRequest<::std::string>;
251 
252 // OCTET_STRING
253 template class SdoDownloadRequest<::std::vector<uint8_t>>;
254 template class SdoUploadRequest<::std::vector<uint8_t>>;
255 
256 // UNICODE_STRING
257 template class SdoDownloadRequest<::std::basic_string<char16_t>>;
258 template class SdoUploadRequest<::std::basic_string<char16_t>>;
259 
260 // TIME_OF_DAY
261 // TIME_DIFFERENCE
262 // DOMAIN
263 // INTEGER24
264 
265 // REAL64
266 template class SdoDownloadRequest<double>;
267 template class SdoUploadRequest<double>;
268 
269 // INTEGER40
270 // INTEGER48
271 // INTEGER56
272 
273 // INTEGER64
274 template class SdoDownloadRequest<int64_t>;
275 template class SdoUploadRequest<int64_t>;
276 
277 // UNSIGNED24
278 // UNSIGNED40
279 // UNSIGNED48
280 // UNSIGNED56
281 
282 // UNSIGNED64
283 template class SdoDownloadRequest<uint64_t>;
284 template class SdoUploadRequest<uint64_t>;
285 
286 #endif // !DOXYGEN_SHOULD_SKIP_THIS
287 
288 Sdo::Sdo() = default;
289 
290 Sdo::Sdo(CANNet* net, uint8_t id) : Sdo(net, nullptr, id) {}
291 
292 Sdo::Sdo(CANNet* net, CODev* dev, uint8_t num)
293  : impl_(new Impl_(net, dev, num)) {}
294 
295 Sdo::Sdo(COCSDO* sdo) : impl_(new Impl_(sdo, sdo->getTimeout())) {}
296 
297 Sdo& Sdo::operator=(Sdo&&) = default;
298 
299 Sdo::~Sdo() = default;
300 
301 void
303  impl_->Submit(req);
304 }
305 
306 bool
308  return impl_->Cancel(&req, ac) != 0;
309 }
310 
311 ::std::size_t
313  return impl_->Cancel(nullptr, ac);
314 }
315 
316 bool
318  return impl_->Abort(&req) != 0;
319 }
320 
321 ::std::size_t
323  return impl_->Abort(nullptr);
324 }
325 
326 Sdo::Impl_::Impl_(CANNet* net, CODev* dev, uint8_t num)
327  : sdo(make_shared_c<COCSDO>(net, dev, num)) {
328  sllist_init(&queue);
329 }
330 
331 Sdo::Impl_::Impl_(COCSDO* sdo_, int timeout)
332  : sdo(sdo_, [=](COCSDO* sdo) { sdo->setTimeout(timeout); }) {
333  sllist_init(&queue);
334 }
335 
336 Sdo::Impl_::~Impl_() { Cancel(nullptr, SdoErrc::NO_SDO); }
337 
338 void
339 Sdo::Impl_::Submit(detail::SdoRequestBase& req) {
340  assert(req.exec);
341  ev::Executor exec(req.exec);
342 
343  exec.on_task_init();
344  if (!sdo) {
345  req.id = 0;
346  req.ec = SdoErrc::NO_SDO;
347  exec.post(req);
348  exec.on_task_fini();
349  } else {
350  req.id = sdo->getPar().id;
351  bool first = sllist_empty(&queue);
352  sllist_push_back(&queue, &req._node);
353  if (first) req.OnRequest(this);
354  }
355 }
356 
357 ::std::size_t
358 Sdo::Impl_::Cancel(detail::SdoRequestBase* req, SdoErrc ac) {
359  sllist queue;
360  sllist_init(&queue);
361 
362  // Cancel all matching requests, except for the first (ongoing) request.
363  if (Pop(req, queue))
364  // Stop the ongoing request, if any.
365  sdo->abortReq(static_cast<uint32_t>(ac));
366 
367  ::std::size_t n = 0;
368  slnode* node;
369  while ((node = sllist_pop_front(&queue))) {
370  req = static_cast<detail::SdoRequestBase*>(ev_task_from_node(node));
371  req->ec = ac;
372 
373  auto exec = req->GetExecutor();
374  exec.post(*req);
375  exec.on_task_fini();
376 
377  n += n < ::std::numeric_limits<::std::size_t>::max();
378  }
379  return n;
380 }
381 
382 ::std::size_t
383 Sdo::Impl_::Abort(detail::SdoRequestBase* req) {
384  sllist queue;
385  sllist_init(&queue);
386 
387  // Abort all matching requests, except for the first (ongoing) request.
388  Pop(req, queue);
389 
390  return ev_task_queue_abort(&queue);
391 }
392 
393 bool
394 Sdo::Impl_::Pop(detail::SdoRequestBase* req, sllist& queue) {
395  if (!req) {
396  // Cancel all pending requests except for the first (ongoing) request.
397  auto node = sllist_pop_front(&this->queue);
398  sllist_append(&queue, &this->queue);
399  if (node) {
400  sllist_push_front(&this->queue, node);
401  req = static_cast<detail::SdoRequestBase*>(ev_task_from_node(node));
402  }
403  } else if (&req->_node != sllist_first(&this->queue)) {
404  if (sllist_remove(&queue, &req->_node))
405  sllist_push_back(&this->queue, &req->_node);
406  req = nullptr;
407  }
408  // Return true if the first request matched (but was not removed).
409  return req != nullptr;
410 }
411 
412 template <class T>
413 void
414 Sdo::Impl_::OnDownload(detail::SdoDownloadRequestBase<T>& req) noexcept {
415  constexpr auto N = co_type_traits_T<T>::index;
416  assert(&req._node == sllist_first(&queue));
417 
418  int errsv = get_errc();
419  set_errc(0);
420 
421  sdo->setTimeout(detail::to_sdo_timeout(req.timeout));
422  if (sdo->dnReq<N, Impl_, &Impl_::OnDnCon>(req.idx, req.subidx, req.value,
423  this) == -1) {
424  req.ec = util::make_error_code();
425  OnCompletion(req);
426  }
427 
428  set_errc(errsv);
429 }
430 
431 template <class T>
432 void
433 Sdo::Impl_::OnUpload(detail::SdoUploadRequestBase<T>& req) noexcept {
434  assert(&req._node == sllist_first(&queue));
435 
436  int errsv = get_errc();
437  set_errc(0);
438 
439  sdo->setTimeout(detail::to_sdo_timeout(req.timeout));
440  if (sdo->upReq<T, Impl_, &Impl_::OnUpCon<T>>(req.idx, req.subidx, this) ==
441  -1) {
442  req.ec = util::make_error_code();
443  OnCompletion(req);
444  }
445 
446  set_errc(errsv);
447 }
448 
449 void
450 Sdo::Impl_::OnDnCon(COCSDO*, uint16_t idx, uint8_t subidx,
451  uint32_t ac) noexcept {
452  auto task = ev_task_from_node(sllist_first(&queue));
453  assert(task);
454  auto req = static_cast<detail::SdoRequestBase*>(task);
455 
456  req->idx = idx;
457  req->subidx = subidx;
458  req->ec = SdoErrc(ac);
459 
460  OnCompletion(*req);
461 }
462 
463 template <class T>
464 void
465 Sdo::Impl_::OnUpCon(COCSDO*, uint16_t idx, uint8_t subidx, uint32_t ac,
466  T value) noexcept {
467  auto task = ev_task_from_node(sllist_first(&queue));
468  assert(task);
469  auto req = static_cast<detail::SdoUploadRequestBase<T>*>(task);
470 
471  req->idx = idx;
472  req->subidx = subidx;
473  req->ec = SdoErrc(ac);
474  req->value = ::std::move(value);
475 
476  OnCompletion(*req);
477 }
478 
479 void
480 Sdo::Impl_::OnCompletion(detail::SdoRequestBase& req) noexcept {
481  assert(&req._node == sllist_first(&queue));
482  sllist_pop_front(&queue);
483 
484  ev::Executor exec(req.exec);
485  exec.post(req);
486  exec.on_task_fini();
487 
488  auto task = ev_task_from_node(sllist_first(&queue));
489  if (task) static_cast<detail::SdoRequestBase*>(task)->OnRequest(this);
490 }
491 
492 } // namespace canopen
493 
494 } // namespace lely
inline ::std::shared_ptr< T > make_shared_c(Args &&... args)
Creates an instance of a trivial, standard layout or incomplete C type and wraps it in a std::shared_...
Definition: c_type.hpp:93
An opaque CAN network interface type.
Definition: net.hpp:85
An opaque CANopen Client-SDO service type.
Definition: csdo.hpp:156
An opaque CANopen device type.
Definition: dev.hpp:77
A Client-SDO queue.
Definition: sdo.hpp:407
Sdo()
Default-constructs an invalid Client-SDO queue.
bool Abort(detail::SdoRequestBase &req)
Aborts a pending SDO request.
Definition: sdo.cpp:317
void Submit(detail::SdoRequestBase &req)
Queues an LSS request.
Definition: sdo.cpp:302
bool Cancel(detail::SdoRequestBase &req, SdoErrc ac)
Cancels a pending SDO request.
Definition: sdo.cpp:307
~Sdo()
Destructs the Client-SDO queue.
::std::size_t AbortAll()
Aborts all pending SDO requests.
Definition: sdo.cpp:322
::std::size_t CancelAll(SdoErrc ac)
Cancels all pending SDO requests and stops the ongoing request, if any.
Definition: sdo.cpp:312
This is the internal header file of the C++ CANopen application library.
This header file is part of the CANopen library; it contains the C++ interface of the Client-SDO decl...
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
Definition: errnum.c:947
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:957
int to_sdo_timeout(const ::std::chrono::duration< Rep, Period > &d)
Converts a duration to an SDO timeout.
Definition: sdo.hpp:148
SdoErrc
The SDO abort codes.
Definition: sdo_error.hpp:42
@ NO_SDO
Resource not available: SDO connection.
::std::error_code make_error_code(SdoErrc e) noexcept
Creates an error code corresponding to an SDO abort code.
Definition: sdo_error.cpp:170
This header file is part of the C++ CANopen master library; it contains the Client-SDO queue declarat...
void sllist_init(struct sllist *list)
Initializes a singly-linked list.
Definition: sllist.h:184
struct sllist * sllist_append(struct sllist *dst, struct sllist *src)
Appends the singly-linked list at src to the one at dst.
Definition: sllist.h:233
void sllist_push_front(struct sllist *list, struct slnode *node)
Pushes a node to the front of a singly-linked list.
Definition: sllist.h:205
struct slnode * sllist_remove(struct sllist *list, struct slnode *node)
Removes a node from a singly-linked list.
Definition: sllist.c:46
void sllist_push_back(struct sllist *list, struct slnode *node)
Pushes a node to the back of a singly-linked list.
Definition: sllist.h:213
int sllist_empty(const struct sllist *list)
Returns 1 if the singly-linked list is empty, and 0 if not.
Definition: sllist.h:190
struct slnode * sllist_pop_front(struct sllist *list)
Pops a node from the front of a singly-linked list.
Definition: sllist.h:221
struct slnode * sllist_first(const struct sllist *list)
Returns a pointer to the first node in a singly-linked list.
Definition: sllist.h:244
The internal implementation of the Client-SDO queue.
Definition: sdo.cpp:42
A singly-linked list.
Definition: sllist.h:51
A node in a singly-linked list.
Definition: sllist.h:39
size_t ev_task_queue_abort(struct sllist *queue)
Aborts the tasks in queue by invoking ev_exec_on_task_fini() for each of them.
Definition: task.c:55
struct ev_task * ev_task_from_node(struct slnode *node)
Converts a pointer to a node in a queue to the address of the task containing the node.
Definition: task.c:32