Lely core libraries 2.3.4
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#if !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
50static int co_sdo_req_dn_buf(
51 struct co_sdo_req *req, const void **pptr, size_t *pnbyte);
52
54static void co_sdo_req_up_buf(struct co_sdo_req *req);
55
56const char *
57co_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";
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";
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";
79 return "General internal incompatibility in the device";
80 case CO_SDO_AC_HARDWARE: return "Access failed due to a hardware error";
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";
98 return "Data cannot be transferred or stored to the application because of local control";
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
108void
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, NULL, 0);
118}
119
120void
122{
123 assert(req);
124
125 membuf_fini(&req->membuf);
126}
127
128void
130{
131 req->size = 0;
132 req->buf = NULL;
133 req->nbyte = 0;
134 req->offset = 0;
135 membuf_clear(&req->membuf);
136}
137
138int
139co_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#if LELY_NO_ERRNO
149 ac = CO_SDO_AC_ERROR;
150#else
151 // clang-format off
152 ac = get_errnum() == ERRNUM_NOMEM
155 // clang-format on
156#endif
157 set_errc(errc);
158 // ...falls through ...
159 case 0:
160 // Return without an abort code if not all data is present. This
161 // is not an error.
162 if (pac)
163 *pac = ac;
164 return -1;
165 case 1: return 0;
166 }
167}
168
169int
170co_sdo_req_dn_val(struct co_sdo_req *req, co_unsigned16_t type, void *val,
171 co_unsigned32_t *pac)
172{
173 co_unsigned32_t ac = 0;
174
175 const void *ptr = NULL;
176 size_t nbyte = 0;
177 if (co_sdo_req_dn(req, &ptr, &nbyte, pac) == -1)
178 return -1;
179
180 // Read the value.
181 co_val_init(type, val);
182 size_t size = co_val_read(
183 type, val, ptr, (const uint_least8_t *)ptr + nbyte);
184
185 // Check the size of the value.
186 if (co_type_is_array(type)) {
187 if (size != nbyte) {
188 ac = CO_SDO_AC_NO_MEM;
189 goto error_read;
190 }
191 } else {
192 if (!size) {
194 goto error_read;
195 } else if (size < nbyte) {
197 goto error_read;
198 }
199 }
200
201 return 0;
202
203error_read:
204 co_val_fini(type, val);
205 if (pac)
206 *pac = ac;
207 return -1;
208}
209
210#if !LELY_NO_CO_OBJ_FILE
211int
212co_sdo_req_dn_file(struct co_sdo_req *req, const char *filename,
213 co_unsigned32_t *pac)
214{
215 int errc = get_errc();
216 co_unsigned32_t ac = 0;
217
218 const void *ptr = NULL;
219 size_t nbyte = 0;
220 if (co_sdo_req_dn(req, &ptr, &nbyte, pac) == -1)
221 return -1;
222
223 fwbuf_t *fbuf = fwbuf_create(filename);
224 if (!fbuf) {
225 diag(DIAG_ERROR, get_errc(), "%s", filename);
226 ac = CO_SDO_AC_DATA;
227 goto error_create_fbuf;
228 }
229
230 if (fwbuf_write(fbuf, ptr, nbyte) != (ssize_t)nbyte) {
231 diag(DIAG_ERROR, get_errc(), "%s", filename);
232 ac = CO_SDO_AC_DATA;
233 goto error_write;
234 }
235
236 if (fwbuf_commit(fbuf) == -1) {
237 diag(DIAG_ERROR, get_errc(), "%s", filename);
238 ac = CO_SDO_AC_DATA;
239 goto error_commit;
240 }
241
242 fwbuf_destroy(fbuf);
243
244 return 0;
245
246error_commit:
247error_write:
248 fwbuf_destroy(fbuf);
249error_create_fbuf:
250 if (pac)
251 *pac = ac;
252 set_errc(errc);
253 return -1;
254}
255#endif // !LELY_NO_CO_OBJ_FILE
256
257int
258co_sdo_req_up(struct co_sdo_req *req, const void *ptr, size_t n,
259 co_unsigned32_t *pac)
260{
261 assert(req);
262 struct membuf *buf = &req->membuf;
263
264 co_unsigned32_t ac = 0;
265
266 membuf_clear(buf);
267 int errc = get_errc();
268 if (n && !membuf_reserve(buf, n)) {
269 ac = CO_SDO_AC_NO_MEM;
270 set_errc(errc);
271 goto error_reserve;
272 }
273
274 if (ptr)
275 membuf_write(buf, ptr, n);
276
278 return 0;
279
280error_reserve:
281 if (pac)
282 *pac = ac;
283 return -1;
284}
285
286int
287co_sdo_req_up_val(struct co_sdo_req *req, co_unsigned16_t type, const void *val,
288 co_unsigned32_t *pac)
289{
290 assert(req);
291 struct membuf *buf = &req->membuf;
292
293 co_unsigned32_t ac = 0;
294
295 size_t size = co_val_write(type, val, NULL, NULL);
296
297 membuf_clear(buf);
298 int errc = get_errc();
299 if (size && !membuf_reserve(buf, size)) {
300 ac = CO_SDO_AC_NO_MEM;
301 set_errc(errc);
302 goto error_reserve;
303 }
304
305 uint_least8_t *begin = membuf_alloc(buf, &size);
306 if (co_val_write(type, val, begin, begin + size) != size) {
307 ac = CO_SDO_AC_ERROR;
308 goto error_write;
309 }
310
312 return 0;
313
314error_write:
315error_reserve:
316 if (pac)
317 *pac = ac;
318 return -1;
319}
320
321#if !LELY_NO_CO_OBJ_FILE
322int
323co_sdo_req_up_file(struct co_sdo_req *req, const char *filename,
324 co_unsigned32_t *pac)
325{
326 assert(req);
327 struct membuf *buf = &req->membuf;
328
329 int errc = get_errc();
330 co_unsigned32_t ac = 0;
331
332 frbuf_t *fbuf = frbuf_create(filename);
333 if (!fbuf) {
334 diag(DIAG_ERROR, get_errc(), "%s", filename);
335 ac = CO_SDO_AC_DATA;
336 goto error_create_fbuf;
337 }
338
339 intmax_t size = frbuf_get_size(fbuf);
340 if (size == -1) {
341 diag(DIAG_ERROR, get_errc(), "%s", filename);
342 ac = CO_SDO_AC_DATA;
343 goto error_get_size;
344 }
345 size_t nbyte = (size_t)size;
346
347 membuf_clear(buf);
348 if (size && !membuf_reserve(buf, nbyte)) {
349 ac = CO_SDO_AC_NO_MEM;
350 goto error_reserve;
351 }
352
353 void *ptr = membuf_alloc(buf, &nbyte);
354 if (frbuf_read(fbuf, ptr, nbyte) != (ssize_t)nbyte) {
355 diag(DIAG_ERROR, get_errc(), "%s", filename);
356 ac = CO_SDO_AC_DATA;
357 goto error_read;
358 }
359
360 frbuf_destroy(fbuf);
361
363 return 0;
364
365error_read:
366error_reserve:
367error_get_size:
368 frbuf_destroy(fbuf);
369error_create_fbuf:
370 if (pac)
371 *pac = ac;
372 set_errc(errc);
373 return -1;
374}
375#endif // !LELY_NO_CO_OBJ_FILE
376
377static int
378co_sdo_req_dn_buf(struct co_sdo_req *req, const void **pptr, size_t *pnbyte)
379{
380 assert(req);
381 struct membuf *buf = &req->membuf;
382
383 // In case of an error, keep track of the offset with respect to the
384 // position indicator of the buffer.
385 ptrdiff_t offset = -(ptrdiff_t)membuf_size(buf);
386
387 const void *ptr;
388 if (co_sdo_req_first(req) && co_sdo_req_last(req)) {
389 // If the entire value is available right away, skip copying the
390 // data to the buffer.
391 ptr = req->buf;
392 } else {
393 if (co_sdo_req_first(req)) {
394 membuf_clear(buf);
395 assert(req->size);
396 if (!membuf_reserve(buf, req->size))
397 goto error_reserve;
398 } else {
399 // Adjust the offset if necessary. Only backtracking is
400 // allowed.
401 offset += req->offset;
402 if (offset) {
403 if (offset > 0) {
405 goto error_offset;
406 }
407 membuf_seek(buf, offset);
408 }
409 }
410
411 if (req->nbyte) {
412 if (req->nbyte > membuf_capacity(buf)) {
414 goto error_nbyte;
415 }
416 membuf_write(buf, req->buf, req->nbyte);
417 }
418
419 if (!co_sdo_req_last(req))
420 return 0;
421
422 ptr = membuf_begin(buf);
423 }
424
425 if (pptr)
426 *pptr = ptr;
427 if (pnbyte)
428 *pnbyte = req->size;
429
430 return 1;
431
432error_nbyte:
433error_reserve:
434 // Restore the position indicator of the buffer.
435 membuf_seek(buf, -offset);
436error_offset:
437 return -1;
438}
439
440static void
442{
443 assert(req);
444 struct membuf *buf = &req->membuf;
445
446 req->size = membuf_size(buf);
447 req->buf = membuf_begin(buf);
448 req->nbyte = req->size;
449 req->offset = 0;
450}
This header file is part of the utilities library; it contains the diagnostic declarations.
@ DIAG_ERROR
An error.
Definition diag.h:57
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
Definition diag.c:171
@ ERRNUM_NOMEM
Not enough space.
Definition errnum.h:172
@ ERRNUM_INVAL
Invalid argument.
Definition errnum.h:132
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
Definition errnum.c:932
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition errnum.c:944
errnum_t get_errnum(void)
Returns the last (thread-specific) platform-independent error number set by a system call or library ...
Definition errnum.h:418
void set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
Definition errnum.h:424
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:172
void frbuf_destroy(frbuf_t *buf)
Destroys a read file buffer.
Definition frbuf.c:163
frbuf_t * frbuf_create(const char *filename)
Creates a new read file buffer.
Definition frbuf.c:138
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:275
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:489
void fwbuf_destroy(fwbuf_t *buf)
Destroys a write file buffer.
Definition fwbuf.c:330
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:979
fwbuf_t * fwbuf_create(const char *filename)
Creates a new (atomic) write file buffer.
Definition fwbuf.c:305
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:343
#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:349
#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:193
void * membuf_begin(const struct membuf *buf)
Returns a pointer to the first byte in a memory buffer.
Definition membuf.h:161
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:52
void membuf_fini(struct membuf *buf)
Finalizes a memory buffer.
Definition membuf.c:40
size_t membuf_capacity(const struct membuf *buf)
Returns the number of unused bytes remaining in a memory buffer.
Definition membuf.h:185
void membuf_init(struct membuf *buf, void *ptr, size_t size)
Initializes a memory buffer.
Definition membuf.h:151
size_t membuf_write(struct membuf *buf, const void *ptr, size_t size)
Writes data to a memory buffer.
Definition membuf.h:222
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:211
void membuf_clear(struct membuf *buf)
Clears a memory buffer.
Definition membuf.h:169
size_t membuf_size(const struct membuf *buf)
Returns the total number of bytes written to a memory buffer.
Definition membuf.h:177
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:441
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:170
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:378
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:287
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:258
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:212
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:323
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:52
An (atomic) write file buffer struct.
Definition fwbuf.c:59
A CANopen SDO upload/download request.
Definition sdo.h:181
size_t offset
The offset of the bytes at buf.
Definition sdo.h:196
size_t size
The total size (in bytes) of the value to be uploaded/downloaded.
Definition sdo.h:187
const void * buf
A pointer to the next bytes to be uploaded/downloaded.
Definition sdo.h:189
struct membuf membuf
A memory buffer for use by the upload/download indication function.
Definition sdo.h:202
size_t nbyte
The number of bytes available at buf.
Definition sdo.h:191
A memory buffer.
Definition membuf.h:36
char * begin
A pointer to the first byte in the buffer.
Definition membuf.h:40
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
ptrdiff_t ssize_t
Used for a count of bytes or an error indication.
Definition types.h:43
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:451
int co_val_init(co_unsigned16_t type, void *val)
Initializes a value of the specified data type to zero.
Definition val.c:62
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:791
void co_val_fini(co_unsigned16_t type, void *val)
Finalizes a value of the specified data type.
Definition val.c:249