sdo.cpp
Go to the documentation of this file.
2 
3 using namespace canopen;
4 
5 const uint8_t COMMAND_MASK = (1<<7) | (1<<6) | (1<<5);
6 const uint8_t INITIATE_DOWNLOAD_REQUEST = (0 << 5);
7 const uint8_t INITIATE_DOWNLOAD_RESPONSE = (1 << 5);
8 const uint8_t DOWNLOAD_SEGMENT_REQUEST = (1 << 5);
9 const uint8_t DOWNLOAD_SEGMENT_RESPONSE = (3 << 5);
10 const uint8_t INITIATE_UPLOAD_REQUEST = (2 << 5);
11 const uint8_t INITIATE_UPLOAD_RESPONSE = (2 << 5);
12 const uint8_t UPLOAD_SEGMENT_REQUEST = (3 << 5);
13 const uint8_t UPLOAD_SEGMENT_RESPONSE = (0 << 5);
14 const uint8_t ABORT_TRANSFER_REQUEST = (4 << 5);
15 
16 
17 #pragma pack(push) /* push current alignment to stack */
18 #pragma pack(1) /* set alignment to 1 byte boundary */
19 
20 struct SDOid{
21  uint32_t id:29;
22  uint32_t extended:1;
23  uint32_t dynamic:1;
24  uint32_t invalid:1;
25  SDOid(uint32_t val){
26  *(uint32_t*) this = val;
27  }
29  return can::Header(id, extended, false, false);
30  }
31 };
32 
34  uint8_t :5;
35  uint8_t command:3;
36  uint16_t index;
37  uint8_t sub_index;
38  uint8_t reserved[4];
39 };
40 
41 struct InitiateLong{
42  uint8_t size_indicated:1;
43  uint8_t expedited:1;
44  uint8_t num:2;
45  uint8_t :1;
46  uint8_t command:3;
47  uint16_t index;
48  uint8_t sub_index;
49  uint8_t payload[4];
50 
51  size_t data_size(){
52  if(expedited && size_indicated) return 4-num;
53  else if(!expedited && size_indicated) return payload[0] | (payload[3]<<8);
54  else return 0;
55  }
56  size_t apply_buffer(const String &buffer){
57  size_t size = buffer.size();
58  size_indicated = 1;
59  if(size > 4){
60  expedited = 0;
61  payload[0] = size & 0xFF;
62  payload[3] = (size >> 8) & 0xFF;
63  return 0;
64  }else{
65  expedited = 1;
66  size_indicated = 1;
67  num = 4-size;
68  memcpy(payload, buffer.data(), size);
69  return size;
70  }
71  }
72 };
73 
74 struct SegmentShort{
75  uint8_t :4;
76  uint8_t toggle:1;
77  uint8_t command:3;
78  uint8_t reserved[7];
79 };
80 
81 struct SegmentLong{
82  uint8_t done:1;
83  uint8_t num:3;
84  uint8_t toggle:1;
85  uint8_t command:3;
86  uint8_t payload[7];
87  size_t data_size(){
88  return 7-num;
89  }
90  size_t apply_buffer(const String &buffer, const size_t offset){
91  size_t size = buffer.size() - offset;
92  if(size > 7) size = 7;
93  else done = 1;
94  num = 7 - size;
95  memcpy(payload, buffer.data() + offset, size);
96  return offset + size;
97  }
98 };
99 
100 struct DownloadInitiateRequest: public FrameOverlay<InitiateLong>{
101  static const uint8_t command = 1;
102 
103  DownloadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry, const String &buffer, size_t &offset) : FrameOverlay(h) {
104  data.command = command;
105  data.index = entry.index;
106  data.sub_index = entry.sub_index;
107  offset = data.apply_buffer(buffer);
108  }
110 };
111 
112 struct DownloadInitiateResponse: public FrameOverlay<InitiateShort>{
113  static const uint8_t command = 3;
114 
116 
117  bool test(const can::Frame &msg, uint32_t &reason){
118  DownloadInitiateRequest req(msg);
119  if(req.data.command == DownloadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){
120  return true;
121  }
122  reason = 0x08000000; // General error
123  return false;
124  }
125 };
126 
127 struct DownloadSegmentRequest: public FrameOverlay<SegmentLong>{
128  static const uint8_t command = 0;
129 
131 
132  DownloadSegmentRequest(const Header &h, bool toggle, const String &buffer, size_t& offset) : FrameOverlay(h) {
133  data.command = command;
134  data.toggle = toggle?1:0;
135  offset = data.apply_buffer(buffer, offset);
136  }
137 };
138 
139 struct DownloadSegmentResponse : public FrameOverlay<SegmentShort>{
140  static const uint8_t command = 1;
142  }
143  bool test(const can::Frame &msg, uint32_t &reason){
144  DownloadSegmentRequest req(msg);
146  reason = 0x08000000; // General error
147  return false;
148  }else if( data.toggle != req.data.toggle){
149  reason = 0x05030000; // Toggle bit not alternated
150  return false;
151  }
152  return true;
153  }
154 };
155 
156 struct UploadInitiateRequest: public FrameOverlay<InitiateShort>{
157  static const uint8_t command = 2;
158  UploadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry) : FrameOverlay(h) {
159  data.command = command;
160  data.index = entry.index;
161  data.sub_index = entry.sub_index;
162  }
164 };
165 
166 struct UploadInitiateResponse: public FrameOverlay<InitiateLong>{
167  static const uint8_t command = 2;
169  bool test(const can::Frame &msg, size_t size, uint32_t &reason){
170  UploadInitiateRequest req(msg);
171  if(req.data.command == UploadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){
172  size_t ds = data.data_size();
173  if(ds == 0 || size == 0 || ds >= size) { // should be ==, but >= is needed for Elmo, it responses with more byte than requested
174  if(!data.expedited || (ds <= 4 && size <= 4)) return true;
175  }else{
176  reason = 0x06070010; // Data type does not match, length of service parameter does not match
177  return false;
178  }
179  }
180  reason = 0x08000000; // General error
181  return false;
182  }
183  bool read_data(String & buffer, size_t & offset, size_t & total){
184  if(data.size_indicated && total == 0){
185  total = data.data_size();
186  buffer.resize(total);
187  }
188  if(data.expedited){
189  memcpy(&buffer.front(), data.payload, buffer.size());
190  offset = buffer.size();
191  return true;
192  }
193  return false;
194  }
195 };
196 struct UploadSegmentRequest: public FrameOverlay<SegmentShort>{
197  static const uint8_t command = 3;
198  UploadSegmentRequest(const Header &h, bool toggle) : FrameOverlay(h) {
199  data.command = command;
200  data.toggle = toggle?1:0;
201  }
203 };
204 
205 struct UploadSegmentResponse : public FrameOverlay<SegmentLong>{
206  static const uint8_t command = 0;
208  }
209  bool test(const can::Frame &msg, uint32_t &reason){
210  UploadSegmentRequest req(msg);
212  reason = 0x08000000; // General error
213  return false;
214  }else if( data.toggle != req.data.toggle){
215  reason = 0x05030000; // Toggle bit not alternated
216  return false;
217  }
218  return true;
219  }
220  bool read_data(String & buffer, size_t & offset, const size_t & total){
221  uint32_t n = data.data_size();
222  if(total == 0){
223  buffer.resize(offset + n);
224  }
225  if(offset + n <= buffer.size()){
226  memcpy(&buffer[offset], data.payload, n);
227  offset += n;
228  return true;
229  }
230  return false;
231  }
232 };
233 
234 struct AbortData{
235  uint8_t :5;
236  uint8_t command:3;
237  uint16_t index;
238  uint8_t sub_index;
239  uint32_t reason;
240 
241  const char * text(){
242  switch(reason){
243  case 0x05030000: return "Toggle bit not alternated.";
244  case 0x05040000: return "SDO protocol timed out.";
245  case 0x05040001: return "Client/server command specifier not valid or unknown.";
246  case 0x05040002: return "Invalid block size (block mode only).";
247  case 0x05040003: return "Invalid sequence number (block mode only).";
248  case 0x05040004: return "CRC error (block mode only).";
249  case 0x05040005: return "Out of memory.";
250  case 0x06010000: return "Unsupported access to an object.";
251  case 0x06010001: return "Attempt to read a write only object.";
252  case 0x06010002: return "Attempt to write a read only object.";
253  case 0x06020000: return "Object does not exist in the object dictionary.";
254  case 0x06040041: return "Object cannot be mapped to the PDO.";
255  case 0x06040042: return "The number and length of the objects to be mapped would exceed PDO length.";
256  case 0x06040043: return "General parameter incompatibility reason.";
257  case 0x06040047: return "General internal incompatibility in the device.";
258  case 0x06060000: return "Access failed due to an hardware error.";
259  case 0x06070010: return "Data type does not match, length of service parameter does not match";
260  case 0x06070012: return "Data type does not match, length of service parameter too high";
261  case 0x06070013: return "Data type does not match, length of service parameter too low";
262  case 0x06090011: return "Sub-index does not exist.";
263  case 0x06090030: return "Invalid value for parameter (download only).";
264  case 0x06090031: return "Value of parameter written too high (download only).";
265  case 0x06090032: return "Value of parameter written too low (download only).";
266  case 0x06090036: return "Maximum value is less than minimum value.";
267  case 0x060A0023: return "Resource not available: SDO connection";
268  case 0x08000000: return "General error";
269  case 0x08000020: return "Data cannot be transferred or stored to the application.";
270  case 0x08000021: return "Data cannot be transferred or stored to the application because of local control.";
271  case 0x08000022: return "Data cannot be transferred or stored to the application because of the present device state.";
272  case 0x08000023: return "Object dictionary dynamic generation fails or no object dictionary is present (e.g.object dictionary is generated from file and generation fails because of an file error).";
273  case 0x08000024: return "No data available";
274  default: return "Abort code is reserved";
275  }
276  }
277 };
278 
279 struct AbortTranserRequest: public FrameOverlay<AbortData>{
280  static const uint8_t command = 4;
282  AbortTranserRequest(const Header &h, uint16_t index, uint8_t sub_index, uint32_t reason) : FrameOverlay(h) {
283  data.command = command;
284  data.index = index;
285  data.sub_index = sub_index;
286  data.reason = reason;
287  }
288 };
289 
290 #pragma pack(pop) /* pop previous alignment from stack */
291 
292 void SDOClient::abort(uint32_t reason){
293  if(current_entry){
294  interface_->send(last_msg = AbortTranserRequest(client_id, current_entry->index, current_entry->sub_index, reason));
295  }
296 }
297 
299  if(msg.dlc != 8) return false;
300 
301  uint32_t reason = 0;
302  switch(msg.data[0] >> 5){
304  {
305  DownloadInitiateResponse resp(msg);
306  if(resp.test(last_msg, reason) ){
307  if(offset < total){
308  interface_->send(last_msg = DownloadSegmentRequest(client_id, false, buffer, offset));
309  }else{
310  done = true;
311  }
312  }
313  break;
314  }
316  {
317  DownloadSegmentResponse resp(msg);
318  if( resp.test(last_msg, reason) ){
319  if(offset < total){
320  interface_->send(last_msg = DownloadSegmentRequest(client_id, !resp.data.toggle, buffer, offset));
321  }else{
322  done = true;
323  }
324  }
325  break;
326  }
327 
329  {
330  UploadInitiateResponse resp(msg);
331  if( resp.test(last_msg, total, reason) ){
332  if(resp.read_data(buffer, offset, total)){
333  done = true;
334  }else{
335  interface_->send(last_msg = UploadSegmentRequest(client_id, false));
336  }
337  }
338  break;
339  }
341  {
342  UploadSegmentResponse resp(msg);
343  if( resp.test(last_msg, reason) ){
344  if(resp.read_data(buffer, offset, total)){
345  if(resp.data.done || offset == total){
346  done = true;
347  }else{
348  interface_->send(last_msg = UploadSegmentRequest(client_id, !resp.data.toggle));
349  }
350  }else{
351  // abort, size mismatch
352  LOG("abort, size mismatch" << buffer.size() << " " << resp.data.data_size());
353  reason = 0x06070010; // Data type does not match, length of service parameter does not match
354  }
355  }
356  break;
357  }
359  LOG("abort" << std::hex << (uint32_t) AbortTranserRequest(msg).data.index << "#"<< std::dec << (uint32_t) AbortTranserRequest(msg).data.sub_index << ", reason: " << AbortTranserRequest(msg).data.text());
360  offset = 0;
361  return false;
362  break;
363  }
364  if(reason){
365  abort(reason);
366  offset = 0;
367  return false;
368  }
369  return true;
370 
371 }
372 
374  assert(storage_);
375  assert(interface_);
376  const canopen::ObjectDict & dict = *storage_->dict_;
377 
378  try{
379  client_id = SDOid(NodeIdOffset<uint32_t>::apply(dict(0x1200, 1).value(), storage_->node_id_)).header();
380  }
381  catch(...){
382  client_id = can::MsgHeader(0x600+ storage_->node_id_);
383  }
384 
385  last_msg = AbortTranserRequest(client_id, 0,0,0);
386  current_entry = 0;
387 
388  can::Header server_id;
389  try{
390  server_id = SDOid(NodeIdOffset<uint32_t>::apply(dict(0x1200, 2).value(), storage_->node_id_)).header();
391  }
392  catch(...){
393  server_id = can::MsgHeader(0x580+ storage_->node_id_);
394  }
395  reader_.listen(interface_, server_id);
396 }
397 
398 void SDOClient::transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result){
399  buffer = data;
400  offset = 0;
401  total = buffer.size();
402  current_entry = &entry;
403  done = false;
404 
405  can::BufferedReader::ScopedEnabler enabler(reader_);
406 
407  if(result){
408  interface_->send(last_msg = UploadInitiateRequest(client_id, entry));
409  }else{
410  interface_->send(last_msg = DownloadInitiateRequest(client_id, entry, buffer, offset));
411  }
412 
413  boost::this_thread::disable_interruption di;
414  can::Frame msg;
415 
416  while(!done){
417  if(!reader_.read(&msg,boost::chrono::seconds(1)))
418  {
419  abort(0x05040000); // SDO protocol timed out.
420  LOG("Did not receive a response message");
421  break;
422  }
423  if(!processFrame(msg)){
424  LOG("Could not process message");
425  break;
426  }
427  }
428  if(offset == 0 || offset != total){
429  THROW_WITH_KEY(TimeoutException("SDO"), ObjectDict::Key(*current_entry));
430  }
431 
432  if(result) *result=buffer;
433 
434 }
435 
437  boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2));
438  if(lock){
439  transmitAndWait(entry, data, &data);
440  }else{
441  THROW_WITH_KEY(TimeoutException("SDO read"), ObjectDict::Key(entry));
442  }
443 }
444 void SDOClient::write(const canopen::ObjectDict::Entry &entry, const String &data){
445  boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2));
446  if(lock){
447  transmitAndWait(entry, data, 0);
448  }else{
449  THROW_WITH_KEY(TimeoutException("SDO write"), ObjectDict::Key(entry));
450  }
451 }
size_t apply_buffer(const String &buffer)
Definition: sdo.cpp:56
void read(const canopen::ObjectDict::Entry &entry, String &data)
Definition: sdo.cpp:436
bool test(const can::Frame &msg, uint32_t &reason)
Definition: sdo.cpp:117
UploadInitiateResponse(const can::Frame &f)
Definition: sdo.cpp:168
uint32_t reason
Definition: sdo.cpp:239
boost::unordered_map< Key, EntryConstSharedPtr > dict_
Definition: objdict.h:228
UploadInitiateRequest(const can::Frame &f)
Definition: sdo.cpp:163
const uint8_t INITIATE_UPLOAD_RESPONSE
Definition: sdo.cpp:11
UploadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry)
Definition: sdo.cpp:158
const uint8_t DOWNLOAD_SEGMENT_REQUEST
Definition: sdo.cpp:8
bool read_data(String &buffer, size_t &offset, const size_t &total)
Definition: sdo.cpp:220
uint16_t index
Definition: sdo.cpp:237
uint16_t index
Definition: sdo.cpp:36
const uint8_t ABORT_TRANSFER_REQUEST
Definition: sdo.cpp:14
const uint8_t UPLOAD_SEGMENT_REQUEST
Definition: sdo.cpp:12
DownloadInitiateRequest(const can::Frame &f)
Definition: sdo.cpp:109
const uint8_t INITIATE_DOWNLOAD_REQUEST
Definition: sdo.cpp:6
boost::array< value_type, 8 > data
const uint8_t INITIATE_UPLOAD_REQUEST
Definition: sdo.cpp:10
uint8_t command
Definition: sdo.cpp:77
static const uint8_t command
Definition: sdo.cpp:101
size_t data_size()
Definition: sdo.cpp:87
static const uint8_t command
Definition: sdo.cpp:128
void transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result)
Definition: sdo.cpp:398
DownloadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry, const String &buffer, size_t &offset)
Definition: sdo.cpp:103
SDOid(uint32_t val)
Definition: sdo.cpp:25
DownloadSegmentResponse(const can::Frame &f)
Definition: sdo.cpp:141
static const uint8_t command
Definition: sdo.cpp:113
const uint8_t UPLOAD_SEGMENT_RESPONSE
Definition: sdo.cpp:13
#define LOG(log)
DownloadInitiateResponse(const can::Frame &f)
Definition: sdo.cpp:115
uint8_t sub_index
Definition: sdo.cpp:48
UploadSegmentRequest(const can::Frame &f)
Definition: sdo.cpp:202
bool test(const can::Frame &msg, uint32_t &reason)
Definition: sdo.cpp:143
uint8_t sub_index
Definition: sdo.cpp:238
static const uint8_t command
Definition: sdo.cpp:167
uint8_t toggle
Definition: sdo.cpp:76
UploadSegmentRequest(const Header &h, bool toggle)
Definition: sdo.cpp:198
uint8_t done
Definition: sdo.cpp:82
uint8_t command
Definition: sdo.cpp:35
UploadSegmentResponse(const can::Frame &f)
Definition: sdo.cpp:207
Definition: sdo.cpp:20
bool test(const can::Frame &msg, size_t size, uint32_t &reason)
Definition: sdo.cpp:169
uint8_t toggle
Definition: sdo.cpp:84
static const uint8_t command
Definition: sdo.cpp:197
const char * text()
Definition: sdo.cpp:241
bool processFrame(const can::Frame &msg)
Definition: sdo.cpp:298
DownloadSegmentRequest(const Header &h, bool toggle, const String &buffer, size_t &offset)
Definition: sdo.cpp:132
void write(const canopen::ObjectDict::Entry &entry, const String &data)
Definition: sdo.cpp:444
size_t data_size()
Definition: sdo.cpp:51
can::Header header()
Definition: sdo.cpp:28
static const uint8_t command
Definition: sdo.cpp:157
DownloadSegmentRequest(const can::Frame &f)
Definition: sdo.cpp:130
static const uint8_t command
Definition: sdo.cpp:280
AbortTranserRequest(const can::Frame &f)
Definition: sdo.cpp:281
AbortTranserRequest(const Header &h, uint16_t index, uint8_t sub_index, uint32_t reason)
Definition: sdo.cpp:282
static const uint8_t command
Definition: sdo.cpp:140
bool test(const can::Frame &msg, uint32_t &reason)
Definition: sdo.cpp:209
const uint8_t INITIATE_DOWNLOAD_RESPONSE
Definition: sdo.cpp:7
void abort(uint32_t reason)
Definition: sdo.cpp:292
bool read_data(String &buffer, size_t &offset, size_t &total)
Definition: sdo.cpp:183
unsigned char dlc
uint16_t index
Definition: sdo.cpp:47
#define THROW_WITH_KEY(e, k)
Definition: objdict.h:114
uint8_t command
Definition: sdo.cpp:46
uint8_t command
Definition: sdo.cpp:85
const uint8_t COMMAND_MASK
Definition: sdo.cpp:5
static const uint8_t command
Definition: sdo.cpp:206
uint8_t sub_index
Definition: sdo.cpp:37
size_t apply_buffer(const String &buffer, const size_t offset)
Definition: sdo.cpp:90
const uint8_t DOWNLOAD_SEGMENT_RESPONSE
Definition: sdo.cpp:9


canopen_master
Author(s): Mathias Lüdtke
autogenerated on Sat May 4 2019 02:40:43