Lely core libraries 2.3.4
csdo.c
Go to the documentation of this file.
1
24#include "co.h"
25
26#if !LELY_NO_CO_CSDO
27
28#include "sdo.h"
29#include <lely/co/crc.h>
30#include <lely/co/csdo.h>
31#include <lely/co/dev.h>
32#include <lely/co/obj.h>
33#include <lely/co/val.h>
34#include <lely/util/endian.h>
35#include <lely/util/errnum.h>
36
37#include <assert.h>
38#include <stdlib.h>
39#include <string.h>
40
41#if LELY_NO_MALLOC
42#ifndef CO_CSDO_MEMBUF_SIZE
48#define CO_CSDO_MEMBUF_SIZE 8
49#endif
50#endif
51
52struct __co_csdo_state;
54typedef const struct __co_csdo_state co_csdo_state_t;
55
59 co_unsigned32_t n;
61 const uint_least8_t *begin;
63 const uint_least8_t *end;
67 void *data;
68};
69
71struct __co_csdo {
77 co_unsigned8_t num;
89 co_unsigned32_t ac;
91 co_unsigned16_t idx;
93 co_unsigned8_t subidx;
95 co_unsigned32_t size;
97 co_unsigned8_t toggle;
99 co_unsigned8_t blksize;
101 co_unsigned8_t ackseq;
103 unsigned crc : 1;
107 struct membuf *up_buf;
112 struct membuf buf;
113#if LELY_NO_MALLOC
118 char begin[CO_CSDO_MEMBUF_SIZE];
119#endif
138};
139
146static int co_csdo_update(co_csdo_t *sdo);
147
154static co_unsigned32_t co_1280_dn_ind(
155 co_sub_t *sub, struct co_sdo_req *req, void *data);
156
162static int co_csdo_recv(const struct can_msg *msg, void *data);
163
169static int co_csdo_timer(const struct timespec *tp, void *data);
170
175static inline void co_csdo_enter(co_csdo_t *sdo, co_csdo_state_t *next);
176
184static inline void co_csdo_emit_abort(co_csdo_t *sdo, co_unsigned32_t ac);
185
193static inline void co_csdo_emit_time(co_csdo_t *sdo, const struct timespec *tp);
194
202static inline void co_csdo_emit_recv(co_csdo_t *sdo, const struct can_msg *msg);
203
207 co_csdo_state_t *(*on_enter)(co_csdo_t *sdo);
217 co_csdo_state_t *(*on_abort)(co_csdo_t *sdo, co_unsigned32_t ac);
226 co_csdo_state_t *(*on_time)(co_csdo_t *sdo, const struct timespec *tp);
236 co_csdo_state_t *(*on_recv)(co_csdo_t *sdo, const struct can_msg *msg);
238 void (*on_leave)(co_csdo_t *sdo);
239};
240
241#define LELY_CO_DEFINE_STATE(name, ...) \
242 static co_csdo_state_t *const name = &(co_csdo_state_t){ __VA_ARGS__ };
243
246 co_csdo_t *sdo, co_unsigned32_t ac);
247
249// clang-format off
250LELY_CO_DEFINE_STATE(co_csdo_stopped_state,
251 .on_abort = &co_csdo_stopped_on_abort
253// clang-format on
254
255
257 co_csdo_t *sdo, co_unsigned32_t ac);
258
261 co_csdo_t *sdo, const struct can_msg *msg);
262
264// clang-format off
265LELY_CO_DEFINE_STATE(co_csdo_wait_state,
266 .on_abort = &co_csdo_wait_on_abort,
267 .on_recv = &co_csdo_wait_on_recv
269// clang-format on
270
273
275static void co_csdo_abort_on_leave(co_csdo_t *sdo);
276
278// clang-format off
279LELY_CO_DEFINE_STATE(co_csdo_abort_state,
280 .on_enter = &co_csdo_abort_on_enter,
281 .on_leave = &co_csdo_abort_on_leave
283// clang-format on
284
287 co_csdo_t *sdo, co_unsigned32_t ac);
288
291 co_csdo_t *sdo, const struct timespec *tp);
292
298 co_csdo_t *sdo, const struct can_msg *msg);
299
301// clang-format off
302LELY_CO_DEFINE_STATE(co_csdo_dn_ini_state,
303 .on_abort = &co_csdo_dn_ini_on_abort,
304 .on_time = &co_csdo_dn_ini_on_time,
305 .on_recv = &co_csdo_dn_ini_on_recv
307// clang-format on
308
311
314 co_csdo_t *sdo, co_unsigned32_t ac);
315
318 co_csdo_t *sdo, const struct timespec *tp);
319
324 co_csdo_t *sdo, const struct can_msg *msg);
325
327// clang-format off
328LELY_CO_DEFINE_STATE(co_csdo_dn_seg_state,
329 .on_enter = &co_csdo_dn_seg_on_enter,
330 .on_abort = &co_csdo_dn_seg_on_abort,
331 .on_time = &co_csdo_dn_seg_on_time,
332 .on_recv = &co_csdo_dn_seg_on_recv
334// clang-format on
335
338 co_csdo_t *sdo, co_unsigned32_t ac);
339
342 co_csdo_t *sdo, const struct timespec *tp);
343
346 co_csdo_t *sdo, const struct can_msg *msg);
347
349// clang-format off
350LELY_CO_DEFINE_STATE(co_csdo_up_ini_state,
351 .on_abort = &co_csdo_up_ini_on_abort,
352 .on_time = &co_csdo_up_ini_on_time,
353 .on_recv = &co_csdo_up_ini_on_recv
355// clang-format on
356
359 co_csdo_t *sdo, co_unsigned32_t ac);
360
363 co_csdo_t *sdo, const struct timespec *tp);
364
367 co_csdo_t *sdo, const struct can_msg *msg);
368
370// clang-format off
371LELY_CO_DEFINE_STATE(co_csdo_up_seg_state,
372 .on_abort = &co_csdo_up_seg_on_abort,
373 .on_time = &co_csdo_up_seg_on_time,
374 .on_recv = &co_csdo_up_seg_on_recv
376// clang-format on
377
380 co_csdo_t *sdo, co_unsigned32_t ac);
381
384 co_csdo_t *sdo, const struct timespec *tp);
385
391 co_csdo_t *sdo, const struct can_msg *msg);
392
394// clang-format off
395LELY_CO_DEFINE_STATE(co_csdo_blk_dn_ini_state,
396 .on_abort = &co_csdo_blk_dn_ini_on_abort,
397 .on_time = &co_csdo_blk_dn_ini_on_time,
400// clang-format on
401
404
407 co_csdo_t *sdo, co_unsigned32_t ac);
408
411 co_csdo_t *sdo, const struct timespec *tp);
412
418 co_csdo_t *sdo, const struct can_msg *msg);
419
421// clang-format off
422LELY_CO_DEFINE_STATE(co_csdo_blk_dn_sub_state,
423 .on_enter = &co_csdo_blk_dn_sub_on_enter,
424 .on_abort = &co_csdo_blk_dn_sub_on_abort,
425 .on_time = &co_csdo_blk_dn_sub_on_time,
428// clang-format on
429
432 co_csdo_t *sdo, co_unsigned32_t ac);
433
436 co_csdo_t *sdo, const struct timespec *tp);
437
443 co_csdo_t *sdo, const struct can_msg *msg);
444
446// clang-format off
447LELY_CO_DEFINE_STATE(co_csdo_blk_dn_end_state,
448 .on_abort = &co_csdo_blk_dn_end_on_abort,
449 .on_time = &co_csdo_blk_dn_end_on_time,
452// clang-format on
453
456 co_csdo_t *sdo, co_unsigned32_t ac);
457
460 co_csdo_t *sdo, const struct timespec *tp);
461
467 co_csdo_t *sdo, const struct can_msg *msg);
468
470// clang-format off
471LELY_CO_DEFINE_STATE(co_csdo_blk_up_ini_state,
472 .on_abort = &co_csdo_blk_up_ini_on_abort,
473 .on_time = &co_csdo_blk_up_ini_on_time,
476// clang-format on
477
480 co_csdo_t *sdo, co_unsigned32_t ac);
481
484 co_csdo_t *sdo, const struct timespec *tp);
485
491 co_csdo_t *sdo, const struct can_msg *msg);
492
494// clang-format off
495LELY_CO_DEFINE_STATE(co_csdo_blk_up_sub_state,
496 .on_abort = &co_csdo_blk_up_sub_on_abort,
497 .on_time = &co_csdo_blk_up_sub_on_time,
500// clang-format on
501
504 co_csdo_t *sdo, co_unsigned32_t ac);
505
508 co_csdo_t *sdo, const struct timespec *tp);
509
514 co_csdo_t *sdo, const struct can_msg *msg);
515
517// clang-format off
518LELY_CO_DEFINE_STATE(co_csdo_blk_up_end_state,
519 .on_abort = &co_csdo_blk_up_end_on_abort,
520 .on_time = &co_csdo_blk_up_end_on_time,
523// clang-format on
524
525#undef LELY_CO_DEFINE_STATE
526
533static co_csdo_state_t *co_csdo_abort_ind(co_csdo_t *sdo, co_unsigned32_t ac);
534
546static co_csdo_state_t *co_csdo_abort_res(co_csdo_t *sdo, co_unsigned32_t ac);
547
556static int co_csdo_dn_ind(co_csdo_t *sdo, co_unsigned16_t idx,
557 co_unsigned8_t subidx, const void *ptr, size_t n,
558 co_csdo_dn_con_t *con, void *data);
559
568static int co_csdo_up_ind(co_csdo_t *sdo, co_unsigned16_t idx,
569 co_unsigned8_t subidx, struct membuf *buf,
570 co_csdo_up_con_t *con, void *data);
571
578static void co_csdo_send_abort(co_csdo_t *sdo, co_unsigned32_t ac);
579
581static void co_csdo_send_dn_exp_req(co_csdo_t *sdo);
582
584static void co_csdo_send_dn_ini_req(co_csdo_t *sdo);
585
594static void co_csdo_send_dn_seg_req(
595 co_csdo_t *sdo, co_unsigned32_t n, int last);
596
598static void co_csdo_send_up_ini_req(co_csdo_t *sdo);
599
601static void co_csdo_send_up_seg_req(co_csdo_t *sdo);
602
604static void co_csdo_send_blk_dn_ini_req(co_csdo_t *sdo);
605
612static void co_csdo_send_blk_dn_sub_req(co_csdo_t *sdo, co_unsigned8_t seqno);
613
615static void co_csdo_send_blk_dn_end_req(co_csdo_t *sdo);
616
623static void co_csdo_send_blk_up_ini_req(co_csdo_t *sdo, co_unsigned8_t pst);
624
626static void co_csdo_send_start_up_req(co_csdo_t *sdo);
627
629static void co_csdo_send_blk_up_sub_res(co_csdo_t *sdo);
630
632static void co_csdo_send_blk_up_end_res(co_csdo_t *sdo);
633
641static void co_csdo_init_ini_req(
642 co_csdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs);
643
651static void co_csdo_init_seg_req(
652 co_csdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs);
653
660static void co_csdo_dn_dcf_dn_con(co_csdo_t *sdo, co_unsigned16_t idx,
661 co_unsigned8_t subidx, co_unsigned32_t ac, void *data);
662
663int
664co_dev_dn_req(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx,
665 const void *ptr, size_t n, co_csdo_dn_con_t *con, void *data)
666{
667 assert(dev);
668
669 int errc = get_errc();
670 struct co_sdo_req req = CO_SDO_REQ_INIT;
671
672 co_unsigned32_t ac = 0;
673
674 co_obj_t *obj = co_dev_find_obj(dev, idx);
675 if (!obj) {
676 ac = CO_SDO_AC_NO_OBJ;
677 goto done;
678 }
679
680 co_sub_t *sub = co_obj_find_sub(obj, subidx);
681 if (!sub) {
682 ac = CO_SDO_AC_NO_SUB;
683 goto done;
684 }
685
686 if (co_sdo_req_up(&req, ptr, n, &ac) == -1)
687 goto done;
688
689 ac = co_sub_dn_ind(sub, &req);
690
691done:
692 if (con)
693 con(NULL, idx, subidx, ac, data);
694
695 co_sdo_req_fini(&req);
696 set_errc(errc);
697 return 0;
698}
699
700int
701co_dev_dn_val_req(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx,
702 co_unsigned16_t type, const void *val, co_csdo_dn_con_t *con,
703 void *data)
704{
705 assert(dev);
706
707 int errc = get_errc();
708 struct co_sdo_req req = CO_SDO_REQ_INIT;
709
710 co_unsigned32_t ac = 0;
711
712 co_obj_t *obj = co_dev_find_obj(dev, idx);
713 if (!obj) {
714 ac = CO_SDO_AC_NO_OBJ;
715 goto done;
716 }
717
718 co_sub_t *sub = co_obj_find_sub(obj, subidx);
719 if (!sub) {
720 ac = CO_SDO_AC_NO_SUB;
721 goto done;
722 }
723
724 if (co_sdo_req_up_val(&req, type, val, &ac) == -1)
725 goto done;
726
727 ac = co_sub_dn_ind(sub, &req);
728
729done:
730 if (con)
731 con(NULL, idx, subidx, ac, data);
732
733 co_sdo_req_fini(&req);
734 set_errc(errc);
735 return 0;
736}
737
738int
739co_dev_dn_dcf_req(co_dev_t *dev, const uint_least8_t *begin,
740 const uint_least8_t *end, co_csdo_dn_con_t *con, void *data)
741{
742 assert(dev);
743 assert(begin);
744 assert(end >= begin);
745
746 int errc = get_errc();
747 struct co_sdo_req req = CO_SDO_REQ_INIT;
748
749 co_unsigned16_t idx = 0;
750 co_unsigned8_t subidx = 0;
751 co_unsigned32_t ac = 0;
752
753 // Read the total number of sub-indices.
754 co_unsigned32_t n;
755 if (co_val_read(CO_DEFTYPE_UNSIGNED32, &n, begin, end) != 4) {
757 goto done;
758 }
759 begin += 4;
760
761 for (size_t i = 0; i < n && !ac; i++) {
762 idx = 0;
763 subidx = 0;
765 // Read the object index.
766 if (co_val_read(CO_DEFTYPE_UNSIGNED16, &idx, begin, end) != 2)
767 break;
768 begin += 2;
769 // Read the object sub-index.
770 if (co_val_read(CO_DEFTYPE_UNSIGNED8, &subidx, begin, end) != 1)
771 break;
772 begin += 1;
773 // Read the value size (in bytes).
774 co_unsigned32_t size;
775 if (co_val_read(CO_DEFTYPE_UNSIGNED32, &size, begin, end) != 4)
776 break;
777 begin += 4;
778 if (end - begin < (ptrdiff_t)size)
779 break;
780 co_obj_t *obj = co_dev_find_obj(dev, idx);
781
782 if (!obj) {
783 ac = CO_SDO_AC_NO_OBJ;
784 break;
785 }
786 co_sub_t *sub = co_obj_find_sub(obj, subidx);
787 if (!sub) {
788 ac = CO_SDO_AC_NO_SUB;
789 break;
790 }
791
792 // Write the value to the object dictionary.
793 co_sdo_req_clear(&req);
794 // cppcheck-suppress redundantAssignment
795 ac = 0;
796 if (!co_sdo_req_up(&req, begin, size, &ac))
797 ac = co_sub_dn_ind(sub, &req);
798
799 begin += size;
800 }
801
802done:
803 if (con)
804 con(NULL, idx, subidx, ac, data);
805
806 co_sdo_req_fini(&req);
807 set_errc(errc);
808 return 0;
809}
810
811int
812co_dev_up_req(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx,
813 co_csdo_up_con_t *con, void *data)
814{
815 assert(dev);
816
817 int errc = get_errc();
818 struct membuf buf = MEMBUF_INIT;
819 co_unsigned32_t ac = 0;
820
821 const co_obj_t *obj = co_dev_find_obj(dev, idx);
822 if (!obj) {
823 ac = CO_SDO_AC_NO_OBJ;
824 goto done;
825 }
826
827 const co_sub_t *sub = co_obj_find_sub(obj, subidx);
828 if (!sub) {
829 ac = CO_SDO_AC_NO_SUB;
830 goto done;
831 }
832
833 // If the object is an array, check whether the element exists.
835 && subidx > co_obj_get_val_u8(obj, 0)) {
837 goto done;
838 }
839
840 struct co_sdo_req req = CO_SDO_REQ_INIT;
841
842 ac = co_sub_up_ind(sub, &req);
843 if (!ac && req.size && !membuf_reserve(&buf, req.size))
844 ac = CO_SDO_AC_NO_MEM;
845
846 while (!ac && membuf_size(&buf) < req.size) {
847 membuf_write(&buf, req.buf, req.nbyte);
848 if (!co_sdo_req_last(&req))
849 ac = co_sub_up_ind(sub, &req);
850 }
851
852 co_sdo_req_fini(&req);
853
854done:
855 if (con)
856 con(NULL, idx, subidx, ac, ac ? NULL : buf.begin,
857 ac ? 0 : membuf_size(&buf), data);
858
860 set_errc(errc);
861 return 0;
862}
863
864void *
865__co_csdo_alloc(void)
866{
867 void *ptr = malloc(sizeof(struct __co_csdo));
868#if !LELY_NO_ERRNO
869 if (!ptr)
870 set_errc(errno2c(errno));
871#endif
872 return ptr;
873}
874
875void
876__co_csdo_free(void *ptr)
877{
878 free(ptr);
879}
880
881struct __co_csdo *
882__co_csdo_init(struct __co_csdo *sdo, can_net_t *net, co_dev_t *dev,
883 co_unsigned8_t num)
884{
885 assert(sdo);
886 assert(net);
887
888 int errc = 0;
889
890 if (!num || num > (dev ? CO_NUM_SDOS : CO_NUM_NODES)) {
891 errc = errnum2c(ERRNUM_INVAL);
892 goto error_param;
893 }
894
895 // Find the SDO client parameter in the object dictionary.
896 co_obj_t *obj_1280 =
897 dev ? co_dev_find_obj(dev, 0x1280 + num - 1) : NULL;
898 if (dev && !obj_1280) {
899 errc = errnum2c(ERRNUM_INVAL);
900 goto error_param;
901 }
902
903 sdo->net = net;
904 sdo->dev = dev;
905 sdo->num = num;
906
907 // Initialize the SDO parameter record with the default values.
908 sdo->par.n = 3;
909 sdo->par.id = num;
910 sdo->par.cobid_req = 0x600 + sdo->par.id;
911 sdo->par.cobid_res = 0x580 + sdo->par.id;
912
913 sdo->recv = can_recv_create();
914 if (!sdo->recv) {
915 errc = get_errc();
916 goto error_create_recv;
917 }
919
920 sdo->timeout = 0;
921
922 sdo->timer = can_timer_create();
923 if (!sdo->timer) {
924 errc = get_errc();
925 goto error_create_timer;
926 }
928
930
931 sdo->ac = 0;
932 sdo->idx = 0;
933 sdo->subidx = 0;
934 sdo->size = 0;
935
936 sdo->toggle = 0;
937 sdo->blksize = 0;
938 sdo->ackseq = 0;
939 sdo->crc = 0;
940
941 membuf_init(&sdo->dn_buf, NULL, 0);
942 sdo->up_buf = NULL;
943#if LELY_NO_MALLOC
944 membuf_init(&sdo->buf, sdo->begin, CO_CSDO_MEMBUF_SIZE);
945 memset(sdo->begin, 0, CO_CSDO_MEMBUF_SIZE);
946#else
947 membuf_init(&sdo->buf, NULL, 0);
948#endif
949
950 sdo->dn_con = NULL;
951 sdo->dn_con_data = NULL;
952
953 sdo->dn_ind = NULL;
954 sdo->dn_ind_data = NULL;
955
956 sdo->up_con = NULL;
957 sdo->up_con_data = NULL;
958
959 sdo->up_ind = NULL;
960 sdo->up_ind_data = NULL;
961
962 sdo->dn_dcf = (struct co_csdo_dn_dcf){ 0 };
963
964 if (co_csdo_start(sdo) == -1) {
965 errc = get_errc();
966 goto error_start;
967 }
968
969 return sdo;
970
971 // co_csdo_stop(sdo);
972error_start:
974error_create_timer:
976error_create_recv:
977error_param:
978 set_errc(errc);
979 return NULL;
980}
981
982void
983__co_csdo_fini(struct __co_csdo *sdo)
984{
985 assert(sdo);
986 assert(sdo->num >= 1 && sdo->num <= CO_NUM_SDOS);
987
988 co_csdo_stop(sdo);
989
990 membuf_fini(&sdo->buf);
991
994}
995
996co_csdo_t *
997co_csdo_create(can_net_t *net, co_dev_t *dev, co_unsigned8_t num)
998{
999 trace("creating Client-SDO %d", num);
1000
1001 int errc = 0;
1002
1003 co_csdo_t *sdo = __co_csdo_alloc();
1004 if (!sdo) {
1005 errc = get_errc();
1006 goto error_alloc_sdo;
1007 }
1008
1009 if (!__co_csdo_init(sdo, net, dev, num)) {
1010 errc = get_errc();
1011 goto error_init_sdo;
1012 }
1013
1014 return sdo;
1015
1016error_init_sdo:
1017 __co_csdo_free(sdo);
1018error_alloc_sdo:
1019 set_errc(errc);
1020 return NULL;
1021}
1022
1023void
1025{
1026 if (csdo) {
1027 trace("destroying Client-SDO %d", csdo->num);
1028 __co_csdo_fini(csdo);
1029 __co_csdo_free(csdo);
1030 }
1031}
1032
1033int
1035{
1036 assert(sdo);
1037
1038 if (!co_csdo_is_stopped(sdo))
1039 return 0;
1040
1041 if (sdo->dev) {
1042 co_unsigned16_t idx_1280 = 0x1280 + sdo->num - 1;
1043 co_obj_t *obj_1280 = co_dev_find_obj(sdo->dev, idx_1280);
1044 assert(obj_1280);
1045 // Copy the SDO parameters.
1046 memset(&sdo->par, 0, sizeof(sdo->par));
1047 sdo->par.n = co_dev_get_val_u8(sdo->dev, idx_1280, 0);
1048 sdo->par.cobid_req = co_dev_get_val_u32(sdo->dev, idx_1280, 1);
1049 sdo->par.cobid_res = co_dev_get_val_u32(sdo->dev, idx_1280, 2);
1050 sdo->par.id = co_dev_get_val_u8(sdo->dev, idx_1280, 3);
1051 // Set the download indication function for the SDO parameter
1052 // record.
1053 co_obj_set_dn_ind(obj_1280, &co_1280_dn_ind, sdo);
1054 }
1055
1057
1058 if (co_csdo_update(sdo) == -1)
1059 goto error_update;
1060
1061 return 0;
1062
1063error_update:
1064 co_csdo_stop(sdo);
1065 return -1;
1066}
1067
1068void
1070{
1071 assert(sdo);
1072
1073 if (co_csdo_is_stopped(sdo))
1074 return;
1075
1076 // Abort any ongoing transfer.
1078
1079 can_timer_stop(sdo->timer);
1080 can_recv_stop(sdo->recv);
1081
1082 co_obj_t *obj_1280 = NULL;
1083 if (sdo->dev) {
1084 obj_1280 = co_dev_find_obj(sdo->dev, 0x1280 + sdo->num - 1);
1085 // Remove the download indication functions for the SDO
1086 // parameter record.
1087 co_obj_set_dn_ind(obj_1280, NULL, NULL);
1088 }
1089
1091}
1092
1093int
1095{
1096 assert(sdo);
1097
1098 return sdo->state == co_csdo_stopped_state;
1099}
1100
1101can_net_t *
1103{
1104 assert(sdo);
1105
1106 return sdo->net;
1107}
1108
1109co_dev_t *
1111{
1112 assert(sdo);
1113
1114 return sdo->dev;
1115}
1116
1117co_unsigned8_t
1119{
1120 assert(sdo);
1121
1122 return sdo->num;
1123}
1124
1125const struct co_sdo_par *
1127{
1128 assert(sdo);
1129
1130 return &sdo->par;
1131}
1132
1133int
1135{
1136 assert(sdo);
1137
1138 return sdo->timeout;
1139}
1140
1141void
1143{
1144 assert(sdo);
1145
1146 if (sdo->timeout && timeout <= 0)
1147 can_timer_stop(sdo->timer);
1148
1149 sdo->timeout = MAX(0, timeout);
1150}
1151
1152void
1153co_csdo_get_dn_ind(const co_csdo_t *sdo, co_csdo_ind_t **pind, void **pdata)
1154{
1155 assert(sdo);
1156
1157 if (pind)
1158 *pind = sdo->dn_ind;
1159 if (pdata)
1160 *pdata = sdo->dn_ind_data;
1161}
1162
1163void
1165{
1166 assert(sdo);
1167
1168 sdo->dn_ind = ind;
1169 sdo->dn_ind_data = data;
1170}
1171
1172void
1173co_csdo_get_up_ind(const co_csdo_t *sdo, co_csdo_ind_t **pind, void **pdata)
1174{
1175 assert(sdo);
1176
1177 if (pind)
1178 *pind = sdo->up_ind;
1179 if (pdata)
1180 *pdata = sdo->up_ind_data;
1181}
1182
1183void
1185{
1186 assert(sdo);
1187
1188 sdo->up_ind = ind;
1189 sdo->up_ind_data = data;
1190}
1191
1192int
1194{
1195 assert(sdo);
1196
1197 int valid_req = !(sdo->par.cobid_req & CO_SDO_COBID_VALID);
1198 int valid_res = !(sdo->par.cobid_res & CO_SDO_COBID_VALID);
1199 return valid_req && valid_res;
1200}
1201
1202int
1204{
1205 assert(sdo);
1206
1207 return sdo->state == co_csdo_wait_state;
1208}
1209
1210void
1211co_csdo_abort_req(co_csdo_t *sdo, co_unsigned32_t ac)
1212{
1213 assert(sdo);
1214
1215 co_csdo_emit_abort(sdo, ac);
1216}
1217
1218int
1219co_csdo_dn_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx,
1220 const void *ptr, size_t n, co_csdo_dn_con_t *con, void *data)
1221{
1222 assert(sdo);
1223
1224 if (co_csdo_dn_ind(sdo, idx, subidx, ptr, n, con, data) == -1)
1225 return -1;
1226
1227 trace("CSDO: %04X:%02X: initiate download", idx, subidx);
1228
1229 if (sdo->timeout)
1230 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1231 if (sdo->size && sdo->size <= 4)
1233 else
1236
1237 return 0;
1238}
1239
1240int
1241co_csdo_dn_val_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx,
1242 co_unsigned16_t type, const void *val, co_csdo_dn_con_t *con,
1243 void *data)
1244{
1245 assert(sdo);
1246 struct membuf *buf = &sdo->buf;
1247
1248 // Obtain the size of the serialized value (which may be 0 for arrays).
1249 size_t n = co_val_write(type, val, NULL, NULL);
1250 if (!n && co_val_sizeof(type, val))
1251 return -1;
1252
1253 membuf_clear(buf);
1254 if (!membuf_reserve(buf, n))
1255 return -1;
1256 void *ptr = membuf_alloc(buf, &n);
1257
1258 if (co_val_write(type, val, ptr, (uint_least8_t *)ptr + n) != n)
1259 return -1;
1260
1261 return co_csdo_dn_req(sdo, idx, subidx, ptr, n, con, data);
1262}
1263
1264int
1265co_csdo_dn_dcf_req(co_csdo_t *sdo, const uint_least8_t *begin,
1266 const uint_least8_t *end, co_csdo_dn_con_t *con, void *data)
1267{
1268 assert(sdo);
1269 assert(begin);
1270 assert(end >= begin);
1271
1272 // Check whether the SDO exists, is valid and is in the waiting state.
1273 if (!co_csdo_is_valid(sdo) || !co_csdo_is_idle(sdo)) {
1275 return -1;
1276 }
1277
1278 co_unsigned32_t ac = 0;
1279
1280 // Read the total number of sub-indices.
1281 co_unsigned32_t n;
1284 begin += 4;
1285
1286 // Start the first SDO request.
1287 sdo->dn_dcf = (struct co_csdo_dn_dcf){ n, begin, end, con, data };
1288 co_csdo_dn_dcf_dn_con(sdo, 0, 0, ac, NULL);
1289
1290 return 0;
1291}
1292
1293int
1294co_csdo_up_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx,
1295 co_csdo_up_con_t *con, void *data)
1296{
1297 assert(sdo);
1298
1299 if (co_csdo_up_ind(sdo, idx, subidx, NULL, con, data) == -1)
1300 return -1;
1301
1302 trace("CSDO: %04X:%02X: initiate upload", idx, subidx);
1303
1304 if (sdo->timeout)
1305 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1308
1309 return 0;
1310}
1311
1312int
1313co_csdo_blk_dn_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx,
1314 const void *ptr, size_t n, co_csdo_dn_con_t *con, void *data)
1315{
1316 assert(sdo);
1317
1318 if (co_csdo_dn_ind(sdo, idx, subidx, ptr, n, con, data) == -1)
1319 return -1;
1320
1321 trace("CSDO: %04X:%02X: initiate block download", idx, subidx);
1322
1323 if (sdo->timeout)
1324 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1327
1328 return 0;
1329}
1330
1331int
1332co_csdo_blk_dn_val_req(co_csdo_t *sdo, co_unsigned16_t idx,
1333 co_unsigned8_t subidx, co_unsigned16_t type, const void *val,
1334 co_csdo_dn_con_t *con, void *data)
1335{
1336 assert(sdo);
1337 struct membuf *buf = &sdo->buf;
1338
1339 // Obtain the size of the serialized value (which may be 0 for arrays).
1340 size_t n = co_val_write(type, val, NULL, NULL);
1341 if (!n && co_val_sizeof(type, val))
1342 return -1;
1343
1344 membuf_clear(buf);
1345 if (!membuf_reserve(buf, n))
1346 return -1;
1347 void *ptr = membuf_alloc(buf, &n);
1348
1349 if (co_val_write(type, val, ptr, (uint_least8_t *)ptr + n) != n)
1350 return -1;
1351
1352 return co_csdo_blk_dn_req(sdo, idx, subidx, ptr, n, con, data);
1353}
1354
1355int
1356co_csdo_blk_up_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx,
1357 co_unsigned8_t pst, co_csdo_up_con_t *con, void *data)
1358{
1359 assert(sdo);
1360
1361 if (co_csdo_up_ind(sdo, idx, subidx, NULL, con, data) == -1)
1362 return -1;
1363
1364 trace("CSDO: %04X:%02X: initiate block upload", idx, subidx);
1365
1366 // Use the maximum block size by default.
1368
1369 if (sdo->timeout)
1370 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1373
1374 return 0;
1375}
1376
1377static int
1379{
1380 assert(sdo);
1381
1382 // Abort any ongoing transfer.
1384
1385 if (co_csdo_is_valid(sdo)) {
1386 uint_least32_t id = sdo->par.cobid_res;
1387 uint_least8_t flags = 0;
1388 if (id & CO_SDO_COBID_FRAME) {
1389 id &= CAN_MASK_EID;
1390 flags |= CAN_FLAG_IDE;
1391 } else {
1392 id &= CAN_MASK_BID;
1393 }
1394 can_recv_start(sdo->recv, sdo->net, id, flags);
1395 } else {
1396 can_recv_stop(sdo->recv);
1397 }
1398
1399 return 0;
1400}
1401
1402static co_unsigned32_t
1403co_1280_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
1404{
1405 assert(sub);
1406 assert(req);
1407 co_csdo_t *sdo = data;
1408 assert(sdo);
1409 assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1280 + sdo->num - 1);
1410
1411 co_unsigned16_t type = co_sub_get_type(sub);
1412 assert(!co_type_is_array(type));
1413
1414 union co_val val;
1415 co_unsigned32_t ac = 0;
1416 if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
1417 return ac;
1418
1419 switch (co_sub_get_subidx(sub)) {
1420 case 0: return CO_SDO_AC_NO_WRITE;
1421 case 1: {
1422 assert(type == CO_DEFTYPE_UNSIGNED32);
1423 co_unsigned32_t cobid = val.u32;
1424 co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
1425 if (cobid == cobid_old)
1426 return 0;
1427
1428 // The CAN-ID cannot be changed when the SDO is and remains
1429 // valid.
1430 int valid = !(cobid & CO_SDO_COBID_VALID);
1431 int valid_old = !(cobid_old & CO_SDO_COBID_VALID);
1432 uint_least32_t canid = cobid & CAN_MASK_EID;
1433 uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
1434 if (valid && valid_old && canid != canid_old)
1435 return CO_SDO_AC_PARAM_VAL;
1436
1437 // A 29-bit CAN-ID is only valid if the frame bit is set.
1438 if (!(cobid & CO_SDO_COBID_FRAME)
1439 && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID)))
1440 return CO_SDO_AC_PARAM_VAL;
1441
1442 sdo->par.cobid_req = cobid;
1443 break;
1444 }
1445 case 2: {
1446 assert(type == CO_DEFTYPE_UNSIGNED32);
1447 co_unsigned32_t cobid = val.u32;
1448 co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
1449 if (cobid == cobid_old)
1450 return 0;
1451
1452 // The CAN-ID cannot be changed when the SDO is and remains
1453 // valid.
1454 int valid = !(cobid & CO_SDO_COBID_VALID);
1455 int valid_old = !(cobid_old & CO_SDO_COBID_VALID);
1456 uint_least32_t canid = cobid & CAN_MASK_EID;
1457 uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
1458 if (valid && valid_old && canid != canid_old)
1459 return CO_SDO_AC_PARAM_VAL;
1460
1461 // A 29-bit CAN-ID is only valid if the frame bit is set.
1462 if (!(cobid & CO_SDO_COBID_FRAME)
1463 && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID)))
1464 return CO_SDO_AC_PARAM_VAL;
1465
1466 sdo->par.cobid_res = cobid;
1467 break;
1468 }
1469 case 3: {
1470 assert(type == CO_DEFTYPE_UNSIGNED8);
1471 co_unsigned8_t id = val.u8;
1472 co_unsigned8_t id_old = co_sub_get_val_u8(sub);
1473 if (id == id_old)
1474 return 0;
1475
1476 sdo->par.id = id;
1477 break;
1478 }
1479 default: return CO_SDO_AC_NO_SUB;
1480 }
1481
1482 co_sub_dn(sub, &val);
1483
1484 co_csdo_update(sdo);
1485 return 0;
1486}
1487
1488static int
1489co_csdo_recv(const struct can_msg *msg, void *data)
1490{
1491 assert(msg);
1492 co_csdo_t *sdo = data;
1493 assert(sdo);
1494
1495 // Ignore remote frames.
1496 if (msg->flags & CAN_FLAG_RTR)
1497 return 0;
1498
1499#if !LELY_NO_CANFD
1500 // Ignore CAN FD format frames.
1501 if (msg->flags & CAN_FLAG_EDL)
1502 return 0;
1503#endif
1504
1505 co_csdo_emit_recv(sdo, msg);
1506
1507 return 0;
1508}
1509
1510static int
1511co_csdo_timer(const struct timespec *tp, void *data)
1512{
1513 assert(tp);
1514 co_csdo_t *sdo = data;
1515 assert(sdo);
1516
1517 co_csdo_emit_time(sdo, tp);
1518
1519 return 0;
1520}
1521
1522static inline void
1524{
1525 assert(sdo);
1526 assert(sdo->state);
1527
1528 while (next) {
1529 co_csdo_state_t *prev = sdo->state;
1530 sdo->state = next;
1531
1532 if (prev->on_leave)
1533 prev->on_leave(sdo);
1534
1535 next = next->on_enter ? next->on_enter(sdo) : NULL;
1536 }
1537}
1538
1539static inline void
1540co_csdo_emit_abort(co_csdo_t *sdo, co_unsigned32_t ac)
1541{
1542 assert(sdo);
1543 assert(sdo->state);
1544 assert(sdo->state->on_abort);
1545
1546 co_csdo_enter(sdo, sdo->state->on_abort(sdo, ac));
1547}
1548
1549static inline void
1550co_csdo_emit_time(co_csdo_t *sdo, const struct timespec *tp)
1551{
1552 assert(sdo);
1553 assert(sdo->state);
1554 assert(sdo->state->on_time);
1555
1556 co_csdo_enter(sdo, sdo->state->on_time(sdo, tp));
1557}
1558
1559static inline void
1560co_csdo_emit_recv(co_csdo_t *sdo, const struct can_msg *msg)
1561{
1562 assert(sdo);
1563 assert(sdo->state);
1564 assert(sdo->state->on_recv);
1565
1566 co_csdo_enter(sdo, sdo->state->on_recv(sdo, msg));
1567}
1568
1569static co_csdo_state_t *
1570co_csdo_stopped_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
1571{
1572 (void)sdo;
1573 (void)ac;
1574
1575 return NULL;
1576}
1577
1578static co_csdo_state_t *
1579co_csdo_wait_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
1580{
1581 (void)sdo;
1582 (void)ac;
1583
1584 return NULL;
1585}
1586
1587static co_csdo_state_t *
1589{
1590 assert(sdo);
1591 assert(msg);
1592
1593 if (msg->len < 1)
1595 co_unsigned8_t cs = msg->data[0];
1596
1597 co_unsigned32_t ac;
1598 switch (cs & CO_SDO_CS_MASK) {
1599 case CO_SDO_CS_ABORT:
1600 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
1601 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
1602 default: return NULL;
1603 }
1604}
1605
1606static co_csdo_state_t *
1608{
1609 (void)sdo;
1610
1611 can_timer_stop(sdo->timer);
1612
1613 return co_csdo_wait_state;
1614}
1615
1616static void
1618{
1619 assert(sdo);
1620
1621 co_csdo_dn_con_t *dn_con = sdo->dn_con;
1622 sdo->dn_con = NULL;
1623 void *dn_con_data = sdo->dn_con_data;
1624 sdo->dn_con_data = NULL;
1625
1626 co_csdo_up_con_t *up_con = sdo->up_con;
1627 sdo->up_con = NULL;
1628 void *up_con_data = sdo->up_con_data;
1629 sdo->up_con_data = NULL;
1630
1631 if (dn_con) {
1632 dn_con(sdo, sdo->idx, sdo->subidx, sdo->ac, dn_con_data);
1633 } else if (up_con) {
1634 struct membuf *buf = sdo->up_buf;
1635 assert(buf);
1636
1637 up_con(sdo, sdo->idx, sdo->subidx, sdo->ac,
1638 sdo->ac ? NULL : buf->begin,
1639 sdo->ac ? 0 : membuf_size(buf), up_con_data);
1640 }
1641}
1642
1643static co_csdo_state_t *
1644co_csdo_dn_ini_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
1645{
1646 return co_csdo_abort_res(sdo, ac);
1647}
1648
1649static co_csdo_state_t *
1650co_csdo_dn_ini_on_time(co_csdo_t *sdo, const struct timespec *tp)
1651{
1652 (void)tp;
1653
1655}
1656
1657static co_csdo_state_t *
1659{
1660 assert(sdo);
1661 assert(msg);
1662
1663 if (msg->len < 1)
1665 co_unsigned8_t cs = msg->data[0];
1666
1667 // Check the server command specifier.
1668 co_unsigned32_t ac;
1669 switch (cs & CO_SDO_CS_MASK) {
1670 case CO_SDO_SCS_DN_INI_RES: break;
1671 case CO_SDO_CS_ABORT:
1672 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
1673 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
1674 default: return co_csdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1675 }
1676
1677 // Check the object index and sub-index.
1678 if (msg->len < 4)
1680 co_unsigned16_t idx = ldle_u16(msg->data + 1);
1681 co_unsigned8_t subidx = msg->data[3];
1682 if (idx != sdo->idx || subidx != sdo->subidx)
1684
1685 return co_csdo_dn_seg_state;
1686}
1687
1688static co_csdo_state_t *
1690{
1691 assert(sdo);
1692 struct membuf *buf = &sdo->dn_buf;
1693
1694 size_t n = sdo->size - membuf_size(buf);
1695 // 0-byte values cannot be sent using expedited transfer, so we need to
1696 // send one empty segment. We use the toggle bit to check if it was
1697 // sent.
1698 if (n || (!sdo->size && !sdo->toggle)) {
1699 if (sdo->timeout)
1700 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1701 co_csdo_send_dn_seg_req(sdo, MIN(n, 7), n <= 7);
1702 return NULL;
1703 } else {
1704 return co_csdo_abort_ind(sdo, 0);
1705 }
1706}
1707
1708static co_csdo_state_t *
1709co_csdo_dn_seg_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
1710{
1711 return co_csdo_abort_res(sdo, ac);
1712}
1713
1714static co_csdo_state_t *
1715co_csdo_dn_seg_on_time(co_csdo_t *sdo, const struct timespec *tp)
1716{
1717 (void)tp;
1718
1720}
1721
1722static co_csdo_state_t *
1724{
1725 assert(sdo);
1726 assert(msg);
1727
1728 if (msg->len < 1)
1730 co_unsigned8_t cs = msg->data[0];
1731
1732 // Check the server command specifier.
1733 co_unsigned32_t ac;
1734 switch (cs & CO_SDO_CS_MASK) {
1735 case CO_SDO_SCS_DN_SEG_RES: break;
1736 case CO_SDO_CS_ABORT:
1737 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
1738 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
1739 default: return co_csdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1740 }
1741
1742 // Check the value of the toggle bit.
1743 if ((cs & CO_SDO_SEG_TOGGLE) == sdo->toggle)
1745
1746 return co_csdo_dn_seg_state;
1747}
1748
1749static co_csdo_state_t *
1750co_csdo_up_ini_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
1751{
1752 return co_csdo_abort_res(sdo, ac);
1753}
1754
1755static co_csdo_state_t *
1756co_csdo_up_ini_on_time(co_csdo_t *sdo, const struct timespec *tp)
1757{
1758 (void)tp;
1759
1761}
1762
1763static co_csdo_state_t *
1765{
1766 assert(sdo);
1767 assert(msg);
1768 struct membuf *buf = sdo->up_buf;
1769 assert(buf);
1770
1771 if (msg->len < 1)
1773 co_unsigned8_t cs = msg->data[0];
1774
1775 // Check the server command specifier.
1776 co_unsigned32_t ac;
1777 switch (cs & CO_SDO_CS_MASK) {
1778 case CO_SDO_SCS_UP_INI_RES: break;
1779 case CO_SDO_CS_ABORT:
1780 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
1781 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
1782 default: return co_csdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1783 }
1784
1785 // Check the object index and sub-index.
1786 if (msg->len < 4)
1788 co_unsigned16_t idx = ldle_u16(msg->data + 1);
1789 co_unsigned8_t subidx = msg->data[3];
1790 if (idx != sdo->idx || subidx != sdo->subidx)
1792
1793 // 0-pad the data bytes to handle servers which send CAN frames less
1794 // than 8 bytes.
1795 uint_least8_t data[4] = { 0 };
1796 memcpy(data, msg->data + 4, msg->len - 4);
1797
1798 // Obtain the size from the command specifier.
1799 int exp = !!(cs & CO_SDO_INI_SIZE_EXP);
1800 sdo->size = 0;
1801 if (exp) {
1802 if (cs & CO_SDO_INI_SIZE_IND)
1803 sdo->size = CO_SDO_INI_SIZE_EXP_GET(cs);
1804 else
1805 sdo->size = msg->len - 4;
1806 } else if (cs & CO_SDO_INI_SIZE_IND) {
1807 sdo->size = ldle_u32(data);
1808 }
1809
1810 // Allocate the buffer.
1811 if (sdo->size && !membuf_reserve(buf, sdo->size))
1813
1814 if (exp) {
1815 // Perform an expedited transfer.
1816 membuf_write(buf, data, sdo->size);
1817
1818 return co_csdo_abort_ind(sdo, 0);
1819 } else {
1820 if (sdo->size && sdo->up_ind)
1821 sdo->up_ind(sdo, sdo->idx, sdo->subidx, sdo->size, 0,
1822 sdo->up_ind_data);
1823 if (sdo->timeout)
1824 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1826 return co_csdo_up_seg_state;
1827 }
1828}
1829
1830static co_csdo_state_t *
1831co_csdo_up_seg_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
1832{
1833 return co_csdo_abort_res(sdo, ac);
1834}
1835
1836static co_csdo_state_t *
1837co_csdo_up_seg_on_time(co_csdo_t *sdo, const struct timespec *tp)
1838{
1839 (void)tp;
1840
1842}
1843
1844static co_csdo_state_t *
1846{
1847 assert(sdo);
1848 assert(msg);
1849 struct membuf *buf = sdo->up_buf;
1850 assert(buf);
1851
1852 if (msg->len < 1)
1854 co_unsigned8_t cs = msg->data[0];
1855
1856 // Check the server command specifier.
1857 co_unsigned32_t ac;
1858 switch (cs & CO_SDO_CS_MASK) {
1859 case CO_SDO_SCS_UP_SEG_RES: break;
1860 case CO_SDO_CS_ABORT:
1861 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
1862 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
1863 default: return co_csdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1864 }
1865
1866 // Check the value of the toggle bit.
1867 if ((cs & CO_SDO_SEG_TOGGLE) == sdo->toggle)
1868 return co_csdo_up_seg_state;
1869
1870 // Obtain the size of the segment.
1871 size_t n = CO_SDO_SEG_SIZE_GET(cs);
1872 if (msg->len < 1 + n)
1874 int last = !!(cs & CO_SDO_SEG_LAST);
1875
1876 if (membuf_size(buf) + n > sdo->size)
1878
1879 // Copy the data to the buffer.
1880 assert(membuf_capacity(buf) >= n);
1881 membuf_write(buf, msg->data + 1, n);
1882
1883 if ((last || !(membuf_size(buf) % (CO_SDO_MAX_SEQNO * 7))) && sdo->size
1884 && sdo->up_ind)
1885 sdo->up_ind(sdo, sdo->idx, sdo->subidx, sdo->size,
1886 membuf_size(buf), sdo->up_ind_data);
1887 if (last) {
1888 if (sdo->size && membuf_size(buf) != sdo->size)
1890 return co_csdo_abort_ind(sdo, 0);
1891 } else {
1892 if (sdo->timeout)
1893 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1895 return co_csdo_up_seg_state;
1896 }
1897}
1898
1899static co_csdo_state_t *
1901{
1902 return co_csdo_abort_res(sdo, ac);
1903}
1904
1905static co_csdo_state_t *
1906co_csdo_blk_dn_ini_on_time(co_csdo_t *sdo, const struct timespec *tp)
1907{
1908 (void)tp;
1909
1911}
1912
1913static co_csdo_state_t *
1915{
1916 assert(sdo);
1917 assert(msg);
1918
1919 if (msg->len < 1)
1921 co_unsigned8_t cs = msg->data[0];
1922
1923 // Check the server command specifier.
1924 co_unsigned32_t ac;
1925 switch (cs & CO_SDO_CS_MASK) {
1926 case CO_SDO_SCS_BLK_DN_RES: break;
1927 case CO_SDO_CS_ABORT:
1928 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
1929 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
1930 default: return co_csdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1931 }
1932
1933 // Check the server subcommand.
1934 if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_INI_BLK)
1936
1937 // Check if the server supports generating a CRC.
1938 sdo->crc = !!(cs & CO_SDO_BLK_CRC);
1939
1940 // Check the object index and sub-index.
1941 if (msg->len < 4)
1943 co_unsigned16_t idx = ldle_u16(msg->data + 1);
1944 co_unsigned8_t subidx = msg->data[3];
1945 if (idx != sdo->idx || subidx != sdo->subidx)
1947
1948 // Load the number of segments per block.
1949 if (msg->len < 5)
1951 sdo->blksize = msg->data[4];
1952
1954}
1955
1956static co_csdo_state_t *
1958{
1959 assert(sdo);
1960 struct membuf *buf = &sdo->dn_buf;
1961
1962 size_t n = sdo->size - membuf_size(buf);
1963 if ((n > 0 && !sdo->blksize) || sdo->blksize > CO_SDO_MAX_SEQNO)
1965 sdo->blksize = (co_unsigned8_t)MIN((n + 6) / 7, sdo->blksize);
1966
1967 if (sdo->size && sdo->dn_ind)
1968 sdo->dn_ind(sdo, sdo->idx, sdo->subidx, sdo->size,
1969 membuf_size(buf), sdo->dn_ind_data);
1970 if (sdo->timeout)
1971 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1972 if (n) {
1973 // Send all segments in the current block.
1974 for (co_unsigned8_t seqno = 1; seqno <= sdo->blksize; seqno++)
1975 co_csdo_send_blk_dn_sub_req(sdo, seqno);
1976 return NULL;
1977 } else {
1980 }
1981}
1982
1983static co_csdo_state_t *
1985{
1986 return co_csdo_abort_res(sdo, ac);
1987}
1988
1989static co_csdo_state_t *
1990co_csdo_blk_dn_sub_on_time(co_csdo_t *sdo, const struct timespec *tp)
1991{
1992 (void)tp;
1993
1995}
1996
1997static co_csdo_state_t *
1999{
2000 assert(sdo);
2001 assert(msg);
2002 struct membuf *buf = &sdo->dn_buf;
2003
2004 if (msg->len < 1)
2006 co_unsigned8_t cs = msg->data[0];
2007
2008 // Check the server command specifier.
2009 co_unsigned32_t ac;
2010 switch (cs & CO_SDO_CS_MASK) {
2011 case CO_SDO_SCS_BLK_DN_RES: break;
2012 case CO_SDO_CS_ABORT:
2013 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
2014 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
2015 default: return co_csdo_abort_res(sdo, CO_SDO_AC_NO_CS);
2016 }
2017
2018 // Check the server subcommand.
2019 if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_BLK_RES)
2021
2022 if (msg->len < 2)
2024 co_unsigned8_t ackseq = msg->data[1];
2025 if (ackseq < sdo->blksize) {
2026 // If the sequence number of the last segment that was
2027 // successfully received is smaller than the number of segments
2028 // in the block, resend the missing segments.
2029 size_t n = (membuf_size(buf) + 6) / 7;
2030 assert(n >= sdo->blksize);
2031 n -= sdo->blksize - ackseq;
2032 buf->cur = buf->begin + n * 7;
2033 }
2034
2035 // Read the number of segments in the next block.
2036 if (msg->len < 3)
2038 sdo->blksize = msg->data[2];
2039
2041}
2042
2043static co_csdo_state_t *
2045{
2046 return co_csdo_abort_res(sdo, ac);
2047}
2048
2049static co_csdo_state_t *
2050co_csdo_blk_dn_end_on_time(co_csdo_t *sdo, const struct timespec *tp)
2051{
2052 (void)tp;
2053
2055}
2056
2057static co_csdo_state_t *
2059{
2060 assert(sdo);
2061 assert(msg);
2062
2063 if (msg->len < 1)
2065 co_unsigned8_t cs = msg->data[0];
2066
2067 // Check the server command specifier.
2068 co_unsigned32_t ac;
2069 switch (cs & CO_SDO_CS_MASK) {
2070 case CO_SDO_SCS_BLK_DN_RES: break;
2071 case CO_SDO_CS_ABORT:
2072 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
2073 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
2074 default: return co_csdo_abort_res(sdo, CO_SDO_AC_NO_CS);
2075 }
2076
2077 // Check the server subcommand.
2078 if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_END_BLK)
2080
2081 return co_csdo_abort_ind(sdo, 0);
2082}
2083
2084static co_csdo_state_t *
2086{
2087 return co_csdo_abort_res(sdo, ac);
2088}
2089
2090static co_csdo_state_t *
2091co_csdo_blk_up_ini_on_time(co_csdo_t *sdo, const struct timespec *tp)
2092{
2093 (void)tp;
2094
2096}
2097
2098static co_csdo_state_t *
2100{
2101 assert(sdo);
2102 assert(msg);
2103 struct membuf *buf = sdo->up_buf;
2104 assert(buf);
2105
2106 if (msg->len < 1)
2108 co_unsigned8_t cs = msg->data[0];
2109
2110 // Check the server command specifier.
2111 co_unsigned32_t ac;
2112 switch (cs & CO_SDO_CS_MASK) {
2114 // In case of a server-induced protocol switch, fall back to the
2115 // SDO upload protocol.
2116 return co_csdo_up_ini_on_recv(sdo, msg);
2117 case CO_SDO_SCS_BLK_UP_RES: break;
2118 case CO_SDO_CS_ABORT:
2119 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
2120 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
2121 default: return co_csdo_abort_res(sdo, CO_SDO_AC_NO_CS);
2122 }
2123
2124 // Check the server subcommand.
2125 if ((cs & 0x01) != CO_SDO_SC_INI_BLK)
2127
2128 // Check if the server supports generating a CRC.
2129 sdo->crc = !!(cs & CO_SDO_BLK_CRC);
2130
2131 // Check the object index and sub-index.
2132 if (msg->len < 4)
2134 co_unsigned16_t idx = ldle_u16(msg->data + 1);
2135 co_unsigned8_t subidx = msg->data[3];
2136 if (idx != sdo->idx || subidx != sdo->subidx)
2138
2139 // Obtain the data set size.
2140 sdo->size = 0;
2141 if (cs & CO_SDO_BLK_SIZE_IND) {
2142 // 0-pad the data bytes to handle servers which send CAN frames
2143 // less than 8 bytes.
2144 uint_least8_t data[4] = { 0 };
2145 memcpy(data, msg->data + 4, msg->len - 4);
2146 sdo->size = ldle_u32(data);
2147 }
2148
2149 // Allocate the buffer.
2150 if (sdo->size && !membuf_reserve(buf, sdo->size))
2152
2153 sdo->ackseq = 0;
2154
2155 if (sdo->timeout)
2156 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
2159}
2160
2161static co_csdo_state_t *
2163{
2164 return co_csdo_abort_res(sdo, ac);
2165}
2166
2167static co_csdo_state_t *
2168co_csdo_blk_up_sub_on_time(co_csdo_t *sdo, const struct timespec *tp)
2169{
2170 (void)tp;
2171
2173}
2174
2175static co_csdo_state_t *
2177{
2178 assert(sdo);
2179 assert(msg);
2180 struct membuf *buf = sdo->up_buf;
2181 assert(buf);
2182
2183 if (msg->len < 1)
2185 co_unsigned8_t cs = msg->data[0];
2186
2187 if (cs == CO_SDO_CS_ABORT) {
2188 co_unsigned32_t ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
2189 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
2190 }
2191
2192 co_unsigned8_t seqno = cs & ~CO_SDO_SEQ_LAST;
2193 int last = !!(cs & CO_SDO_SEQ_LAST);
2194
2195 // Only accept sequential segments. Dropped segments will be resent
2196 // after the confirmation message.
2197 if (seqno == sdo->ackseq + 1) {
2198 sdo->ackseq++;
2199
2200 // Determine the number of bytes to copy.
2201 assert(sdo->size >= membuf_size(buf));
2202 size_t n = MIN(sdo->size - membuf_size(buf), 7);
2203 if (!last && n < 7)
2205
2206 // Copy the data to the buffer.
2207 assert(membuf_capacity(buf) >= n);
2208 membuf_write(buf, msg->data + 1, n);
2209 }
2210
2211 // If this is the last segment in the block, send a confirmation.
2212 if (seqno == sdo->blksize || last) {
2214 sdo->ackseq = 0;
2215 }
2216
2217 if (sdo->timeout)
2218 can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
2220}
2221
2222static co_csdo_state_t *
2224{
2225 return co_csdo_abort_res(sdo, ac);
2226}
2227
2228static co_csdo_state_t *
2229co_csdo_blk_up_end_on_time(co_csdo_t *sdo, const struct timespec *tp)
2230{
2231 (void)tp;
2232
2234}
2235
2236static co_csdo_state_t *
2238{
2239 assert(sdo);
2240 assert(msg);
2241 struct membuf *buf = sdo->up_buf;
2242 assert(buf);
2243
2244 if (msg->len < 1)
2246 co_unsigned8_t cs = msg->data[0];
2247
2248 // Check the server command specifier.
2249 co_unsigned32_t ac;
2250 switch (cs & CO_SDO_CS_MASK) {
2251 case CO_SDO_SCS_BLK_UP_RES: break;
2252 case CO_SDO_CS_ABORT:
2253 ac = msg->len < 8 ? 0 : ldle_u32(msg->data + 4);
2254 return co_csdo_abort_ind(sdo, ac ? ac : CO_SDO_AC_ERROR);
2255 default: return co_csdo_abort_res(sdo, CO_SDO_AC_NO_CS);
2256 }
2257
2258 // Check the server subcommand.
2259 if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_END_BLK)
2261
2262 // Check the total length.
2263 if (sdo->size && membuf_size(buf) != sdo->size)
2265
2266 // Check the number of bytes in the last segment.
2267 co_unsigned8_t n = sdo->size ? (sdo->size - 1) % 7 + 1 : 0;
2268 if (CO_SDO_BLK_SIZE_GET(cs) != n)
2270
2271 // Check the CRC.
2272 if (sdo->crc) {
2273 co_unsigned16_t crc = ldle_u16(msg->data + 1);
2274 if (crc != co_crc(0, (uint_least8_t *)buf->begin, sdo->size))
2276 }
2277
2279 return co_csdo_abort_ind(sdo, 0);
2280}
2281
2282static co_csdo_state_t *
2283co_csdo_abort_ind(co_csdo_t *sdo, co_unsigned32_t ac)
2284{
2285 assert(sdo);
2286
2287 sdo->ac = ac;
2288 return co_csdo_abort_state;
2289}
2290
2291static co_csdo_state_t *
2292co_csdo_abort_res(co_csdo_t *sdo, co_unsigned32_t ac)
2293{
2294 co_csdo_send_abort(sdo, ac);
2295 return co_csdo_abort_ind(sdo, ac);
2296}
2297
2298static int
2299co_csdo_dn_ind(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx,
2300 const void *ptr, size_t n, co_csdo_dn_con_t *con, void *data)
2301{
2302 assert(sdo);
2303
2304 // Check whether the SDO exists, is valid and is in the waiting state.
2305 if (!co_csdo_is_valid(sdo) || !co_csdo_is_idle(sdo)) {
2307 return -1;
2308 }
2309
2310 sdo->ac = 0;
2311 sdo->idx = idx;
2312 sdo->subidx = subidx;
2313 sdo->size = ptr ? n : 0;
2314
2315 sdo->toggle = 0;
2316 sdo->blksize = 0;
2317 sdo->ackseq = 0;
2318 sdo->crc = 0;
2319
2320 // Casting away const is safe here since a download (write) request only
2321 // reads from the provided buffer.
2322 membuf_init(&sdo->dn_buf, (void *)ptr, n);
2323
2324 sdo->dn_con = con;
2325 sdo->dn_con_data = data;
2326
2327 sdo->up_con = NULL;
2328 sdo->up_con_data = NULL;
2329
2330 return 0;
2331}
2332
2333static int
2334co_csdo_up_ind(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx,
2335 struct membuf *buf, co_csdo_up_con_t *con, void *data)
2336{
2337 assert(sdo);
2338
2339 // Check whether the SDO exists, is valid and is in the waiting state.
2340 if (!co_csdo_is_valid(sdo) || !co_csdo_is_idle(sdo)) {
2342 return -1;
2343 }
2344
2345 sdo->ac = 0;
2346 sdo->idx = idx;
2347 sdo->subidx = subidx;
2348 sdo->size = 0;
2349
2350 sdo->toggle = 0;
2351 sdo->blksize = 0;
2352 sdo->ackseq = 0;
2353 sdo->crc = 0;
2354
2355 sdo->up_buf = buf ? buf : &sdo->buf;
2356 membuf_clear(sdo->up_buf);
2357
2358 sdo->dn_con = NULL;
2359 sdo->dn_con_data = NULL;
2360
2361 sdo->up_con = con;
2362 sdo->up_con_data = data;
2363
2364 return 0;
2365}
2366
2367static void
2368co_csdo_send_abort(co_csdo_t *sdo, co_unsigned32_t ac)
2369{
2370 assert(sdo);
2371
2372 struct can_msg msg;
2374 stle_u32(msg.data + 4, ac);
2375 can_net_send(sdo->net, &msg);
2376}
2377
2378static void
2380{
2381 assert(sdo);
2382 assert(sdo->size && sdo->size <= 4);
2383 struct membuf *buf = &sdo->dn_buf;
2384
2385 co_unsigned8_t cs = CO_SDO_CCS_DN_INI_REQ
2387
2388 struct can_msg msg;
2389 co_csdo_init_ini_req(sdo, &msg, cs);
2390 memcpy(msg.data + 4, buf->cur, sdo->size);
2391 buf->cur += sdo->size;
2392 can_net_send(sdo->net, &msg);
2393}
2394
2395static void
2397{
2398 assert(sdo);
2399 assert(!sdo->size || sdo->size > 4);
2400
2401 co_unsigned8_t cs = CO_SDO_CCS_DN_INI_REQ | CO_SDO_INI_SIZE_IND;
2402
2403 struct can_msg msg;
2404 co_csdo_init_ini_req(sdo, &msg, cs);
2405 stle_u32(msg.data + 4, sdo->size);
2406 can_net_send(sdo->net, &msg);
2407
2408 if (sdo->size && sdo->dn_ind)
2409 sdo->dn_ind(sdo, sdo->idx, sdo->subidx, sdo->size, 0,
2410 sdo->dn_ind_data);
2411}
2412
2413static void
2414co_csdo_send_dn_seg_req(co_csdo_t *sdo, co_unsigned32_t n, int last)
2415{
2416 assert(sdo);
2417 assert(n <= 7);
2418 struct membuf *buf = &sdo->dn_buf;
2419
2420 co_unsigned8_t cs = CO_SDO_CCS_DN_SEG_REQ | sdo->toggle
2422 sdo->toggle ^= CO_SDO_SEG_TOGGLE;
2423 if (last)
2424 cs |= CO_SDO_SEG_LAST;
2425
2426 struct can_msg msg;
2427 co_csdo_init_seg_req(sdo, &msg, cs);
2428 memcpy(msg.data + 1, buf->cur, n);
2429 buf->cur += n;
2430 can_net_send(sdo->net, &msg);
2431
2432 if ((last || !(membuf_size(buf) % (CO_SDO_MAX_SEQNO * 7))) && sdo->size
2433 && sdo->dn_ind)
2434 sdo->dn_ind(sdo, sdo->idx, sdo->subidx, sdo->size,
2435 membuf_size(buf), sdo->dn_ind_data);
2436}
2437
2438static void
2440{
2441 assert(sdo);
2442
2443 co_unsigned8_t cs = CO_SDO_CCS_UP_INI_REQ;
2444
2445 struct can_msg msg;
2446 co_csdo_init_ini_req(sdo, &msg, cs);
2447 can_net_send(sdo->net, &msg);
2448}
2449
2450static void
2452{
2453 assert(sdo);
2454
2455 co_unsigned8_t cs = CO_SDO_CCS_UP_SEG_REQ | sdo->toggle;
2456 sdo->toggle ^= CO_SDO_SEG_TOGGLE;
2457
2458 struct can_msg msg;
2459 co_csdo_init_seg_req(sdo, &msg, cs);
2460 can_net_send(sdo->net, &msg);
2461}
2462
2463static void
2465{
2466 assert(sdo);
2467
2468 co_unsigned8_t cs = CO_SDO_CCS_BLK_DN_REQ | CO_SDO_BLK_CRC
2470
2471 struct can_msg msg;
2472 co_csdo_init_ini_req(sdo, &msg, cs);
2473 stle_u32(msg.data + 4, sdo->size);
2474 can_net_send(sdo->net, &msg);
2475}
2476
2477static void
2478co_csdo_send_blk_dn_sub_req(co_csdo_t *sdo, co_unsigned8_t seqno)
2479{
2480 assert(sdo);
2481 assert(seqno && seqno <= CO_SDO_MAX_SEQNO);
2482 struct membuf *buf = &sdo->dn_buf;
2483
2484 size_t n = sdo->size - membuf_size(buf);
2485 int last = n <= 7;
2486 n = MIN(n, 7);
2487
2488 co_unsigned8_t cs = seqno;
2489 if (last)
2490 cs |= CO_SDO_SEQ_LAST;
2491
2492 struct can_msg msg;
2493 co_csdo_init_seg_req(sdo, &msg, cs);
2494 memcpy(msg.data + 1, buf->cur, n);
2495 buf->cur += n;
2496 can_net_send(sdo->net, &msg);
2497}
2498
2499static void
2501{
2502 assert(sdo);
2503 struct membuf *buf = &sdo->dn_buf;
2504
2505 // Compute the number of bytes in the last segment containing data.
2506 co_unsigned8_t n = sdo->size ? (sdo->size - 1) % 7 + 1 : 0;
2507
2508 co_unsigned8_t cs = CO_SDO_CCS_BLK_DN_REQ | CO_SDO_SC_END_BLK
2510
2511 co_unsigned16_t crc = sdo->crc
2512 ? co_crc(0, (uint_least8_t *)buf->begin, sdo->size)
2513 : 0;
2514
2515 struct can_msg msg;
2516 co_csdo_init_seg_req(sdo, &msg, cs);
2517 stle_u16(msg.data + 1, crc);
2518 can_net_send(sdo->net, &msg);
2519}
2520
2521static void
2523{
2524 assert(sdo);
2525
2526 co_unsigned8_t cs = CO_SDO_CCS_BLK_UP_REQ | CO_SDO_BLK_CRC
2528
2529 struct can_msg msg;
2530 co_csdo_init_ini_req(sdo, &msg, cs);
2531 msg.data[4] = sdo->blksize;
2532 msg.data[5] = pst;
2533 can_net_send(sdo->net, &msg);
2534}
2535
2536static void
2538{
2539 assert(sdo);
2540
2541 co_unsigned8_t cs = CO_SDO_CCS_BLK_UP_REQ | CO_SDO_SC_START_UP;
2542
2543 struct can_msg msg;
2544 co_csdo_init_seg_req(sdo, &msg, cs);
2545 can_net_send(sdo->net, &msg);
2546
2547 if (sdo->size && sdo->up_ind)
2548 sdo->up_ind(sdo, sdo->idx, sdo->subidx, sdo->size, 0,
2549 sdo->up_ind_data);
2550}
2551
2552static void
2554{
2555 assert(sdo);
2556 struct membuf *buf = sdo->up_buf;
2557 assert(buf);
2558
2559 co_unsigned8_t cs = CO_SDO_CCS_BLK_UP_REQ | CO_SDO_SC_BLK_RES;
2560
2561 struct can_msg msg;
2562 co_csdo_init_seg_req(sdo, &msg, cs);
2563 msg.data[1] = sdo->ackseq;
2564 msg.data[2] = sdo->blksize;
2565 can_net_send(sdo->net, &msg);
2566
2567 if (sdo->size && sdo->up_ind)
2568 sdo->up_ind(sdo, sdo->idx, sdo->subidx, sdo->size,
2569 membuf_size(buf), sdo->up_ind_data);
2570}
2571
2572static void
2574{
2575 assert(sdo);
2576
2577 co_unsigned8_t cs = CO_SDO_CCS_BLK_UP_REQ | CO_SDO_SC_END_BLK;
2578
2579 struct can_msg msg;
2580 co_csdo_init_seg_req(sdo, &msg, cs);
2581 can_net_send(sdo->net, &msg);
2582}
2583
2584static void
2585co_csdo_init_ini_req(co_csdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
2586{
2587 assert(sdo);
2588 assert(msg);
2589
2590 *msg = (struct can_msg)CAN_MSG_INIT;
2591 msg->id = sdo->par.cobid_req;
2592 if (sdo->par.cobid_req & CO_SDO_COBID_FRAME) {
2593 msg->id &= CAN_MASK_EID;
2594 msg->flags |= CAN_FLAG_IDE;
2595 } else {
2596 msg->id &= CAN_MASK_BID;
2597 }
2598 msg->len = CAN_MAX_LEN;
2599 msg->data[0] = cs;
2600 stle_u16(msg->data + 1, sdo->idx);
2601 msg->data[3] = sdo->subidx;
2602}
2603
2604static void
2605co_csdo_init_seg_req(co_csdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
2606{
2607 assert(sdo);
2608 assert(msg);
2609
2610 *msg = (struct can_msg)CAN_MSG_INIT;
2611 msg->id = sdo->par.cobid_req;
2612 if (sdo->par.cobid_req & CO_SDO_COBID_FRAME) {
2613 msg->id &= CAN_MASK_EID;
2614 msg->flags |= CAN_FLAG_IDE;
2615 } else {
2616 msg->id &= CAN_MASK_BID;
2617 }
2618 msg->len = CAN_MAX_LEN;
2619 msg->data[0] = cs;
2620}
2621
2622static void
2623co_csdo_dn_dcf_dn_con(co_csdo_t *sdo, co_unsigned16_t idx,
2624 co_unsigned8_t subidx, co_unsigned32_t ac, void *data)
2625{
2626 assert(sdo);
2627 assert(co_csdo_is_valid(sdo));
2628 assert(co_csdo_is_idle(sdo));
2629 struct co_csdo_dn_dcf *dcf = &sdo->dn_dcf;
2630
2631 if (!ac && dcf->n--) {
2632 idx = 0;
2633 subidx = 0;
2635 // Read the object index.
2636 // clang-format off
2637 if (co_val_read(CO_DEFTYPE_UNSIGNED16, &idx, dcf->begin,
2638 dcf->end) != 2)
2639 // clang-format on
2640 goto done;
2641 dcf->begin += 2;
2642 // Read the object sub-index.
2643 // clang-format off
2644 if (co_val_read(CO_DEFTYPE_UNSIGNED8, &subidx, dcf->begin,
2645 dcf->end) != 1)
2646 // clang-format on
2647 goto done;
2648 dcf->begin += 1;
2649 // Read the value size (in bytes).
2650 co_unsigned32_t size;
2651 // clang-format off
2652 if (co_val_read(CO_DEFTYPE_UNSIGNED32, &size, dcf->begin,
2653 dcf->end) != 4)
2654 // clang-format on
2655 goto done;
2656 dcf->begin += 4;
2657 if (dcf->end - dcf->begin < (ptrdiff_t)size)
2658 goto done;
2659 const void *ptr = dcf->begin;
2660 dcf->begin += size;
2661 // Submit the SDO download request. This cannot fail since we
2662 // already checked that the SDO exists, is valid and is idle.
2663 co_csdo_dn_req(sdo, idx, subidx, ptr, size,
2664 &co_csdo_dn_dcf_dn_con, NULL);
2665 return;
2666 }
2667
2668done:;
2669 co_csdo_dn_con_t *con = dcf->con;
2670 data = dcf->data;
2671
2672 *dcf = (struct co_csdo_dn_dcf){ 0 };
2673
2674 if (con)
2675 con(sdo, idx, subidx, ac, data);
2676}
2677
2678#endif // !LELY_NO_CO_CSDO
@ CAN_FLAG_IDE
The Identifier Extension (IDE) flag.
Definition: msg.h:43
@ CAN_FLAG_RTR
The Remote Transmission Request (RTR) flag (unavailable in CAN FD format frames).
Definition: msg.h:48
#define CAN_MASK_EID
The mask used to extract the 29-bit Extended Identifier from a CAN frame.
Definition: msg.h:34
#define CAN_MAX_LEN
The maximum number of bytes in the payload of a CAN format frame.
Definition: msg.h:72
#define CAN_MASK_BID
The mask used to extract the 11-bit Base Identifier from a CAN frame.
Definition: msg.h:31
#define CAN_MSG_INIT
The static initializer for can_msg.
Definition: msg.h:113
This header file is part of the CANopen library; it contains the device description declarations.
co_obj_t * co_dev_find_obj(const co_dev_t *dev, co_unsigned16_t idx)
Finds an object in the object dictionary of a CANopen device.
Definition: dev.c:279
#define CO_NUM_NODES
The maximum number of nodes in a CANopen network.
Definition: dev.h:56
This header file is part of the CANopen library; it contains the Cyclic Redundancy Check (CRC) declar...
uint_least16_t co_crc(uint_least16_t crc, const uint_least8_t *bp, size_t n)
Computes a CRC-16 checksum.
Definition: crc.c:28
static co_unsigned32_t co_1280_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
The download indication function for (all sub-objects of) CANopen objects 1280..12FF (SDO client para...
Definition: csdo.c:1403
static int co_csdo_recv(const struct can_msg *msg, void *data)
The CAN receive callback function for a Client-SDO service.
Definition: csdo.c:1489
static void co_csdo_emit_recv(co_csdo_t *sdo, const struct can_msg *msg)
Invokes the 'CAN frame received' transition function of the current state of a Client-SDO service.
Definition: csdo.c:1560
static void co_csdo_send_dn_ini_req(co_csdo_t *sdo)
Sends a Client-SDO 'download initiate' request.
Definition: csdo.c:2396
static void co_csdo_init_seg_req(co_csdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
Initializes a Client-SDO download/upload segment request CAN frame.
Definition: csdo.c:2605
int co_csdo_up_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, co_csdo_up_con_t *con, void *data)
Submits an upload request to a remote Server-SDO.
Definition: csdo.c:1294
static void co_csdo_send_up_ini_req(co_csdo_t *sdo)
Sends a Client-SDO 'upload initiate' request.
Definition: csdo.c:2439
int co_dev_up_req(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx, co_csdo_up_con_t *con, void *data)
Submits an upload request to a local device.
Definition: csdo.c:812
void co_csdo_stop(co_csdo_t *sdo)
Stops a Client-SDO service.
Definition: csdo.c:1069
static co_csdo_state_t * co_csdo_up_seg_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'upload segment' state.
Definition: csdo.c:1845
static co_csdo_state_t * co_csdo_blk_up_end_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block upload end' state.
Definition: csdo.c:2237
static int co_csdo_timer(const struct timespec *tp, void *data)
The CAN timer callback function for a Client-SDO service.
Definition: csdo.c:1511
static void co_csdo_dn_dcf_dn_con(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, co_unsigned32_t ac, void *data)
The confirmation function of a single SDO download request during a concise DCF download.
Definition: csdo.c:2623
static co_csdo_state_t * co_csdo_blk_up_ini_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block upload initiate' state.
Definition: csdo.c:2099
static co_csdo_state_t * co_csdo_dn_seg_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'download segment' state.
Definition: csdo.c:1723
int co_csdo_is_idle(const co_csdo_t *sdo)
Returns 1 if the specified Client-SDO service is idle, and 0 if a transfer is ongoing.
Definition: csdo.c:1203
static co_csdo_state_t *const co_csdo_blk_up_end_state
The 'block upload end' state.
Definition: csdo.c:522
static co_csdo_state_t * co_csdo_up_seg_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'upload segment' state.
Definition: csdo.c:1831
static co_csdo_state_t * co_csdo_blk_up_end_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block upload end' state.
Definition: csdo.c:2229
static co_csdo_state_t * co_csdo_up_seg_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'upload segment' state.
Definition: csdo.c:1837
static co_csdo_state_t *const co_csdo_blk_dn_end_state
The 'block download end' state.
Definition: csdo.c:451
static co_csdo_state_t * co_csdo_blk_dn_end_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block download end' state.
Definition: csdo.c:2050
static void co_csdo_send_up_seg_req(co_csdo_t *sdo)
Sends a Client-SDO 'upload segment' request.
Definition: csdo.c:2451
int co_csdo_is_stopped(const co_csdo_t *sdo)
Retuns 1 if the specified Client-SDO service is stopped, and 0 if not.
Definition: csdo.c:1094
int co_csdo_dn_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, const void *ptr, size_t n, co_csdo_dn_con_t *con, void *data)
Submits a download request to a remote Server-SDO.
Definition: csdo.c:1219
int co_dev_dn_val_req(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx, co_unsigned16_t type, const void *val, co_csdo_dn_con_t *con, void *data)
Submits a download request to a local device.
Definition: csdo.c:701
static co_csdo_state_t * co_csdo_abort_ind(co_csdo_t *sdo, co_unsigned32_t ac)
Processes an abort transfer indication by aborting any ongoing transfer of a Client-SDO and returning...
Definition: csdo.c:2283
static co_csdo_state_t *const co_csdo_blk_up_ini_state
The 'block upload initiate' state.
Definition: csdo.c:475
static int co_csdo_update(co_csdo_t *sdo)
Updates and (de)activates a Client-SDO service.
Definition: csdo.c:1378
int co_dev_dn_req(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx, const void *ptr, size_t n, co_csdo_dn_con_t *con, void *data)
Submits a download request to a local device.
Definition: csdo.c:664
static co_csdo_state_t * co_csdo_wait_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'waiting' state.
Definition: csdo.c:1588
static co_csdo_state_t * co_csdo_dn_ini_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'download initiate' state.
Definition: csdo.c:1650
static co_csdo_state_t * co_csdo_blk_dn_ini_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'block download initiate' state.
Definition: csdo.c:1900
int co_dev_dn_dcf_req(co_dev_t *dev, const uint_least8_t *begin, const uint_least8_t *end, co_csdo_dn_con_t *con, void *data)
Submits a series of download requests to a local device.
Definition: csdo.c:739
static co_csdo_state_t * co_csdo_blk_up_sub_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block upload sub-block' state.
Definition: csdo.c:2176
static co_csdo_state_t * co_csdo_blk_up_sub_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block upload sub-block' state.
Definition: csdo.c:2168
can_net_t * co_csdo_get_net(const co_csdo_t *sdo)
Returns a pointer to the CAN network of a Client-SDO.
Definition: csdo.c:1102
static void co_csdo_enter(co_csdo_t *sdo, co_csdo_state_t *next)
Enters the specified state of a Client-SDO service and invokes the exit and entry functions.
Definition: csdo.c:1523
co_dev_t * co_csdo_get_dev(const co_csdo_t *sdo)
Returns a pointer to the CANopen device of a Client-SDO.
Definition: csdo.c:1110
static co_csdo_state_t *const co_csdo_stopped_state
The 'stopped' state.
Definition: csdo.c:252
static co_csdo_state_t * co_csdo_stopped_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'stopped' state.
Definition: csdo.c:1570
static void co_csdo_send_blk_up_sub_res(co_csdo_t *sdo)
Sends a Client-SDO 'block upload sub-block' response.
Definition: csdo.c:2553
void co_csdo_set_timeout(co_csdo_t *sdo, int timeout)
Sets the timeout of a Client-SDO.
Definition: csdo.c:1142
static void co_csdo_send_blk_up_ini_req(co_csdo_t *sdo, co_unsigned8_t pst)
Sends a Client-SDO 'block upload initiate' request.
Definition: csdo.c:2522
static co_csdo_state_t * co_csdo_abort_res(co_csdo_t *sdo, co_unsigned32_t ac)
Sends an abort transfer request and aborts any ongoing transfer by invoking co_csdo_abort_ind().
Definition: csdo.c:2292
int co_csdo_get_timeout(const co_csdo_t *sdo)
Returns the timeout (in milliseconds) of a Client-SDO.
Definition: csdo.c:1134
void co_csdo_destroy(co_csdo_t *csdo)
Destroys a CANopen Client-SDO service.
Definition: csdo.c:1024
static void co_csdo_send_blk_up_end_res(co_csdo_t *sdo)
Sends a Client-SDO 'block upload end' response.
Definition: csdo.c:2573
int co_csdo_start(co_csdo_t *sdo)
Starts a Client-SDO service.
Definition: csdo.c:1034
static co_csdo_state_t *const co_csdo_dn_seg_state
The 'download segment' state.
Definition: csdo.c:333
int co_csdo_blk_up_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, co_unsigned8_t pst, co_csdo_up_con_t *con, void *data)
Submits a block upload request to a remote Server-SDO.
Definition: csdo.c:1356
static co_csdo_state_t *const co_csdo_abort_state
The 'abort transfer' state.
Definition: csdo.c:282
int co_csdo_blk_dn_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, const void *ptr, size_t n, co_csdo_dn_con_t *con, void *data)
Submits a block download request to a remote Server-SDO.
Definition: csdo.c:1313
int co_csdo_blk_dn_val_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, co_unsigned16_t type, const void *val, co_csdo_dn_con_t *con, void *data)
Submits a block download request to a remote Server-SDO.
Definition: csdo.c:1332
void co_csdo_set_up_ind(co_csdo_t *sdo, co_csdo_ind_t *ind, void *data)
Sets the indication function used to notify the user of the progress of the current SDO upload reques...
Definition: csdo.c:1184
static co_csdo_state_t * co_csdo_blk_up_sub_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'block upload sub-block' state.
Definition: csdo.c:2162
static void co_csdo_emit_time(co_csdo_t *sdo, const struct timespec *tp)
Invokes the 'timeout' transition function of the current state of a Client-SDO service.
Definition: csdo.c:1550
static co_csdo_state_t *const co_csdo_up_ini_state
The 'upload initiate' state.
Definition: csdo.c:354
static co_csdo_state_t * co_csdo_abort_on_enter(co_csdo_t *sdo)
The entry function of the 'abort transfer' state.
Definition: csdo.c:1607
static void co_csdo_init_ini_req(co_csdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
Initializes a Client-SDO download/upload initiate request CAN frame.
Definition: csdo.c:2585
static co_csdo_state_t * co_csdo_dn_ini_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'download initiate' state.
Definition: csdo.c:1658
void co_csdo_abort_req(co_csdo_t *sdo, co_unsigned32_t ac)
Submits an abort transfer request to a remote Server-SDO.
Definition: csdo.c:1211
static co_csdo_state_t * co_csdo_dn_ini_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'download initiate' state.
Definition: csdo.c:1644
static co_csdo_state_t * co_csdo_dn_seg_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'download segment' state.
Definition: csdo.c:1709
int co_csdo_dn_dcf_req(co_csdo_t *sdo, const uint_least8_t *begin, const uint_least8_t *end, co_csdo_dn_con_t *con, void *data)
Submits a series of download requests to a remote Server-SDO.
Definition: csdo.c:1265
static co_csdo_state_t * co_csdo_blk_dn_end_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block download end' state.
Definition: csdo.c:2058
static co_csdo_state_t *const co_csdo_wait_state
The 'waiting' state.
Definition: csdo.c:268
void co_csdo_get_up_ind(const co_csdo_t *sdo, co_csdo_ind_t **pind, void **pdata)
Retrieves the indication function used to notify the user of the progress of the current SDO upload r...
Definition: csdo.c:1173
static void co_csdo_send_blk_dn_end_req(co_csdo_t *sdo)
Sends a Client-SDO 'block download end' request.
Definition: csdo.c:2500
static co_csdo_state_t * co_csdo_dn_seg_on_enter(co_csdo_t *sdo)
The entry function of the 'download segment' state.
Definition: csdo.c:1689
const struct co_sdo_par * co_csdo_get_par(const co_csdo_t *sdo)
Returns a pointer to the SDO parameter record of a Client-SDO.
Definition: csdo.c:1126
static co_csdo_state_t *const co_csdo_blk_dn_ini_state
The 'block download initiate' state.
Definition: csdo.c:399
static int co_csdo_up_ind(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, struct membuf *buf, co_csdo_up_con_t *con, void *data)
Processes an upload request from a Client-SDO by checking and updating the state.
Definition: csdo.c:2334
static co_csdo_state_t * co_csdo_blk_dn_ini_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block download initiate' state.
Definition: csdo.c:1906
static void co_csdo_emit_abort(co_csdo_t *sdo, co_unsigned32_t ac)
Invokes the 'abort' transition function of the current state of a Client-SDO service.
Definition: csdo.c:1540
static co_csdo_state_t *const co_csdo_blk_up_sub_state
The 'block upload sub-block' state.
Definition: csdo.c:499
static co_csdo_state_t * co_csdo_blk_up_ini_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block upload initiate' state.
Definition: csdo.c:2091
static co_csdo_state_t * co_csdo_blk_up_end_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'block upload end' state.
Definition: csdo.c:2223
static void co_csdo_send_start_up_req(co_csdo_t *sdo)
Sends a Client-SDO 'start upload' request.
Definition: csdo.c:2537
static void co_csdo_send_dn_seg_req(co_csdo_t *sdo, co_unsigned32_t n, int last)
Sends a Client-SDO 'download segment' request.
Definition: csdo.c:2414
static co_csdo_state_t * co_csdo_wait_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'waiting' state.
Definition: csdo.c:1579
static co_csdo_state_t * co_csdo_blk_dn_sub_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block download sub-block' state.
Definition: csdo.c:1990
static co_csdo_state_t * co_csdo_up_ini_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'upload initiate' state.
Definition: csdo.c:1764
void co_csdo_set_dn_ind(co_csdo_t *sdo, co_csdo_ind_t *ind, void *data)
Sets the indication function used to notify the user of the progress of the current SDO download requ...
Definition: csdo.c:1164
static int co_csdo_dn_ind(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, const void *ptr, size_t n, co_csdo_dn_con_t *con, void *data)
Processes a download request from a Client-SDO by checking and updating the state and copying the val...
Definition: csdo.c:2299
static void co_csdo_send_blk_dn_ini_req(co_csdo_t *sdo)
Sends a Client-SDO 'block download initiate' request.
Definition: csdo.c:2464
co_csdo_t * co_csdo_create(can_net_t *net, co_dev_t *dev, co_unsigned8_t num)
Creates a new CANopen Client-SDO service.
Definition: csdo.c:997
static co_csdo_state_t * co_csdo_up_ini_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'upload initiate' state.
Definition: csdo.c:1756
static co_csdo_state_t *const co_csdo_blk_dn_sub_state
The 'block download sub-block' state.
Definition: csdo.c:427
co_unsigned8_t co_csdo_get_num(const co_csdo_t *sdo)
Returns the SDO number of a Client-SDO.
Definition: csdo.c:1118
static co_csdo_state_t * co_csdo_blk_dn_sub_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'block download sub-block' state.
Definition: csdo.c:1984
static co_csdo_state_t *const co_csdo_dn_ini_state
The 'download initiate' state.
Definition: csdo.c:306
int co_csdo_is_valid(const co_csdo_t *sdo)
Returns 1 of the COB-IDs of the specified Client-SDO service are valid, and 0 if not.
Definition: csdo.c:1193
void co_csdo_get_dn_ind(const co_csdo_t *sdo, co_csdo_ind_t **pind, void **pdata)
Retrieves the indication function used to notify the user of the progress of the current SDO download...
Definition: csdo.c:1153
static void co_csdo_send_dn_exp_req(co_csdo_t *sdo)
Sends a Client-SDO 'download initiate' (expedited) request.
Definition: csdo.c:2379
int co_csdo_dn_val_req(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, co_unsigned16_t type, const void *val, co_csdo_dn_con_t *con, void *data)
Submits a download request to a remote Server-SDO.
Definition: csdo.c:1241
static void co_csdo_abort_on_leave(co_csdo_t *sdo)
The exit function of the 'abort transfer' state.
Definition: csdo.c:1617
static void co_csdo_send_abort(co_csdo_t *sdo, co_unsigned32_t ac)
Sends an abort transfer request.
Definition: csdo.c:2368
static co_csdo_state_t * co_csdo_blk_up_ini_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'block upload initiate' state.
Definition: csdo.c:2085
static void co_csdo_send_blk_dn_sub_req(co_csdo_t *sdo, co_unsigned8_t seqno)
Sends a Client-SDO 'block download sub-block' request.
Definition: csdo.c:2478
static co_csdo_state_t * co_csdo_blk_dn_sub_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block download sub-block' state.
Definition: csdo.c:1998
static co_csdo_state_t *const co_csdo_up_seg_state
The 'upload segment' state.
Definition: csdo.c:375
static co_csdo_state_t * co_csdo_up_ini_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'upload initiate' state.
Definition: csdo.c:1750
static co_csdo_state_t * co_csdo_dn_seg_on_time(co_csdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'download segment' state.
Definition: csdo.c:1715
static co_csdo_state_t * co_csdo_blk_dn_sub_on_enter(co_csdo_t *sdo)
The entry function of the 'block download sub-block' state.
Definition: csdo.c:1957
static co_csdo_state_t * co_csdo_blk_dn_ini_on_recv(co_csdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block download initiate' state.
Definition: csdo.c:1914
static co_csdo_state_t * co_csdo_blk_dn_end_on_abort(co_csdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'block download end' state.
Definition: csdo.c:2044
This header file is part of the CANopen library; it contains the Client-SDO declarations.
void co_csdo_up_con_t(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, co_unsigned32_t ac, const void *ptr, size_t n, void *data)
The type of a CANopen Client-SDO upload confirmation callback function, invoked when an upload reques...
Definition: csdo.h:59
void co_csdo_dn_con_t(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, co_unsigned32_t ac, void *data)
The type of a CANopen Client-SDO download confirmation callback function, invoked when a download req...
Definition: csdo.h:43
void co_csdo_ind_t(const co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, size_t size, size_t nbyte, void *data)
The type of a CANopen Client-SDO request progress indication function, used to notify the user of the...
Definition: csdo.h:79
This header file is part of the utilities library; it contains the byte order (endianness) function d...
uint_least32_t ldle_u32(const uint_least8_t src[4])
Loads a 32-bit unsigned integer in little-endian byte order.
Definition: endian.h:596
uint_least16_t ldle_u16(const uint_least8_t src[2])
Loads a 16-bit unsigned integer in little-endian byte order.
Definition: endian.h:516
void stle_u16(uint_least8_t dst[2], uint_least16_t x)
Stores a 16-bit unsigned integer in little-endian byte order.
Definition: endian.h:504
void stle_u32(uint_least8_t dst[4], uint_least32_t x)
Stores a 32-bit unsigned integer in little-endian byte order.
Definition: endian.h:582
This header file is part of the utilities library; it contains the native and platform-independent er...
int errnum2c(errnum_t errnum)
Transforms a platform-independent error number to a native error code.
Definition: errnum.c:810
@ 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
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:46
void set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
Definition: errnum.h:424
#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
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
#define CO_SDO_AC_TOGGLE
SDO abort code: Toggle bit not altered.
Definition: sdo.h:63
#define CO_SDO_REQ_INIT
The static initializer for struct co_sdo_req.
Definition: sdo.h:206
#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
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
#define CO_SDO_AC_NO_CS
SDO abort code: Client/server command specifier not valid or unknown.
Definition: sdo.h:69
void co_sdo_req_fini(struct co_sdo_req *req)
Finalizes a CANopen SDO upload/download request.
Definition: sdo.c:121
#define CO_SDO_AC_BLK_SEQ
SDO abort code: Invalid sequence number (block mode only).
Definition: sdo.h:75
#define CO_SDO_AC_ERROR
SDO abort code: General error.
Definition: sdo.h:150
#define CO_SDO_AC_NO_SDO
SDO abort code: Resource not available: SDO connection.
Definition: sdo.h:147
#define CO_SDO_AC_NO_OBJ
SDO abort code: Object does not exist in the object dictionary.
Definition: sdo.h:93
#define CO_SDO_COBID_FRAME
The bit in the SDO COB-ID specifying whether to use an 11-bit (0) or 29-bit (1) CAN-ID.
Definition: sdo.h:39
void co_sdo_req_clear(struct co_sdo_req *req)
Clears a CANopen SDO upload/download request, including its buffer.
Definition: sdo.c:129
#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_COBID_VALID
The bit in the SDO COB-ID specifying whether the SDO exists and is valid.
Definition: sdo.h:33
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_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_NUM_SDOS
The maximum number of Client/Server-SDOs.
Definition: sdo.h:178
#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 MIN(a, b)
Returns the minimum of a and b.
Definition: util.h:57
#define MAX(a, b)
Returns the maximum of a and b.
Definition: util.h:65
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
#define MEMBUF_INIT
The static initializer for struct membuf.
Definition: membuf.h:46
size_t membuf_size(const struct membuf *buf)
Returns the total number of bytes written to a memory buffer.
Definition: membuf.h:177
void can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
Definition: net.c:462
int can_net_send(can_net_t *net, const struct can_msg *msg)
Sends a CAN frame from a network interface.
Definition: net.c:300
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:376
void can_timer_set_func(can_timer_t *timer, can_timer_func_t *func, void *data)
Sets the callback function invoked when a CAN timer is triggered.
Definition: net.c:422
void can_recv_stop(can_recv_t *recv)
Stops a CAN frame receiver from processing frames and unregisters it with the network interface.
Definition: net.c:609
void can_recv_set_func(can_recv_t *recv, can_recv_func_t *func, void *data)
Sets the callback function used to process CAN frames with a receiver.
Definition: net.c:578
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:558
void can_timer_timeout(can_timer_t *timer, can_net_t *net, int timeout)
Starts a CAN timer and registers it with a network interface.
Definition: net.c:478
void can_recv_start(can_recv_t *recv, can_net_t *net, uint_least32_t id, uint_least8_t flags)
Registers a CAN frame receiver with a network interface and starts processing frames.
Definition: net.c:587
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:533
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:401
This header file is part of the CANopen library; it contains the object dictionary declarations.
co_unsigned8_t co_sub_get_subidx(const co_sub_t *sub)
Returns the sub-index of a CANopen sub-object.
Definition: obj.c:559
co_unsigned32_t co_sub_dn_ind(co_sub_t *sub, struct co_sdo_req *req)
Invokes the download indication function of a CANopen sub-object, registered with co_sub_set_dn_ind()...
Definition: obj.c:958
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
Definition: obj.c:164
co_unsigned32_t co_sub_up_ind(const co_sub_t *sub, struct co_sdo_req *req)
Invokes the upload indication function of a CANopen sub-object, registered with co_sub_set_up_ind().
Definition: obj.c:1066
co_sub_t * co_obj_find_sub(const co_obj_t *obj, co_unsigned8_t subidx)
Finds a sub-object in a CANopen object.
Definition: obj.c:240
int co_sub_dn(co_sub_t *sub, void *val)
Downloads (moves) a value into a CANopen sub-object if the refuse-write-on-download flag (CO_OBJ_FLAG...
Definition: obj.c:996
co_unsigned8_t co_obj_get_code(const co_obj_t *obj)
Returns the object code of a CANopen object.
Definition: obj.c:303
#define CO_OBJECT_ARRAY
A multiple data field object where each data field is a simple variable of the same basic data type.
Definition: obj.h:48
void co_obj_set_dn_ind(co_obj_t *obj, co_sub_dn_ind_t *ind, void *data)
Sets the download indication function for a CANopen object.
Definition: obj.c:389
co_obj_t * co_sub_get_obj(const co_sub_t *sub)
Returns the a pointer to the CANopen object containing the specified sub-object.
Definition: obj.c:551
co_unsigned16_t co_sub_get_type(const co_sub_t *sub)
Returns the data type of a CANopen sub-object.
Definition: obj.c:603
This is the internal header file of the CANopen library.
This is the internal header file of the Service Data Object (SDO) declarations.
#define CO_SDO_SEQ_LAST
The mask to get/set the last segment bit from an SDO command byte.
Definition: sdo.h:151
#define CO_SDO_SEG_TOGGLE
The mask to get/set the toggle bit from an SDO command byte.
Definition: sdo.h:117
#define CO_SDO_CCS_DN_INI_REQ
The SDO client command specifier 'download initiate' request.
Definition: sdo.h:55
#define CO_SDO_BLK_SIZE_IND
The SDO size indicator flag indicating that the data set size is indicated.
Definition: sdo.h:145
#define CO_SDO_CS_MASK
The mask to extract the command specifier (CS) from an SDO command byte.
Definition: sdo.h:31
#define CO_SDO_BLK_SIZE_SET(n)
Sets the SDO size indicator, indicating n bytes contain segment data (in the range [0....
Definition: sdo.h:173
#define CO_SDO_INI_SIZE_EXP_GET(cs)
Retrieves the number of bytes containing expedited data from the SDO size indicator.
Definition: sdo.h:105
#define CO_SDO_SCS_UP_SEG_RES
The SDO server command specifier 'upload segment' response.
Definition: sdo.h:46
#define CO_SDO_SEG_SIZE_SET(n)
Sets the SDO size indicator, indicating n bytes contain segment data (in the range [0....
Definition: sdo.h:139
#define CO_SDO_SC_END_BLK
The SDO server/client subcommand 'end block download/upload'.
Definition: sdo.h:79
#define CO_SDO_SCS_UP_INI_RES
The SDO server command specifier 'upload initiate' response.
Definition: sdo.h:43
#define CO_SDO_SCS_DN_SEG_RES
The SDO server command specifier 'download segment' response.
Definition: sdo.h:40
#define CO_SDO_CS_ABORT
The SDO client/server command specifier 'abort transfer' request.
Definition: sdo.h:34
#define CO_SDO_CCS_BLK_DN_REQ
The SDO client command specifier 'block download' request.
Definition: sdo.h:67
#define CO_SDO_CCS_UP_INI_REQ
The SDO client command specifier 'upload initiate' request.
Definition: sdo.h:61
#define CO_SDO_SC_MASK
The mask to extract the subcommand (SC) from an SDO command byte.
Definition: sdo.h:73
#define CO_SDO_BLK_CRC
The SDO CRC support flag.
Definition: sdo.h:148
#define CO_SDO_CCS_DN_SEG_REQ
The SDO client command specifier 'download segment' request.
Definition: sdo.h:58
#define CO_SDO_INI_SIZE_IND
The SDO size indicator flag indicating that the data set size is indicated.
Definition: sdo.h:94
#define CO_SDO_SCS_BLK_DN_RES
The SDO server command specifier 'block download' response.
Definition: sdo.h:49
#define CO_SDO_INI_SIZE_EXP_SET(n)
Sets the SDO size indicator, indicating the expedited transfer of n bytes (in the range [1....
Definition: sdo.h:113
#define CO_SDO_SCS_BLK_UP_RES
The SDO server command specifier 'block upload' response.
Definition: sdo.h:52
#define CO_SDO_MAX_SEQNO
The maximum sequence number (or segments per block).
Definition: sdo.h:176
#define CO_SDO_SEG_SIZE_GET(cs)
Retrieves the number of bytes containing segment data from the SDO size indicator.
Definition: sdo.h:131
#define CO_SDO_SEG_LAST
The mask to get/set the last segment bit from an SDO command byte.
Definition: sdo.h:142
#define CO_SDO_BLK_SIZE_GET(cs)
Retrieves the number of bytes containing segment data from the SDO size indicator.
Definition: sdo.h:165
#define CO_SDO_SC_START_UP
The SDO client subcommand 'start upload'.
Definition: sdo.h:85
#define CO_SDO_SC_BLK_RES
The SDO client/client subcommand 'block download/upload' response.
Definition: sdo.h:82
#define CO_SDO_CCS_BLK_UP_REQ
The SDO client command specifier 'block upload' request.
Definition: sdo.h:70
#define CO_SDO_INI_SIZE_EXP
The SDO size indicator flag indicating expedited transfer.
Definition: sdo.h:97
#define CO_SDO_SCS_DN_INI_RES
The SDO server command specifier 'download initiate' response.
Definition: sdo.h:37
#define CO_SDO_CCS_UP_SEG_REQ
The SDO client command specifier 'upload segment' request.
Definition: sdo.h:64
#define CO_SDO_SC_INI_BLK
The SDO server/client subcommand 'initiate download/upload'.
Definition: sdo.h:76
This header file is part of the C11 and POSIX compatibility library; it includes <stdlib....
This header file is part of the C11 and POSIX compatibility library; it includes <string....
A CAN network interface.
Definition: net.c:37
A CAN frame receiver.
Definition: net.c:86
A CAN timer.
Definition: net.c:63
A CANopen Client-SDO state.
Definition: csdo.c:205
co_csdo_state_t *(* on_enter)(co_csdo_t *sdo)
A pointer to the function invoked when a new state is entered.
Definition: csdo.c:207
co_csdo_state_t *(* on_recv)(co_csdo_t *sdo, const struct can_msg *msg)
A pointer to the transition function invoked when a CAN frame has been received.
Definition: csdo.c:236
void(* on_leave)(co_csdo_t *sdo)
A pointer to the function invoked when the current state is left.
Definition: csdo.c:238
co_csdo_state_t *(* on_time)(co_csdo_t *sdo, const struct timespec *tp)
A pointer to the transition function invoked when a timeout occurs.
Definition: csdo.c:226
co_csdo_state_t *(* on_abort)(co_csdo_t *sdo, co_unsigned32_t ac)
A pointer to the transition function invoked when an abort code has been received.
Definition: csdo.c:217
A CANopen Client-SDO.
Definition: csdo.c:71
co_unsigned8_t ackseq
The sequence number of the last successfully received segment.
Definition: csdo.c:101
void * up_ind_data
A pointer to user-specified data for up_ind.
Definition: csdo.c:135
void * dn_con_data
A pointer to user-specified data for dn_con.
Definition: csdo.c:123
can_timer_t * timer
A pointer to the CAN timer.
Definition: csdo.c:85
co_csdo_ind_t * up_ind
A pointer to the upload progress indication function.
Definition: csdo.c:133
void * up_con_data
A pointer to user-specified data for up_con.
Definition: csdo.c:131
co_unsigned8_t subidx
The current object sub-index.
Definition: csdo.c:93
int timeout
The SDO timeout (in milliseconds).
Definition: csdo.c:83
co_unsigned16_t idx
The current object index.
Definition: csdo.c:91
co_unsigned8_t num
The SDO number.
Definition: csdo.c:77
co_csdo_up_con_t * up_con
A pointer to the upload confirmation function.
Definition: csdo.c:129
co_unsigned8_t blksize
The number of segments per block.
Definition: csdo.c:99
void * dn_ind_data
A pointer to user-specified data for dn_ind.
Definition: csdo.c:127
struct membuf * up_buf
A pointer to the memory buffer used for upload requests.
Definition: csdo.c:107
co_csdo_ind_t * dn_ind
A pointer to the download progress indication function.
Definition: csdo.c:125
co_unsigned8_t toggle
The current value of the toggle bit.
Definition: csdo.c:97
struct co_csdo_dn_dcf dn_dcf
The state of the concise DCF download request.
Definition: csdo.c:137
co_csdo_dn_con_t * dn_con
A pointer to the download confirmation function.
Definition: csdo.c:121
co_unsigned32_t size
The data set size (in bytes).
Definition: csdo.c:95
co_unsigned32_t ac
The current abort code.
Definition: csdo.c:89
struct membuf buf
The memory buffer used for storing serialized values in the absence of a user-specified buffer.
Definition: csdo.c:112
struct membuf dn_buf
The memory buffer used for download requests.
Definition: csdo.c:105
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: csdo.c:81
co_csdo_state_t * state
A pointer to the current state.
Definition: csdo.c:87
struct co_sdo_par par
The SDO parameter record.
Definition: csdo.c:79
can_net_t * net
A pointer to a CAN network interface.
Definition: csdo.c:73
co_dev_t * dev
A pointer to a CANopen device.
Definition: csdo.c:75
unsigned crc
A flag indicating whether a CRC should be generated.
Definition: csdo.c:103
A CANopen device.
Definition: dev.h:30
A CANopen object.
Definition: obj.h:31
A CANopen sub-object.
Definition: obj.h:53
A CAN or CAN FD format frame.
Definition: msg.h:87
uint_least8_t data[CAN_MSG_MAX_LEN]
The frame payload (in case of a data frame).
Definition: msg.h:102
uint_least32_t id
The identifier (11 or 29 bits, depending on the CAN_FLAG_IDE flag).
Definition: msg.h:89
uint_least8_t flags
The flags (any combination of CAN_FLAG_IDE, CAN_FLAG_RTR, CAN_FLAG_FDF, CAN_FLAG_BRS and CAN_FLAG_ESI...
Definition: msg.h:94
uint_least8_t len
The number of bytes in data (or the requested number of bytes in case of a remote frame).
Definition: msg.h:100
The state of a concise DCF download request.
Definition: csdo.c:57
const uint_least8_t * end
A pointer to one past the last byte in the concise DCF.
Definition: csdo.c:63
void * data
A pointer to user-specified data for con.
Definition: csdo.c:67
co_unsigned32_t n
The number of remaining entries in the concise DCF.
Definition: csdo.c:59
co_csdo_dn_con_t * con
A pointer to the download confirmation function.
Definition: csdo.c:65
const uint_least8_t * begin
A pointer to the next byte in the concise DCF.
Definition: csdo.c:61
An SDO parameter record.
Definition: sdo.h:45
co_unsigned32_t cobid_res
COB-ID server -> client.
Definition: sdo.h:51
co_unsigned8_t n
Highest sub-index supported.
Definition: sdo.h:47
co_unsigned32_t cobid_req
COB-ID client -> server.
Definition: sdo.h:49
co_unsigned8_t id
Node-ID of SDO's client resp. server.
Definition: sdo.h:53
A CANopen SDO upload/download request.
Definition: sdo.h:181
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
size_t nbyte
The number of bytes available at buf.
Definition: sdo.h:191
A memory buffer.
Definition: membuf.h:36
char * end
A pointer to one past the last byte in the buffer.
Definition: membuf.h:42
char * begin
A pointer to the first byte in the buffer.
Definition: membuf.h:40
char * cur
A pointer to one past the last byte written to the buffer.
Definition: membuf.h:38
#define CO_DEFTYPE_UNSIGNED16
The data type (and object index) of a 16-bit unsigned integer.
Definition: type.h:47
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
#define CO_DEFTYPE_UNSIGNED8
The data type (and object index) of an 8-bit unsigned integer.
Definition: type.h:44
#define CO_DEFTYPE_UNSIGNED32
The data type (and object index) of a 32-bit unsigned integer.
Definition: type.h:50
A union of the CANopen static data types.
Definition: val.h:273
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
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
size_t co_val_sizeof(co_unsigned16_t type, const void *val)
Returns the size (in bytes) of a value of the specified data type.
Definition: val.c:269