Lely core libraries  2.2.5
sdo.c
Go to the documentation of this file.
1 
24 #include "co.h"
25 #define LELY_CO_SDO_INLINE extern inline
26 #include <lely/util/diag.h>
27 #ifndef LELY_NO_CO_OBJ_FILE
28 #include <lely/util/frbuf.h>
29 #include <lely/util/fwbuf.h>
30 #endif
31 #include <lely/co/sdo.h>
32 #include <lely/co/val.h>
33 
34 #include <assert.h>
35 
50 static int co_sdo_req_dn_buf(
51  struct co_sdo_req *req, const void **pptr, size_t *pnbyte);
52 
54 static void co_sdo_req_up_buf(struct co_sdo_req *req);
55 
56 const char *
57 co_sdo_ac2str(co_unsigned32_t ac)
58 {
59  switch (ac) {
60  case 0: return "Success";
61  case CO_SDO_AC_TOGGLE: return "Toggle bit not altered";
62  case CO_SDO_AC_TIMEOUT: return "SDO protocol timed out";
63  case CO_SDO_AC_NO_CS:
64  return "Client/server command specifier not valid or unknown";
65  case CO_SDO_AC_BLK_SIZE: return "Invalid block size";
66  case CO_SDO_AC_BLK_SEQ: return "Invalid sequence number";
67  case CO_SDO_AC_BLK_CRC: return "CRC error";
68  case CO_SDO_AC_NO_MEM: return "Out of memory";
69  case CO_SDO_AC_NO_ACCESS: return "Unsupported access to an object";
70  case CO_SDO_AC_NO_READ: return "Attempt to read a write only object";
71  case CO_SDO_AC_NO_WRITE: return "Attempt to write a read only object";
72  case CO_SDO_AC_NO_OBJ:
73  return "Object does not exist in the object dictionary";
74  case CO_SDO_AC_NO_PDO: return "Object cannot be mapped to the PDO";
75  case CO_SDO_AC_PDO_LEN:
76  return "The number and length of the objects to be mapped would exceed the PDO length";
77  case CO_SDO_AC_PARAM: return "General parameter incompatibility reason";
78  case CO_SDO_AC_COMPAT:
79  return "General internal incompatibility in the device";
80  case CO_SDO_AC_HARDWARE: return "Access failed due to a hardware error";
81  case CO_SDO_AC_TYPE_LEN:
82  return "Data type does not match, length of service parameter does not match";
84  return "Data type does not match, length of service parameter too high";
86  return "Data type does not match, length of service parameter too low";
87  case CO_SDO_AC_NO_SUB: return "Sub-index does not exist";
88  case CO_SDO_AC_PARAM_VAL: return "Invalid value for parameter";
89  case CO_SDO_AC_PARAM_HI: return "Value of parameter written too high";
90  case CO_SDO_AC_PARAM_LO: return "Value of parameter written too low";
92  return "Maximum value is less than minimum value";
93  case CO_SDO_AC_NO_SDO: return "Resource not available: SDO connection";
94  case CO_SDO_AC_ERROR: return "General error";
95  case CO_SDO_AC_DATA:
96  return "Data cannot be transferred or stored to the application";
97  case CO_SDO_AC_DATA_CTL:
98  return "Data cannot be transferred or stored to the application because of local control";
99  case CO_SDO_AC_DATA_DEV:
100  return "Data cannot be transferred or stored to the application because of the present device state";
101  case CO_SDO_AC_NO_OD:
102  return "Object dictionary dynamic generation fails or no object dictionary is present";
103  case CO_SDO_AC_NO_DATA: return "No data available";
104  default: return "Unknown abort code";
105  }
106 }
107 
108 void
110 {
111  assert(req);
112 
113  req->size = 0;
114  req->buf = NULL;
115  req->nbyte = 0;
116  req->offset = 0;
117  membuf_init(&req->membuf);
118 }
119 
120 void
122 {
123  assert(req);
124 
125  membuf_fini(&req->membuf);
126 }
127 
128 void
130 {
131  req->size = 0;
132  req->buf = NULL;
133  req->nbyte = 0;
134  req->offset = 0;
135  membuf_clear(&req->membuf);
136 }
137 
138 int
139 co_sdo_req_dn(struct co_sdo_req *req, const void **pptr, size_t *pnbyte,
140  co_unsigned32_t *pac)
141 {
142  co_unsigned32_t ac = 0;
143 
144  int errc = get_errc();
145  switch (co_sdo_req_dn_buf(req, pptr, pnbyte)) {
146  default:
147  // Convert the error number to an SDO abort code.
148  // clang-format off
149  ac = get_errnum() == ERRNUM_NOMEM
151  // clang-format on
152  set_errc(errc);
153  // ...falls through ...
154  case 0:
155  // Return without an abort code if not all data is present. This
156  // is not an error.
157  if (pac)
158  *pac = ac;
159  return -1;
160  case 1: return 0;
161  }
162 }
163 
164 int
165 co_sdo_req_dn_val(struct co_sdo_req *req, co_unsigned16_t type, void *val,
166  co_unsigned32_t *pac)
167 {
168  co_unsigned32_t ac = 0;
169 
170  const void *ptr = NULL;
171  size_t nbyte = 0;
172  if (co_sdo_req_dn(req, &ptr, &nbyte, pac) == -1)
173  return -1;
174 
175  // Read the value.
176  co_val_init(type, val);
177  size_t size = co_val_read(
178  type, val, ptr, (const uint_least8_t *)ptr + nbyte);
179 
180  // Check the size of the value.
181  if (co_type_is_array(type)) {
182  if (size != nbyte) {
183  ac = CO_SDO_AC_NO_MEM;
184  goto error_read;
185  }
186  } else {
187  if (!size) {
189  goto error_read;
190  } else if (size < nbyte) {
192  goto error_read;
193  }
194  }
195 
196  return 0;
197 
198 error_read:
199  co_val_fini(type, val);
200  if (pac)
201  *pac = ac;
202  return -1;
203 }
204 
205 #ifndef LELY_NO_CO_OBJ_FILE
206 int
207 co_sdo_req_dn_file(struct co_sdo_req *req, const char *filename,
208  co_unsigned32_t *pac)
209 {
210  int errc = get_errc();
211  co_unsigned32_t ac = 0;
212 
213  const void *ptr = NULL;
214  size_t nbyte = 0;
215  if (co_sdo_req_dn(req, &ptr, &nbyte, pac) == -1)
216  return -1;
217 
218  fwbuf_t *fbuf = fwbuf_create(filename);
219  if (!fbuf) {
220  diag(DIAG_ERROR, get_errc(), "%s", filename);
221  ac = CO_SDO_AC_DATA;
222  goto error_create_fbuf;
223  }
224 
225  if (fwbuf_write(fbuf, ptr, nbyte) != (ssize_t)nbyte) {
226  diag(DIAG_ERROR, get_errc(), "%s", filename);
227  ac = CO_SDO_AC_DATA;
228  goto error_write;
229  }
230 
231  if (fwbuf_commit(fbuf) == -1) {
232  diag(DIAG_ERROR, get_errc(), "%s", filename);
233  ac = CO_SDO_AC_DATA;
234  goto error_commit;
235  }
236 
237  fwbuf_destroy(fbuf);
238 
239  return 0;
240 
241 error_commit:
242 error_write:
243  fwbuf_destroy(fbuf);
244 error_create_fbuf:
245  if (pac)
246  *pac = ac;
247  set_errc(errc);
248  return -1;
249 }
250 #endif // !LELY_NO_CO_OBJ_FILE
251 
252 int
253 co_sdo_req_up(struct co_sdo_req *req, const void *ptr, size_t n,
254  co_unsigned32_t *pac)
255 {
256  assert(req);
257  struct membuf *buf = &req->membuf;
258 
259  co_unsigned32_t ac = 0;
260 
261  membuf_clear(buf);
262  if (n && !membuf_reserve(buf, n)) {
263  ac = CO_SDO_AC_NO_MEM;
264  goto error_reserve;
265  }
266 
267  if (ptr)
268  membuf_write(buf, ptr, n);
269 
270  co_sdo_req_up_buf(req);
271  return 0;
272 
273 error_reserve:
274  if (pac)
275  *pac = ac;
276  return -1;
277 }
278 
279 int
280 co_sdo_req_up_val(struct co_sdo_req *req, co_unsigned16_t type, const void *val,
281  co_unsigned32_t *pac)
282 {
283  assert(req);
284  struct membuf *buf = &req->membuf;
285 
286  co_unsigned32_t ac = 0;
287 
288  size_t size = co_val_write(type, val, NULL, NULL);
289 
290  membuf_clear(buf);
291  if (size && !membuf_reserve(buf, size)) {
292  ac = CO_SDO_AC_NO_MEM;
293  goto error_reserve;
294  }
295 
296  uint_least8_t *begin = membuf_alloc(buf, &size);
297  if (co_val_write(type, val, begin, begin + size) != size) {
298  ac = CO_SDO_AC_ERROR;
299  goto error_write;
300  }
301 
302  co_sdo_req_up_buf(req);
303  return 0;
304 
305 error_write:
306 error_reserve:
307  if (pac)
308  *pac = ac;
309  return -1;
310 }
311 
312 #ifndef LELY_NO_CO_OBJ_FILE
313 int
314 co_sdo_req_up_file(struct co_sdo_req *req, const char *filename,
315  co_unsigned32_t *pac)
316 {
317  assert(req);
318  struct membuf *buf = &req->membuf;
319 
320  int errc = get_errc();
321  co_unsigned32_t ac = 0;
322 
323  frbuf_t *fbuf = frbuf_create(filename);
324  if (!fbuf) {
325  diag(DIAG_ERROR, get_errc(), "%s", filename);
326  ac = CO_SDO_AC_DATA;
327  goto error_create_fbuf;
328  }
329 
330  intmax_t size = frbuf_get_size(fbuf);
331  if (size == -1) {
332  diag(DIAG_ERROR, get_errc(), "%s", filename);
333  ac = CO_SDO_AC_DATA;
334  goto error_get_size;
335  }
336  size_t nbyte = (size_t)size;
337 
338  membuf_clear(buf);
339  if (size && !membuf_reserve(buf, nbyte)) {
340  ac = CO_SDO_AC_NO_MEM;
341  goto error_reserve;
342  }
343 
344  void *ptr = membuf_alloc(buf, &nbyte);
345  if (frbuf_read(fbuf, ptr, nbyte) != (ssize_t)nbyte) {
346  diag(DIAG_ERROR, get_errc(), "%s", filename);
347  ac = CO_SDO_AC_DATA;
348  goto error_read;
349  }
350 
351  frbuf_destroy(fbuf);
352 
353  co_sdo_req_up_buf(req);
354  return 0;
355 
356 error_read:
357 error_reserve:
358 error_get_size:
359  frbuf_destroy(fbuf);
360 error_create_fbuf:
361  if (pac)
362  *pac = ac;
363  set_errc(errc);
364  return -1;
365 }
366 #endif // !LELY_NO_CO_OBJ_FILE
367 
368 static int
369 co_sdo_req_dn_buf(struct co_sdo_req *req, const void **pptr, size_t *pnbyte)
370 {
371  assert(req);
372  struct membuf *buf = &req->membuf;
373 
374  // In case of an error, keep track of the offset with respect to the
375  // position indicator of the buffer.
376  ptrdiff_t offset = -(ptrdiff_t)membuf_size(buf);
377 
378  const void *ptr;
379  if (co_sdo_req_first(req) && co_sdo_req_last(req)) {
380  // If the entire value is available right away, skip copying the
381  // data to the buffer.
382  ptr = req->buf;
383  } else {
384  if (co_sdo_req_first(req)) {
385  membuf_clear(buf);
386  if (req->size && !membuf_reserve(buf, req->size))
387  goto error_reserve;
388  } else {
389  // Adjust the offset if necessary. Only backtracking is
390  // allowed.
391  offset += req->offset;
392  if (offset) {
393  if (offset > 0) {
395  goto error_offset;
396  }
397  membuf_seek(buf, offset);
398  }
399  }
400 
401  if (req->nbyte) {
402  if (req->nbyte > membuf_capacity(buf)) {
404  goto error_nbyte;
405  }
406  membuf_write(buf, req->buf, req->nbyte);
407  }
408 
409  if (!co_sdo_req_last(req))
410  return 0;
411 
412  ptr = membuf_begin(buf);
413  }
414 
415  if (pptr)
416  *pptr = ptr;
417  if (pnbyte)
418  *pnbyte = req->size;
419 
420  return 1;
421 
422 error_nbyte:
423 error_reserve:
424  // Restore the position indicator of the buffer.
425  membuf_seek(buf, -offset);
426 error_offset:
427  return -1;
428 }
429 
430 static void
432 {
433  assert(req);
434  struct membuf *buf = &req->membuf;
435 
436  req->size = membuf_size(buf);
437  req->buf = membuf_begin(buf);
438  req->nbyte = req->size;
439  req->offset = 0;
440 }
This header file is part of the utilities library; it contains the diagnostic declarations.
@ DIAG_ERROR
An error.
Definition: diag.h:49
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
Definition: diag.c:156
@ ERRNUM_NOMEM
Not enough space.
Definition: errnum.h:169
@ ERRNUM_INVAL
Invalid argument.
Definition: errnum.h:129
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
errnum_t get_errnum(void)
Returns the last (thread-specific) platform-independent error number set by a system call or library ...
Definition: errnum.h:369
void set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
Definition: errnum.h:375
This header file is part of the utilities library; it contains the read file buffer declarations.
intmax_t frbuf_get_size(frbuf_t *buf)
Returns the size (in bytes) of the a read file buffer, or -1 on error.
Definition: frbuf.c:167
frbuf_t * frbuf_create(const char *filename)
Creates a new read file buffer.
Definition: frbuf.c:133
void frbuf_destroy(frbuf_t *buf)
Destroys a read file buffer.
Definition: frbuf.c:158
ssize_t frbuf_read(frbuf_t *buf, void *ptr, size_t size)
Reads bytes from the current position in a read file buffer.
Definition: frbuf.c:270
This header file is part of the utilities library; it contains the (atomic) write file buffer declara...
ssize_t fwbuf_write(fwbuf_t *buf, const void *ptr, size_t size)
Writes bytes to the current position in a write file buffer.
Definition: fwbuf.c:486
void fwbuf_destroy(fwbuf_t *buf)
Destroys a write file buffer.
Definition: fwbuf.c:327
int fwbuf_commit(fwbuf_t *buf)
Commits all changes to a write file buffer to disk if all previous file operations were successful,...
Definition: fwbuf.c:975
fwbuf_t * fwbuf_create(const char *filename)
Creates a new (atomic) write file buffer.
Definition: fwbuf.c:302
This header file is part of the CANopen library; it contains the Service Data Object (SDO) declaratio...
int co_sdo_req_first(const struct co_sdo_req *req)
Returns 1 if the specified request includes the first segment, and 0 otherwise.
Definition: sdo.h:340
#define CO_SDO_AC_TYPE_LEN_LO
SDO abort code: Data type does not match, length of service parameter too low.
Definition: sdo.h:129
#define CO_SDO_AC_PARAM_LO
SDO abort code: Value of parameter written too low (download only).
Definition: sdo.h:141
#define CO_SDO_AC_NO_READ
SDO abort code: Attempt to read a write only object.
Definition: sdo.h:87
#define CO_SDO_AC_NO_OD
SDO abort code: Object dictionary dynamic generation fails or no object dictionary is present (e....
Definition: sdo.h:172
#define CO_SDO_AC_TOGGLE
SDO abort code: Toggle bit not altered.
Definition: sdo.h:63
#define CO_SDO_AC_TYPE_LEN_HI
SDO abort code: Data type does not match, length of service parameter too high.
Definition: sdo.h:123
#define CO_SDO_AC_NO_CS
SDO abort code: Client/server command specifier not valid or unknown.
Definition: sdo.h:69
#define CO_SDO_AC_TYPE_LEN
SDO abort code: Data type does not match, length of service parameter does not match.
Definition: sdo.h:117
#define CO_SDO_AC_BLK_SEQ
SDO abort code: Invalid sequence number (block mode only).
Definition: sdo.h:75
#define CO_SDO_AC_NO_ACCESS
SDO abort code: Unsupported access to an object.
Definition: sdo.h:84
#define CO_SDO_AC_ERROR
SDO abort code: General error.
Definition: sdo.h:150
#define CO_SDO_AC_PARAM
SDO abort code: General parameter incompatibility reason.
Definition: sdo.h:105
#define CO_SDO_AC_DATA
SDO abort code: Data cannot be transferred or stored to the application.
Definition: sdo.h:153
#define CO_SDO_AC_HARDWARE
SDO abort code: Access failed due to a hardware error.
Definition: sdo.h:111
#define CO_SDO_AC_NO_SDO
SDO abort code: Resource not available: SDO connection.
Definition: sdo.h:147
#define CO_SDO_AC_DATA_DEV
SDO abort code: Data cannot be transferred or stored to the application because of the present device...
Definition: sdo.h:165
#define CO_SDO_AC_PARAM_HI
SDO abort code: Value of parameter written too high (download only).
Definition: sdo.h:138
#define CO_SDO_AC_NO_OBJ
SDO abort code: Object does not exist in the object dictionary.
Definition: sdo.h:93
#define CO_SDO_AC_NO_DATA
SDO abort code: No data available.
Definition: sdo.h:175
#define CO_SDO_AC_NO_SUB
SDO abort code: Sub-index does not exist.
Definition: sdo.h:132
#define CO_SDO_AC_BLK_SIZE
SDO abort code: Invalid block size (block mode only).
Definition: sdo.h:72
#define CO_SDO_AC_DATA_CTL
SDO abort code: Data cannot be transferred or stored to the application because of local control.
Definition: sdo.h:159
int co_sdo_req_last(const struct co_sdo_req *req)
Returns 1 if the specified request includes the last segment, and 0 otherwise.
Definition: sdo.h:346
#define CO_SDO_AC_PARAM_VAL
SDO abort code: Invalid value for parameter (download only).
Definition: sdo.h:135
#define CO_SDO_AC_PDO_LEN
SDO abort code: The number and length of the objects to be mapped would exceed the PDO length.
Definition: sdo.h:102
#define CO_SDO_AC_NO_PDO
SDO abort code: Object cannot be mapped to the PDO.
Definition: sdo.h:96
#define CO_SDO_AC_TIMEOUT
SDO abort code: SDO protocol timed out.
Definition: sdo.h:66
#define CO_SDO_AC_BLK_CRC
SDO abort code: CRC error (block mode only).
Definition: sdo.h:78
#define CO_SDO_AC_COMPAT
SDO abort code: General internal incompatibility in the device.
Definition: sdo.h:108
#define CO_SDO_AC_NO_MEM
SDO abort code: Out of memory.
Definition: sdo.h:81
#define CO_SDO_AC_NO_WRITE
SDO abort code: Attempt to write a read only object.
Definition: sdo.h:90
#define CO_SDO_AC_PARAM_RANGE
SDO abort code: Maximum value is less than minimum value (download only).
Definition: sdo.h:144
ptrdiff_t membuf_seek(struct membuf *buf, ptrdiff_t offset)
Adjusts the position indicator of a memory buffer by offset bytes.
Definition: membuf.h:168
void * membuf_begin(const struct membuf *buf)
Returns a pointer to the first byte in a memory buffer.
Definition: membuf.h:144
size_t membuf_reserve(struct membuf *buf, size_t size)
Resizes a memory buffer, if necessary, to make room for at least an additional size bytes.
Definition: membuf.c:46
void membuf_fini(struct membuf *buf)
Finalizes a memory buffer.
Definition: membuf.c:38
size_t membuf_capacity(const struct membuf *buf)
Returns the number of unused bytes remaining in a memory buffer.
Definition: membuf.h:162
size_t membuf_write(struct membuf *buf, const void *ptr, size_t size)
Writes data to a memory buffer.
Definition: membuf.h:192
void * membuf_alloc(struct membuf *buf, size_t *size)
Creates region of *size bytes in a memory buffer, starting at the current position indicator given by...
Definition: membuf.h:184
void membuf_clear(struct membuf *buf)
Clears a memory buffer.
Definition: membuf.h:150
void membuf_init(struct membuf *buf)
Initializes a memory buffer.
Definition: membuf.h:138
size_t membuf_size(const struct membuf *buf)
Returns the total number of bytes written to a memory buffer.
Definition: membuf.h:156
static void co_sdo_req_up_buf(struct co_sdo_req *req)
Constructs a CANopen SDO upload request from its internal buffer.
Definition: sdo.c:431
int co_sdo_req_dn_val(struct co_sdo_req *req, co_unsigned16_t type, void *val, co_unsigned32_t *pac)
Copies the next segment of the specified CANopen SDO download request to the internal buffer and,...
Definition: sdo.c:165
static int co_sdo_req_dn_buf(struct co_sdo_req *req, const void **pptr, size_t *pnbyte)
Copies the next segment of the specified CANopen SDO download request to the internal buffer.
Definition: sdo.c:369
int co_sdo_req_up_val(struct co_sdo_req *req, co_unsigned16_t type, const void *val, co_unsigned32_t *pac)
Writes the specified value to a buffer and constructs a CANopen SDO upload request.
Definition: sdo.c:280
int co_sdo_req_up(struct co_sdo_req *req, const void *ptr, size_t n, co_unsigned32_t *pac)
Writes the specified bytes to a buffer and constructs a CANopen SDO upload request.
Definition: sdo.c:253
void co_sdo_req_fini(struct co_sdo_req *req)
Finalizes a CANopen SDO upload/download request.
Definition: sdo.c:121
int co_sdo_req_dn_file(struct co_sdo_req *req, const char *filename, co_unsigned32_t *pac)
Copies the next segment of the specified CANopen SDO download request to the internal buffer and,...
Definition: sdo.c:207
int co_sdo_req_up_file(struct co_sdo_req *req, const char *filename, co_unsigned32_t *pac)
Loads the specified file into a buffer and constructs a CANopen SDO upload request.
Definition: sdo.c:314
void co_sdo_req_clear(struct co_sdo_req *req)
Clears a CANopen SDO upload/download request, including its buffer.
Definition: sdo.c:129
const char * co_sdo_ac2str(co_unsigned32_t ac)
Returns a string describing an SDO abort code.
Definition: sdo.c:57
void co_sdo_req_init(struct co_sdo_req *req)
Initializes a CANopen SDO upload/download request.
Definition: sdo.c:109
int co_sdo_req_dn(struct co_sdo_req *req, const void **pptr, size_t *pnbyte, co_unsigned32_t *pac)
Copies the next segment of the specified CANopen SDO download request to the internal buffer and,...
Definition: sdo.c:139
This is the internal header file of the CANopen library.
An read file buffer struct.
Definition: frbuf.c:49
An (atomic) write file buffer struct.
Definition: fwbuf.c:56
A CANopen SDO upload/download request.
Definition: sdo.h:178
size_t offset
The offset of the bytes at buf.
Definition: sdo.h:193
size_t size
The total size (in bytes) of the value to be uploaded/downloaded.
Definition: sdo.h:184
const void * buf
A pointer to the next bytes to be uploaded/downloaded.
Definition: sdo.h:186
struct membuf membuf
A memory buffer for use by the upload/download indication function.
Definition: sdo.h:199
size_t nbyte
The number of bytes available at buf.
Definition: sdo.h:188
A memory buffer.
Definition: membuf.h:35
char * begin
A pointer to the first byte in the buffer.
Definition: membuf.h:39
int co_type_is_array(co_unsigned16_t type)
Returns 1 if the specified (static) data type is an array, and 0 if not.
Definition: type.c:40
This header file is part of the CANopen library; it contains the CANopen value declarations.
size_t co_val_read(co_unsigned16_t type, void *val, const uint_least8_t *begin, const uint_least8_t *end)
Reads a value of the specified data type from a memory buffer.
Definition: val.c:481
int co_val_init(co_unsigned16_t type, void *val)
Initializes a value of the specified data type to zero.
Definition: val.c:122
size_t co_val_write(co_unsigned16_t type, const void *val, uint_least8_t *begin, uint_least8_t *end)
Writes a value of the specified data type to a memory buffer.
Definition: val.c:720
void co_val_fini(co_unsigned16_t type, void *val)
Finalizes a value of the specified data type.
Definition: val.c:275