sdo.cpp

Go to the documentation of this file.

 
#include "coapp.hpp"
 
#include <lely/coapp/detail/chrono.hpp>
#include <lely/coapp/sdo.hpp>
 
#include <cassert>
 
#include <lely/aio/queue.h>
#include <lely/co/csdo.hpp>
 
namespace lely {
 
namespace canopen {
 
namespace {
 
::std::error_code
ErrorCode(int errc, uint32_t ac) {
  if (errc)
    return ::std::error_code(errc, ::std::system_category());
  else if (ac)
    return static_cast<SdoErrc>(ac);
  else
    return {};
}
 
}  // namespace
 
using namespace aio;
 
struct Sdo::Impl_ {
  Impl_(CANNet* net, CODev* dev, uint8_t num);
  Impl_(COCSDO* sdo, int timeout);
  ~Impl_();
 
  void Submit(RequestBase_& req);
 
  ::std::size_t Cancel(RequestBase_* req, SdoErrc ac);
 
  template <class T, class U>
  void OnRequest(uint16_t idx, uint8_t subidx, U&& value);
 
  template <class T>
  void OnRequest(uint16_t idx, uint8_t subidx);
 
  void OnDnCon(COCSDO*, uint16_t idx, uint8_t subidx, uint32_t ac) noexcept;
 
  template <class T>
  void OnUpCon(COCSDO*, uint16_t idx, uint8_t subidx, uint32_t ac,
               T value) noexcept;
 
  void OnTaskFinished(RequestBase_& op) noexcept;
 
  aio_queue queue;
  ::std::shared_ptr<COCSDO> sdo;
};
 
template <class T>
void
Sdo::DownloadRequest<T>::OnRequest(Impl_* impl) noexcept {
  impl->OnRequest<T>(this->idx, this->subidx, this->value);
}
 
template <class T>
void
Sdo::DownloadRequest<T>::Func_(aio_task* task) noexcept {
  auto self = static_cast<Sdo::DownloadRequest<T>*>(task);
 
  if (self->con_)
    self->con_(self->idx, self->subidx, ErrorCode(self->errc, self->ac));
}
 
template <class T>
void
Sdo::UploadRequest<T>::OnRequest(Impl_* impl) noexcept {
  impl->OnRequest<T>(this->idx, this->subidx);
}
 
template <class T>
void
Sdo::UploadRequest<T>::Func_(aio_task* task) noexcept {
  auto self = static_cast<Sdo::UploadRequest<T>*>(task);
 
  if (self->con_)
    self->con_(self->idx, self->subidx, ErrorCode(self->errc, self->ac),
               ::std::move(self->value));
}
 
template <class T>
void
Sdo::DownloadRequestWrapper<T>::OnRequest(Impl_* impl) noexcept {
  impl->OnRequest<T>(this->idx, this->subidx, this->value);
}
 
template <class T>
void
Sdo::DownloadRequestWrapper<T>::Func_(aio_task* task) noexcept {
  auto self = static_cast<Sdo::DownloadRequestWrapper<T>*>(task);
 
  ::std::function<DownloadSignature> con = ::std::move(self->con_);
  auto idx = self->idx;
  auto subidx = self->subidx;
  auto ec = ErrorCode(self->errc, self->ac);
  delete self;
 
  if (con) con(idx, subidx, ec);
}
 
template <class T>
void
Sdo::UploadRequestWrapper<T>::OnRequest(Impl_* impl) noexcept {
  impl->OnRequest<T>(this->idx, this->subidx);
}
 
template <class T>
void
Sdo::UploadRequestWrapper<T>::Func_(aio_task* task) noexcept {
  auto self = static_cast<Sdo::UploadRequestWrapper<T>*>(task);
 
  ::std::function<UploadSignature<T>> con = ::std::move(self->con_);
  auto idx = self->idx;
  auto subidx = self->subidx;
  auto ec = ErrorCode(self->errc, self->ac);
  T value = ::std::move(self->value);
  delete self;
 
  if (con) con(idx, subidx, ec, ::std::move(value));
}
 
template <class T>
void
Sdo::AsyncDownloadRequest<T>::OnRequest(Impl_* impl) noexcept {
  impl->OnRequest<T>(this->idx, this->subidx, this->value);
}
 
template <class T>
void
Sdo::AsyncDownloadRequest<T>::Func_(aio_task* task) noexcept {
  auto self = static_cast<Sdo::AsyncDownloadRequest<T>*>(task);
 
  aio::Promise<::std::error_code> promise = ::std::move(self->promise_);
  auto ec = ErrorCode(self->errc, self->ac);
  delete self;
 
  promise.SetValue(ec);
}
 
template <class T>
void
Sdo::AsyncUploadRequest<T>::OnRequest(Impl_* impl) noexcept {
  impl->OnRequest<T>(this->idx, this->subidx);
}
 
template <class T>
void
Sdo::AsyncUploadRequest<T>::Func_(aio_task* task) noexcept {
  auto self = static_cast<Sdo::AsyncUploadRequest<T>*>(task);
 
  aio::Promise<::std::tuple<::std::error_code, T>> promise =
      ::std::move(self->promise_);
  auto value = ::std::make_tuple(ErrorCode(self->errc, self->ac),
                                 ::std::move(self->value));
  delete self;
 
  promise.SetValue(value);
}
 
#ifndef DOXYGEN_SHOULD_SKIP_THIS
 
// BOOLEAN
template class Sdo::DownloadRequest<bool>;
template class Sdo::UploadRequest<bool>;
template class Sdo::DownloadRequestWrapper<bool>;
template class Sdo::UploadRequestWrapper<bool>;
template class Sdo::AsyncDownloadRequest<bool>;
template class Sdo::AsyncUploadRequest<bool>;
 
// INTEGER8
template class Sdo::DownloadRequest<int8_t>;
template class Sdo::UploadRequest<int8_t>;
template class Sdo::DownloadRequestWrapper<int8_t>;
template class Sdo::UploadRequestWrapper<int8_t>;
template class Sdo::AsyncDownloadRequest<int8_t>;
template class Sdo::AsyncUploadRequest<int8_t>;
 
// INTEGER16
template class Sdo::DownloadRequest<int16_t>;
template class Sdo::UploadRequest<int16_t>;
template class Sdo::DownloadRequestWrapper<int16_t>;
template class Sdo::UploadRequestWrapper<int16_t>;
template class Sdo::AsyncDownloadRequest<int16_t>;
template class Sdo::AsyncUploadRequest<int16_t>;
 
// INTEGER32
template class Sdo::DownloadRequest<int32_t>;
template class Sdo::UploadRequest<int32_t>;
template class Sdo::DownloadRequestWrapper<int32_t>;
template class Sdo::UploadRequestWrapper<int32_t>;
template class Sdo::AsyncDownloadRequest<int32_t>;
template class Sdo::AsyncUploadRequest<int32_t>;
 
// UNSIGNED8
template class Sdo::DownloadRequest<uint8_t>;
template class Sdo::UploadRequest<uint8_t>;
template class Sdo::DownloadRequestWrapper<uint8_t>;
template class Sdo::UploadRequestWrapper<uint8_t>;
template class Sdo::AsyncDownloadRequest<uint8_t>;
template class Sdo::AsyncUploadRequest<uint8_t>;
 
// UNSIGNED16
template class Sdo::DownloadRequest<uint16_t>;
template class Sdo::UploadRequest<uint16_t>;
template class Sdo::DownloadRequestWrapper<uint16_t>;
template class Sdo::UploadRequestWrapper<uint16_t>;
template class Sdo::AsyncDownloadRequest<uint16_t>;
template class Sdo::AsyncUploadRequest<uint16_t>;
 
// UNSIGNED32
template class Sdo::DownloadRequest<uint32_t>;
template class Sdo::UploadRequest<uint32_t>;
template class Sdo::DownloadRequestWrapper<uint32_t>;
template class Sdo::UploadRequestWrapper<uint32_t>;
template class Sdo::AsyncDownloadRequest<uint32_t>;
template class Sdo::AsyncUploadRequest<uint32_t>;
 
// REAL32
template class Sdo::DownloadRequest<float>;
template class Sdo::UploadRequest<float>;
template class Sdo::DownloadRequestWrapper<float>;
template class Sdo::UploadRequestWrapper<float>;
template class Sdo::AsyncDownloadRequest<float>;
template class Sdo::AsyncUploadRequest<float>;
 
// VISIBLE_STRING
template class Sdo::DownloadRequest<::std::string>;
template class Sdo::UploadRequest<::std::string>;
template class Sdo::DownloadRequestWrapper<::std::string>;
template class Sdo::UploadRequestWrapper<::std::string>;
template class Sdo::AsyncDownloadRequest<::std::string>;
template class Sdo::AsyncUploadRequest<::std::string>;
 
// OCTET_STRING
template class Sdo::DownloadRequest<::std::vector<uint8_t>>;
template class Sdo::UploadRequest<::std::vector<uint8_t>>;
template class Sdo::DownloadRequestWrapper<::std::vector<uint8_t>>;
template class Sdo::UploadRequestWrapper<::std::vector<uint8_t>>;
template class Sdo::AsyncDownloadRequest<::std::vector<uint8_t>>;
template class Sdo::AsyncUploadRequest<::std::vector<uint8_t>>;
 
// UNICODE_STRING
template class Sdo::DownloadRequest<::std::basic_string<char16_t>>;
template class Sdo::UploadRequest<::std::basic_string<char16_t>>;
template class Sdo::DownloadRequestWrapper<::std::basic_string<char16_t>>;
template class Sdo::UploadRequestWrapper<::std::basic_string<char16_t>>;
template class Sdo::AsyncDownloadRequest<::std::basic_string<char16_t>>;
template class Sdo::AsyncUploadRequest<::std::basic_string<char16_t>>;
 
// TIME_OF_DAY
// TIME_DIFFERENCE
// DOMAIN
// INTEGER24
 
// REAL64
template class Sdo::DownloadRequest<double>;
template class Sdo::UploadRequest<double>;
template class Sdo::DownloadRequestWrapper<double>;
template class Sdo::UploadRequestWrapper<double>;
template class Sdo::AsyncDownloadRequest<double>;
template class Sdo::AsyncUploadRequest<double>;
 
// INTEGER40
// INTEGER48
// INTEGER56
 
// INTEGER64
template class Sdo::DownloadRequest<int64_t>;
template class Sdo::UploadRequest<int64_t>;
template class Sdo::DownloadRequestWrapper<int64_t>;
template class Sdo::UploadRequestWrapper<int64_t>;
template class Sdo::AsyncDownloadRequest<int64_t>;
template class Sdo::AsyncUploadRequest<int64_t>;
 
// UNSIGNED24
// UNSIGNED40
// UNSIGNED48
// UNSIGNED56
 
// UNSIGNED64
template class Sdo::DownloadRequest<uint64_t>;
template class Sdo::UploadRequest<uint64_t>;
template class Sdo::DownloadRequestWrapper<uint64_t>;
template class Sdo::UploadRequestWrapper<uint64_t>;
template class Sdo::AsyncDownloadRequest<uint64_t>;
template class Sdo::AsyncUploadRequest<uint64_t>;
 
#endif  // !DOXYGEN_SHOULD_SKIP_THIS
 
Sdo::Sdo() = default;
 
Sdo::Sdo(CANNet* net, uint8_t id) : Sdo(net, nullptr, id) {}
 
Sdo::Sdo(CANNet* net, CODev* dev, uint8_t num)
    : impl_(new Impl_(net, dev, num)) {}
 
Sdo::Sdo(COCSDO* sdo) : impl_(new Impl_(sdo, sdo->getTimeout())) {}
 
Sdo& Sdo::operator=(Sdo&&) = default;
 
Sdo::~Sdo() = default;
 
void
Sdo::Submit(RequestBase_& op) {
  impl_->Submit(op);
}
 
::std::size_t
Sdo::Cancel(RequestBase_& op, SdoErrc ac) {
  return impl_->Cancel(&op, ac);
}
 
::std::size_t
Sdo::Cancel(SdoErrc ac) {
  return impl_->Cancel(nullptr, ac);
}
 
Sdo::Impl_::Impl_(CANNet* net, CODev* dev, uint8_t num)
    : sdo(make_shared_c<COCSDO>(net, dev, num)) {
  aio_queue_init(&queue);
}
 
Sdo::Impl_::Impl_(COCSDO* sdo_, int timeout)
    : sdo(sdo_, [=](COCSDO* sdo) { sdo->setTimeout(timeout); }) {
  aio_queue_init(&queue);
}
 
Sdo::Impl_::~Impl_() { Cancel(nullptr, SdoErrc::NO_SDO); }
 
void
Sdo::Impl_::Submit(RequestBase_& req) {
  assert(req.exec);
  req.GetExecutor().OnTaskStarted();
  req.errc = errnum2c(ERRNUM_INPROGRESS);
 
  if (!sdo) {
    req.errc = 0;
    req.ac = static_cast<uint32_t>(SdoErrc::NO_SDO);
    OnTaskFinished(req);
  } else {
    bool first = aio_queue_empty(&queue);
    aio_queue_push(&queue, &req);
    if (first) req.OnRequest(this);
  }
}
 
::std::size_t
Sdo::Impl_::Cancel(RequestBase_* req, SdoErrc ac) {
  aio_queue queue_;
  aio_queue_init(&queue_);
 
  if (!req || req != aio_queue_front(&queue)) {
    auto task = aio_queue_pop(&queue);
    aio_queue_move(&queue_, &queue, req);
    if (task) aio_queue_push(&queue, task);
    req = task && !req ? static_cast<RequestBase_*>(task) : nullptr;
  }
 
  if (req) {
    assert(req == aio_queue_front(&queue));
    sdo->abortReq(static_cast<uint32_t>(ac));
  }
 
  auto n = aio_queue_cancel(&queue_, errnum2c(ERRNUM_CANCELED));
  if (req) n += n < ::std::numeric_limits<decltype(n)>::max();
  return n;
}
 
template <class T, class U>
void
Sdo::Impl_::OnRequest(uint16_t idx, uint8_t subidx, U&& value) {
  constexpr auto N = co_type_traits_T<T>::index;
 
  auto task = aio_queue_front(&queue);
  assert(task);
  auto& req = *static_cast<Sdo::RequestBase_*>(task);
 
  sdo->setTimeout(detail::ToTimeout(req.timeout));
  if (sdo->dnReq<N, Impl_, &Impl_::OnDnCon>(
          idx, subidx, ::std::forward<U>(value), this) == -1) {
    req.errc = get_errc();
    OnTaskFinished(req);
  }
}
 
template <class T>
void
Sdo::Impl_::OnRequest(uint16_t idx, uint8_t subidx) {
  auto task = aio_queue_front(&queue);
  assert(task);
  auto& req = *static_cast<Sdo::RequestBase_*>(task);
 
  sdo->setTimeout(detail::ToTimeout(req.timeout));
  if (sdo->upReq<T, Impl_, &Impl_::OnUpCon<T>>(idx, subidx, this) == -1) {
    req.errc = get_errc();
    OnTaskFinished(req);
  }
}
 
void
Sdo::Impl_::OnDnCon(COCSDO*, uint16_t idx, uint8_t subidx,
                    uint32_t ac) noexcept {
  auto task = aio_queue_pop(&queue);
  assert(task);
  auto& req = *static_cast<Sdo::RequestBase_*>(task);
 
  req.errc = 0;
  req.idx = idx;
  req.subidx = subidx;
  req.ac = ac;
 
  OnTaskFinished(req);
}
 
template <class T>
void
Sdo::Impl_::OnUpCon(COCSDO*, uint16_t idx, uint8_t subidx, uint32_t ac,
                    T value) noexcept {
  auto task = aio_queue_pop(&queue);
  assert(task);
  auto& req = *static_cast<Sdo::UploadRequestBase_<T>*>(task);
 
  req.errc = 0;
  req.idx = idx;
  req.subidx = subidx;
  req.ac = ac;
  req.value = ::std::move(value);
 
  OnTaskFinished(req);
}
 
void
Sdo::Impl_::OnTaskFinished(RequestBase_& req) noexcept {
  auto exec = req.GetExecutor();
  exec.Post(req);
  exec.OnTaskFinished();
 
  auto task = aio_queue_front(&queue);
  if (task) static_cast<Sdo::RequestBase_*>(task)->OnRequest(this);
}
 
}  // namespace canopen
 
}  // namespace lely