Lely core libraries  2.3.4
ssdo.c
Go to the documentation of this file.
1 
24 #include "co.h"
25 #include "sdo.h"
26 #include <lely/co/crc.h>
27 #include <lely/co/dev.h>
28 #include <lely/co/obj.h>
29 #include <lely/co/ssdo.h>
30 #include <lely/co/val.h>
31 #include <lely/util/endian.h>
32 #include <lely/util/errnum.h>
33 
34 #include <assert.h>
35 #if !LELY_NO_STDIO
36 #include <inttypes.h>
37 #endif
38 #include <stdlib.h>
39 #include <string.h>
40 
41 #if LELY_NO_MALLOC
42 #ifndef CO_SSDO_MEMBUF_SIZE
48 #define CO_SSDO_MEMBUF_SIZE (CO_SDO_MAX_SEQNO * 7)
49 #endif
50 #endif
51 
52 #if LELY_NO_MALLOC
53 #define CO_SSDO_MAX_SEQNO MIN(CO_SDO_MAX_SEQNO, (CO_SSDO_MEMBUF_SIZE / 7))
54 #else
55 #define CO_SSDO_MAX_SEQNO CO_SDO_MAX_SEQNO
56 #endif
57 
58 struct __co_ssdo_state;
60 typedef const struct __co_ssdo_state co_ssdo_state_t;
61 
63 struct __co_ssdo {
69  co_unsigned8_t num;
71  struct co_sdo_par par;
75  int timeout;
81  co_unsigned16_t idx;
83  co_unsigned8_t subidx;
85  co_unsigned8_t toggle;
87  co_unsigned8_t blksize;
89  co_unsigned8_t ackseq;
91  unsigned gencrc : 1;
93  co_unsigned16_t crc;
95  struct co_sdo_req req;
97  struct membuf buf;
99  size_t nbyte;
100 #if LELY_NO_MALLOC
105  char begin[CO_SSDO_MEMBUF_SIZE];
106 #endif
107 };
108 
115 static int co_ssdo_update(co_ssdo_t *sdo);
116 
123 static co_unsigned32_t co_1200_dn_ind(
124  co_sub_t *sub, struct co_sdo_req *req, void *data);
125 
131 static int co_ssdo_recv(const struct can_msg *msg, void *data);
132 
138 static int co_ssdo_timer(const struct timespec *tp, void *data);
139 
141 static inline void co_ssdo_enter(co_ssdo_t *sdo, co_ssdo_state_t *next);
142 
150 static inline void co_ssdo_emit_abort(co_ssdo_t *sdo, co_unsigned32_t ac);
151 
159 static inline void co_ssdo_emit_time(co_ssdo_t *sdo, const struct timespec *tp);
160 
168 static inline void co_ssdo_emit_recv(co_ssdo_t *sdo, const struct can_msg *msg);
169 
181  co_ssdo_state_t *(*on_abort)(co_ssdo_t *sdo, co_unsigned32_t ac);
190  co_ssdo_state_t *(*on_time)(co_ssdo_t *sdo, const struct timespec *tp);
200  co_ssdo_state_t *(*on_recv)(co_ssdo_t *sdo, const struct can_msg *msg);
201 };
202 
203 #define LELY_CO_DEFINE_STATE(name, ...) \
204  static co_ssdo_state_t *const name = &(co_ssdo_state_t){ __VA_ARGS__ };
205 
207 LELY_CO_DEFINE_STATE(co_ssdo_stopped_state, .on_recv = NULL)
208 
211  co_ssdo_t *sdo, co_unsigned32_t ac);
212 
215  co_ssdo_t *sdo, const struct can_msg *msg);
216 
218 // clang-format off
219 LELY_CO_DEFINE_STATE(co_ssdo_wait_state,
220  .on_abort = &co_ssdo_wait_on_abort,
221  .on_recv = &co_ssdo_wait_on_recv
222 )
223 // clang-format on
224 
225 
230  co_ssdo_t *sdo, const struct can_msg *msg);
231 
233 // LELY_CO_DEFINE_STATE(co_ssdo_dn_ini_state,
234 // .on_recv = &co_ssdo_dn_ini_on_recv
235 //)
236 
239  co_ssdo_t *sdo, co_unsigned32_t ac);
240 
243  co_ssdo_t *sdo, const struct timespec *tp);
244 
249  co_ssdo_t *sdo, const struct can_msg *msg);
250 
252 // clang-format off
253 LELY_CO_DEFINE_STATE(co_ssdo_dn_seg_state,
254  .on_abort = &co_ssdo_dn_seg_on_abort,
255  .on_time = &co_ssdo_dn_seg_on_time,
256  .on_recv = &co_ssdo_dn_seg_on_recv
257 )
258 // clang-format on
259 
262  co_ssdo_t *sdo, const struct can_msg *msg);
263 
265 // LELY_CO_DEFINE_STATE(co_ssdo_up_ini_state,
266 // .on_recv = &co_ssdo_up_ini_on_recv
267 //)
268 
271  co_ssdo_t *sdo, co_unsigned32_t ac);
272 
275  co_ssdo_t *sdo, const struct timespec *tp);
276 
279  co_ssdo_t *sdo, const struct can_msg *msg);
280 
282 // clang-format off
283 LELY_CO_DEFINE_STATE(co_ssdo_up_seg_state,
284  .on_abort = &co_ssdo_up_seg_on_abort,
285  .on_time = &co_ssdo_up_seg_on_time,
286  .on_recv = &co_ssdo_up_seg_on_recv
287 )
288 // clang-format on
289 
295  co_ssdo_t *sdo, const struct can_msg *msg);
296 
298 // LELY_CO_DEFINE_STATE(co_ssdo_blk_dn_ini_state,
299 // .on_recv = &co_ssdo_blk_dn_ini_on_recv
300 //)
301 
304  co_ssdo_t *sdo, co_unsigned32_t ac);
305 
308  co_ssdo_t *sdo, const struct timespec *tp);
309 
315  co_ssdo_t *sdo, const struct can_msg *msg);
316 
318 // clang-format off
319 LELY_CO_DEFINE_STATE(co_ssdo_blk_dn_sub_state,
320  .on_abort = &co_ssdo_blk_dn_sub_on_abort,
321  .on_time = &co_ssdo_blk_dn_sub_on_time,
322  .on_recv = &co_ssdo_blk_dn_sub_on_recv
323 )
324 // clang-format on
325 
328  co_ssdo_t *sdo, co_unsigned32_t ac);
329 
332  co_ssdo_t *sdo, const struct timespec *tp);
333 
339  co_ssdo_t *sdo, const struct can_msg *msg);
340 
342 // clang-format off
343 LELY_CO_DEFINE_STATE(co_ssdo_blk_dn_end_state,
344  .on_abort = &co_ssdo_blk_dn_end_on_abort,
345  .on_time = &co_ssdo_blk_dn_end_on_time,
346  .on_recv = &co_ssdo_blk_dn_end_on_recv
347 )
348 // clang-format on
349 
355  co_ssdo_t *sdo, const struct can_msg *msg);
356 
358 // LELY_CO_DEFINE_STATE(co_ssdo_blk_up_ini_state,
359 // .on_recv = &co_ssdo_blk_up_ini_on_recv
360 //)
361 
364  co_ssdo_t *sdo, co_unsigned32_t ac);
365 
368  co_ssdo_t *sdo, const struct timespec *tp);
369 
375  co_ssdo_t *sdo, const struct can_msg *msg);
376 
378 // clang-format off
379 LELY_CO_DEFINE_STATE(co_ssdo_blk_up_sub_state,
380  .on_abort = &co_ssdo_blk_up_sub_on_abort,
381  .on_time = &co_ssdo_blk_up_sub_on_time,
382  .on_recv = &co_ssdo_blk_up_sub_on_recv
383 )
384 // clang-format on
385 
388  co_ssdo_t *sdo, co_unsigned32_t ac);
389 
392  co_ssdo_t *sdo, const struct timespec *tp);
393 
398  co_ssdo_t *sdo, const struct can_msg *msg);
399 
401 // clang-format off
402 LELY_CO_DEFINE_STATE(co_ssdo_blk_up_end_state,
403  .on_abort = &co_ssdo_blk_up_end_on_abort,
404  .on_time = &co_ssdo_blk_up_end_on_time,
405  .on_recv = &co_ssdo_blk_up_end_on_recv
406 )
407 // clang-format on
408 
409 #undef LELY_CO_DEFINE_STATE
410 
418 
430 static co_ssdo_state_t *co_ssdo_abort_res(co_ssdo_t *sdo, co_unsigned32_t ac);
431 
438 static co_unsigned32_t co_ssdo_dn_ind(co_ssdo_t *sdo);
439 
446 static co_unsigned32_t co_ssdo_up_ind(co_ssdo_t *sdo);
447 
454 static co_unsigned32_t co_ssdo_up_buf(co_ssdo_t *sdo, size_t nbyte);
455 
462 static void co_ssdo_send_abort(co_ssdo_t *sdo, co_unsigned32_t ac);
463 
465 static void co_ssdo_send_dn_ini_res(co_ssdo_t *sdo);
466 
468 static void co_ssdo_send_dn_seg_res(co_ssdo_t *sdo);
469 
471 static void co_ssdo_send_up_exp_res(co_ssdo_t *sdo);
472 
474 static void co_ssdo_send_up_ini_res(co_ssdo_t *sdo);
475 
482 static void co_ssdo_send_up_seg_res(co_ssdo_t *sdo, int last);
483 
485 static void co_ssdo_send_blk_dn_ini_res(co_ssdo_t *sdo);
486 
488 static void co_ssdo_send_blk_dn_sub_res(co_ssdo_t *sdo);
489 
491 static void co_ssdo_send_blk_dn_end_res(co_ssdo_t *sdo);
492 
494 static void co_ssdo_send_blk_up_ini_res(co_ssdo_t *sdo);
495 
502 static void co_ssdo_send_blk_up_sub_res(co_ssdo_t *sdo, int last);
503 
505 static void co_ssdo_send_blk_up_end_res(co_ssdo_t *sdo);
506 
514 static void co_ssdo_init_ini_res(
515  co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs);
516 
524 static void co_ssdo_init_seg_res(
525  co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs);
526 
527 void *
528 __co_ssdo_alloc(void)
529 {
530  void *ptr = malloc(sizeof(struct __co_ssdo));
531 #if !LELY_NO_ERRNO
532  if (!ptr)
533  set_errc(errno2c(errno));
534 #endif
535  return ptr;
536 }
537 
538 void
539 __co_ssdo_free(void *ptr)
540 {
541  free(ptr);
542 }
543 
544 struct __co_ssdo *
545 __co_ssdo_init(struct __co_ssdo *sdo, can_net_t *net, co_dev_t *dev,
546  co_unsigned8_t num)
547 {
548  assert(sdo);
549  assert(net);
550  assert(dev);
551 
552  int errc = 0;
553 
554  if (!num || num > CO_NUM_SDOS) {
555  errc = errnum2c(ERRNUM_INVAL);
556  goto error_param;
557  }
558 
559  // Find the SDO server parameter in the object dictionary. The default
560  // SDO (1200) is optional.
561  co_obj_t *obj_1200 = co_dev_find_obj(dev, 0x1200 + num - 1);
562  if (num != 1 && !obj_1200) {
563  errc = errnum2c(ERRNUM_INVAL);
564  goto error_param;
565  }
566 
567  sdo->net = net;
568  sdo->dev = dev;
569  sdo->num = num;
570 
571  // Initialize the SDO parameter record with the default values.
572  sdo->par.n = 3;
573  sdo->par.id = co_dev_get_id(dev);
574  sdo->par.cobid_req = 0x600 + sdo->par.id;
575  sdo->par.cobid_res = 0x580 + sdo->par.id;
576 
577  sdo->recv = can_recv_create();
578  if (!sdo->recv) {
579  errc = get_errc();
580  goto error_create_recv;
581  }
582  can_recv_set_func(sdo->recv, &co_ssdo_recv, sdo);
583 
584  sdo->timeout = 0;
585 
586  sdo->timer = can_timer_create();
587  if (!sdo->timer) {
588  errc = get_errc();
589  goto error_create_timer;
590  }
592 
594 
595  sdo->idx = 0;
596  sdo->subidx = 0;
597 
598  sdo->toggle = 0;
599  sdo->blksize = 0;
600  sdo->ackseq = 0;
601  sdo->gencrc = 0;
602  sdo->crc = 0;
603 
604  co_sdo_req_init(&sdo->req);
605 #if LELY_NO_MALLOC
606  membuf_init(&sdo->buf, sdo->begin, CO_SSDO_MEMBUF_SIZE);
607 #else
608  membuf_init(&sdo->buf, NULL, 0);
609 #endif
610  sdo->nbyte = 0;
611 #if LELY_NO_MALLOC
612  memset(sdo->begin, 0, CO_SSDO_MEMBUF_SIZE);
613 #endif
614 
615  if (co_ssdo_start(sdo) == -1) {
616  errc = get_errc();
617  goto error_start;
618  }
619 
620  return sdo;
621 
622  // co_ssdo_stop(sdo);
623 error_start:
624  can_timer_destroy(sdo->timer);
625 error_create_timer:
626  can_recv_destroy(sdo->recv);
627 error_create_recv:
628 error_param:
629  set_errc(errc);
630  return NULL;
631 }
632 
633 void
634 __co_ssdo_fini(struct __co_ssdo *sdo)
635 {
636  assert(sdo);
637  assert(sdo->num >= 1 && sdo->num <= CO_NUM_SDOS);
638 
639  co_ssdo_stop(sdo);
640 
641  membuf_fini(&sdo->buf);
642  co_sdo_req_fini(&sdo->req);
643 
644  can_timer_destroy(sdo->timer);
645 
646  can_recv_destroy(sdo->recv);
647 }
648 
649 co_ssdo_t *
651 {
652  trace("creating Server-SDO %d", num);
653 
654  int errc = 0;
655 
656  co_ssdo_t *sdo = __co_ssdo_alloc();
657  if (!sdo) {
658  errc = get_errc();
659  goto error_alloc_sdo;
660  }
661 
662  if (!__co_ssdo_init(sdo, net, dev, num)) {
663  errc = get_errc();
664  goto error_init_sdo;
665  }
666 
667  return sdo;
668 
669 error_init_sdo:
670  __co_ssdo_free(sdo);
671 error_alloc_sdo:
672  set_errc(errc);
673  return NULL;
674 }
675 
676 void
678 {
679  if (ssdo) {
680  trace("destroying Server-SDO %d", ssdo->num);
681  __co_ssdo_fini(ssdo);
682  __co_ssdo_free(ssdo);
683  }
684 }
685 
686 int
688 {
689  assert(sdo);
690 
691  if (!co_ssdo_is_stopped(sdo))
692  return 0;
693 
695 
696  co_unsigned16_t idx_1200 = 0x1200 + sdo->num - 1;
697  co_obj_t *obj_1200 = co_dev_find_obj(sdo->dev, idx_1200);
698  if (obj_1200) {
699  // Copy the SDO parameters.
700  memset(&sdo->par, 0, sizeof(sdo->par));
701  sdo->par.n = co_dev_get_val_u8(sdo->dev, idx_1200, 0);
702  sdo->par.cobid_req = co_dev_get_val_u32(sdo->dev, idx_1200, 1);
703  sdo->par.cobid_res = co_dev_get_val_u32(sdo->dev, idx_1200, 2);
704  sdo->par.id = co_dev_get_val_u8(sdo->dev, idx_1200, 3);
705  // Set the download indication function for the SDO parameter
706  // record.
707  co_obj_set_dn_ind(obj_1200, &co_1200_dn_ind, sdo);
708  }
709 
710  if (co_ssdo_update(sdo) == -1)
711  goto error_update;
712 
713  return 0;
714 
715 error_update:
716  co_ssdo_stop(sdo);
717  return -1;
718 }
719 
720 void
722 {
723  assert(sdo);
724 
725  if (co_ssdo_is_stopped(sdo))
726  return;
727 
728  // Abort any ongoing transfer.
730 
731  can_timer_stop(sdo->timer);
732  can_recv_stop(sdo->recv);
733 
734  // Remove the download indication functions for the SDO parameter
735  // record.
736  co_obj_t *obj_1200 = co_dev_find_obj(sdo->dev, 0x1200 + sdo->num - 1);
737  if (obj_1200)
738  co_obj_set_dn_ind(obj_1200, NULL, NULL);
739 
741 }
742 
743 int
745 {
746  assert(sdo);
747 
748  return sdo->state == co_ssdo_stopped_state;
749 }
750 
751 can_net_t *
753 {
754  assert(sdo);
755 
756  return sdo->net;
757 }
758 
759 co_dev_t *
761 {
762  assert(sdo);
763 
764  return sdo->dev;
765 }
766 
767 co_unsigned8_t
769 {
770  assert(sdo);
771 
772  return sdo->num;
773 }
774 
775 const struct co_sdo_par *
777 {
778  assert(sdo);
779 
780  return &sdo->par;
781 }
782 
783 int
785 {
786  assert(sdo);
787 
788  return sdo->timeout;
789 }
790 
791 void
792 co_ssdo_set_timeout(co_ssdo_t *sdo, int timeout)
793 {
794  assert(sdo);
795 
796  if (sdo->timeout && timeout <= 0)
797  can_timer_stop(sdo->timer);
798 
799  sdo->timeout = MAX(0, timeout);
800 }
801 
802 static int
804 {
805  assert(sdo);
806 
807  // Abort any ongoing transfer.
809 
810  int valid_req = !(sdo->par.cobid_req & CO_SDO_COBID_VALID);
811  int valid_res = !(sdo->par.cobid_res & CO_SDO_COBID_VALID);
812  if (valid_req && valid_res) {
813  uint_least32_t id = sdo->par.cobid_req;
814  uint_least8_t flags = 0;
815  if (id & CO_SDO_COBID_FRAME) {
816  id &= CAN_MASK_EID;
817  flags |= CAN_FLAG_IDE;
818  } else {
819  id &= CAN_MASK_BID;
820  }
821  can_recv_start(sdo->recv, sdo->net, id, flags);
822  } else {
823  can_recv_stop(sdo->recv);
824  }
825 
826  return 0;
827 }
828 
829 static co_unsigned32_t
830 co_1200_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
831 {
832  assert(sub);
833  assert(req);
834  co_ssdo_t *sdo = data;
835  assert(sdo);
836  assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1200 + sdo->num - 1);
837 
838  co_unsigned16_t type = co_sub_get_type(sub);
839  assert(!co_type_is_array(type));
840 
841  co_unsigned32_t ac = 0;
842  union co_val val;
843  if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
844  return ac;
845 
846  switch (co_sub_get_subidx(sub)) {
847  case 0: return CO_SDO_AC_NO_WRITE;
848  case 1: {
849  assert(type == CO_DEFTYPE_UNSIGNED32);
850  co_unsigned32_t cobid = val.u32;
851  co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
852  if (cobid == cobid_old)
853  return 0;
854 
855  // The CAN-ID cannot be changed when the SDO is and remains
856  // valid.
857  int valid = !(cobid & CO_SDO_COBID_VALID);
858  int valid_old = !(cobid_old & CO_SDO_COBID_VALID);
859  uint_least32_t canid = cobid & CAN_MASK_EID;
860  uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
861  if (valid && valid_old && canid != canid_old)
862  return CO_SDO_AC_PARAM_VAL;
863 
864  // A 29-bit CAN-ID is only valid if the frame bit is set.
865  if (!(cobid & CO_SDO_COBID_FRAME)
866  && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID)))
867  return CO_SDO_AC_PARAM_VAL;
868 
869  sdo->par.cobid_req = cobid;
870  break;
871  }
872  case 2: {
873  assert(type == CO_DEFTYPE_UNSIGNED32);
874  co_unsigned32_t cobid = val.u32;
875  co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
876  if (cobid == cobid_old)
877  return 0;
878 
879  // The CAN-ID cannot be changed when the SDO is and remains
880  // valid.
881  int valid = !(cobid & CO_SDO_COBID_VALID);
882  int valid_old = !(cobid_old & CO_SDO_COBID_VALID);
883  uint_least32_t canid = cobid & CAN_MASK_EID;
884  uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
885  if (valid && valid_old && canid != canid_old)
886  return CO_SDO_AC_PARAM_VAL;
887 
888  // A 29-bit CAN-ID is only valid if the frame bit is set.
889  if (!(cobid & CO_SDO_COBID_FRAME)
890  && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID)))
891  return CO_SDO_AC_PARAM_VAL;
892 
893  sdo->par.cobid_res = cobid;
894  break;
895  }
896  case 3: {
897  assert(type == CO_DEFTYPE_UNSIGNED8);
898  co_unsigned8_t id = val.u8;
899  co_unsigned8_t id_old = co_sub_get_val_u8(sub);
900  if (id == id_old)
901  return 0;
902 
903  sdo->par.id = id;
904  break;
905  }
906  default: return CO_SDO_AC_NO_SUB;
907  }
908 
909  co_sub_dn(sub, &val);
910 
911  co_ssdo_update(sdo);
912  return 0;
913 }
914 
915 static int
916 co_ssdo_recv(const struct can_msg *msg, void *data)
917 {
918  assert(msg);
919  co_ssdo_t *sdo = data;
920  assert(sdo);
921 
922  // Ignore remote frames.
923  if (msg->flags & CAN_FLAG_RTR)
924  return 0;
925 
926 #if !LELY_NO_CANFD
927  // Ignore CAN FD format frames.
928  if (msg->flags & CAN_FLAG_EDL)
929  return 0;
930 #endif
931 
932  co_ssdo_emit_recv(sdo, msg);
933 
934  return 0;
935 }
936 
937 static int
938 co_ssdo_timer(const struct timespec *tp, void *data)
939 {
940  assert(tp);
941  co_ssdo_t *sdo = data;
942  assert(sdo);
943 
944  co_ssdo_emit_time(sdo, tp);
945 
946  return 0;
947 }
948 
949 static inline void
951 {
952  assert(sdo);
953 
954  if (next)
955  sdo->state = next;
956 }
957 
958 static inline void
959 co_ssdo_emit_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
960 {
961  assert(sdo);
962  assert(sdo->state);
963  assert(sdo->state->on_recv);
964 
965  co_ssdo_enter(sdo, sdo->state->on_abort(sdo, ac));
966 }
967 
968 static inline void
969 co_ssdo_emit_time(co_ssdo_t *sdo, const struct timespec *tp)
970 {
971  assert(sdo);
972  assert(sdo->state);
973  assert(sdo->state->on_time);
974 
975  co_ssdo_enter(sdo, sdo->state->on_time(sdo, tp));
976 }
977 
978 static inline void
979 co_ssdo_emit_recv(co_ssdo_t *sdo, const struct can_msg *msg)
980 {
981  assert(sdo);
982  assert(sdo->state);
983  assert(sdo->state->on_recv);
984 
985  co_ssdo_enter(sdo, sdo->state->on_recv(sdo, msg));
986 }
987 
988 static co_ssdo_state_t *
989 co_ssdo_wait_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
990 {
991  (void)sdo;
992  (void)ac;
993 
994  return NULL;
995 }
996 
997 static co_ssdo_state_t *
998 co_ssdo_wait_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
999 {
1000  assert(sdo);
1001  assert(msg);
1002 
1003  if (msg->len < 1)
1004  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1005  co_unsigned8_t cs = msg->data[0];
1006 
1007  switch (cs & CO_SDO_CS_MASK) {
1008  case CO_SDO_CCS_DN_INI_REQ: return co_ssdo_dn_ini_on_recv(sdo, msg);
1009  case CO_SDO_CCS_UP_INI_REQ: return co_ssdo_up_ini_on_recv(sdo, msg);
1010  case CO_SDO_CCS_BLK_DN_REQ: return co_ssdo_blk_dn_ini_on_recv(sdo, msg);
1011  case CO_SDO_CCS_BLK_UP_REQ: return co_ssdo_blk_up_ini_on_recv(sdo, msg);
1012  case CO_SDO_CS_ABORT: return NULL;
1013  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1014  }
1015 }
1016 
1017 static co_ssdo_state_t *
1018 co_ssdo_dn_ini_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
1019 {
1020  assert(sdo);
1021  assert(msg);
1022 
1023  assert(msg->len > 0);
1024  co_unsigned8_t cs = msg->data[0];
1025 
1026  // Load the object index and sub-index from the CAN frame.
1027  if (msg->len < 3)
1028  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_OBJ);
1029  sdo->idx = ldle_u16(msg->data + 1);
1030  if (msg->len < 4)
1031  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_SUB);
1032  sdo->subidx = msg->data[3];
1033 
1034  trace("SSDO: %04X:%02X: received download request", sdo->idx,
1035  sdo->subidx);
1036 
1037  // Obtain the size from the command specifier.
1038  co_sdo_req_clear(&sdo->req);
1039  int exp = !!(cs & CO_SDO_INI_SIZE_EXP);
1040  if (exp) {
1041  if (cs & CO_SDO_INI_SIZE_IND)
1042  sdo->req.size = CO_SDO_INI_SIZE_EXP_GET(cs);
1043  else
1044  sdo->req.size = msg->len - 4;
1045  } else if (cs & CO_SDO_INI_SIZE_IND) {
1046  // 0-pad the data bytes to handle clients which send CAN frames
1047  // less than 8 bytes.
1048  uint_least8_t data[4] = { 0 };
1049  memcpy(data, msg->data + 4, msg->len - 4);
1050  sdo->req.size = ldle_u32(data);
1051  }
1052 
1053  if (exp) {
1054  // Perform an expedited transfer.
1055  sdo->req.buf = msg->data + 4;
1056  sdo->req.nbyte = sdo->req.size;
1057  co_unsigned32_t ac = co_ssdo_dn_ind(sdo);
1058  if (ac)
1059  return co_ssdo_abort_res(sdo, ac);
1060  // Finalize the transfer.
1062  return co_ssdo_abort_ind(sdo);
1063  } else {
1065  if (sdo->timeout)
1066  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1067  return co_ssdo_dn_seg_state;
1068  }
1069 }
1070 
1071 static co_ssdo_state_t *
1072 co_ssdo_dn_seg_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1073 {
1074  return co_ssdo_abort_res(sdo, ac);
1075 }
1076 
1077 static co_ssdo_state_t *
1078 co_ssdo_dn_seg_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1079 {
1080  (void)tp;
1081 
1082  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1083 }
1084 
1085 static co_ssdo_state_t *
1086 co_ssdo_dn_seg_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
1087 {
1088  assert(sdo);
1089  assert(msg);
1090 
1091  if (msg->len < 1)
1092  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1093  co_unsigned8_t cs = msg->data[0];
1094 
1095  // Check the client command specifier.
1096  switch (cs & CO_SDO_CS_MASK) {
1097  case CO_SDO_CCS_DN_SEG_REQ: break;
1098  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1099  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1100  }
1101 
1102  // Check the value of the toggle bit.
1103  if ((cs & CO_SDO_SEG_TOGGLE) != sdo->toggle)
1104  return co_ssdo_dn_seg_state;
1105 
1106  // Obtain the size of the segment.
1107  size_t n = CO_SDO_SEG_SIZE_GET(cs);
1108  if (msg->len < 1 + n)
1109  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1110  int last = !!(cs & CO_SDO_SEG_LAST);
1111 
1112  if (sdo->req.offset + sdo->req.nbyte + n > sdo->req.size)
1114 
1115  sdo->req.buf = msg->data + 1;
1116  sdo->req.offset += sdo->req.nbyte;
1117  sdo->req.nbyte = n;
1118 
1119  if (last && !co_sdo_req_last(&sdo->req))
1121 
1122  co_unsigned32_t ac = co_ssdo_dn_ind(sdo);
1123  if (ac)
1124  return co_ssdo_abort_res(sdo, ac);
1125 
1127 
1128  if (last) {
1129  return co_ssdo_abort_ind(sdo);
1130  } else {
1131  if (sdo->timeout)
1132  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1133  return co_ssdo_dn_seg_state;
1134  }
1135 }
1136 
1137 static co_ssdo_state_t *
1138 co_ssdo_up_ini_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
1139 {
1140  assert(sdo);
1141  assert(msg);
1142 
1143  // Load the object index and sub-index from the CAN frame.
1144  if (msg->len < 3)
1145  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_OBJ);
1146  sdo->idx = ldle_u16(msg->data + 1);
1147  if (msg->len < 4)
1148  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_SUB);
1149  sdo->subidx = msg->data[3];
1150 
1151  trace("SSDO: %04X:%02X: received upload request", sdo->idx,
1152  sdo->subidx);
1153 
1154  // Perform access checks and start serializing the value.
1155  co_sdo_req_clear(&sdo->req);
1156  co_unsigned32_t ac = co_ssdo_up_ind(sdo);
1157  if (ac)
1158  return co_ssdo_abort_res(sdo, ac);
1159 
1160  if (sdo->req.size && sdo->req.size <= 4) {
1161  // Perform an expedited transfer.
1162  if ((ac = co_ssdo_up_buf(sdo, sdo->req.size)) != 0)
1163  return co_ssdo_abort_res(sdo, ac);
1165  return co_ssdo_abort_ind(sdo);
1166  } else {
1168  if (sdo->timeout)
1169  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1170  return co_ssdo_up_seg_state;
1171  }
1172 }
1173 
1174 static co_ssdo_state_t *
1175 co_ssdo_up_seg_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1176 {
1177  return co_ssdo_abort_res(sdo, ac);
1178 }
1179 
1180 static co_ssdo_state_t *
1181 co_ssdo_up_seg_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1182 {
1183  (void)tp;
1184 
1185  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1186 }
1187 
1188 static co_ssdo_state_t *
1189 co_ssdo_up_seg_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
1190 {
1191  assert(sdo);
1192  assert(msg);
1193 
1194  if (msg->len < 1)
1195  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1196  co_unsigned8_t cs = msg->data[0];
1197 
1198  // Check the client command specifier.
1199  switch (cs & CO_SDO_CS_MASK) {
1200  case CO_SDO_CCS_UP_SEG_REQ: break;
1201  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1202  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1203  }
1204 
1205  // Check the value of the toggle bit.
1206  if ((cs & CO_SDO_SEG_TOGGLE) != sdo->toggle)
1207  return co_ssdo_abort_res(sdo, CO_SDO_AC_TOGGLE);
1208 
1209  membuf_clear(&sdo->buf);
1210  co_unsigned32_t ac = co_ssdo_up_buf(sdo, 7);
1211  if (ac)
1212  return co_ssdo_abort_res(sdo, ac);
1213 
1214  int last = co_sdo_req_last(&sdo->req) && sdo->nbyte == sdo->req.nbyte;
1215  co_ssdo_send_up_seg_res(sdo, last);
1216 
1217  if (last) {
1218  // Finalize the transfer.
1219  return co_ssdo_abort_ind(sdo);
1220  } else {
1221  if (sdo->timeout)
1222  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1223  return co_ssdo_up_seg_state;
1224  }
1225 }
1226 
1227 static co_ssdo_state_t *
1229 {
1230  assert(sdo);
1231  assert(msg);
1232 
1233  assert(msg->len > 0);
1234  co_unsigned8_t cs = msg->data[0];
1235 
1236  // Check the client subcommand.
1237  if ((cs & 0x01) != CO_SDO_SC_INI_BLK)
1238  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1239 
1240  // Check if the client supports generating a CRC.
1241  sdo->gencrc = !!(cs & CO_SDO_BLK_CRC);
1242 
1243  // Load the object index and sub-index from the CAN frame.
1244  if (msg->len < 3)
1245  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_OBJ);
1246  sdo->idx = ldle_u16(msg->data + 1);
1247  if (msg->len < 4)
1248  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_SUB);
1249  sdo->subidx = msg->data[3];
1250 
1251  trace("SSDO: %04X:%02X: received block download request", sdo->idx,
1252  sdo->subidx);
1253 
1254  // Obtain the data set size.
1255  co_sdo_req_clear(&sdo->req);
1256  if (cs & CO_SDO_BLK_SIZE_IND) {
1257  // 0-pad the data bytes to handle clients which send CAN frames
1258  // less than 8 bytes.
1259  uint_least8_t data[4] = { 0 };
1260  memcpy(data, msg->data + 4, msg->len - 4);
1261  sdo->req.size = ldle_u32(data);
1262  }
1263 
1264  // Use the maximum block size by default.
1265  sdo->blksize = CO_SSDO_MAX_SEQNO;
1266  sdo->ackseq = 0;
1267 
1269 
1270  if (sdo->timeout)
1271  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1272  return co_ssdo_blk_dn_sub_state;
1273 }
1274 
1275 static co_ssdo_state_t *
1276 co_ssdo_blk_dn_sub_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1277 {
1278  return co_ssdo_abort_res(sdo, ac);
1279 }
1280 
1281 static co_ssdo_state_t *
1282 co_ssdo_blk_dn_sub_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1283 {
1284  (void)tp;
1285 
1286  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1287 }
1288 
1289 static co_ssdo_state_t *
1291 {
1292  assert(sdo);
1293  assert(msg);
1294 
1295  if (msg->len < 1)
1296  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1297  co_unsigned8_t cs = msg->data[0];
1298 
1299  if (cs == CO_SDO_CS_ABORT)
1300  return co_ssdo_abort_ind(sdo);
1301 
1302  co_unsigned8_t seqno = cs & ~CO_SDO_SEQ_LAST;
1303  int last = !!(cs & CO_SDO_SEQ_LAST);
1304 
1305  if (!seqno || seqno > sdo->blksize)
1306  return co_ssdo_abort_res(sdo, CO_SDO_AC_BLK_SEQ);
1307 
1308  // Only accept sequential segments. Dropped segments will be resent
1309  // after the confirmation message.
1310  if (seqno == sdo->ackseq + 1) {
1311  sdo->ackseq++;
1312  // Update the CRC.
1313  if (sdo->gencrc)
1314  sdo->crc = co_crc(
1315  sdo->crc, sdo->req.buf, sdo->req.nbyte);
1316  // Pass the previous frame to the download indication function.
1317  co_unsigned32_t ac = co_ssdo_dn_ind(sdo);
1318  if (ac)
1319  return co_ssdo_abort_res(sdo, ac);
1320  // Determine the number of bytes to copy.
1321  assert(sdo->req.size >= sdo->req.offset + sdo->req.nbyte);
1322  size_t n = MIN(sdo->req.size - sdo->req.offset - sdo->req.nbyte,
1323  7);
1324  if (!last && n < 7)
1326  // Copy the new frame to the SDO request.
1327  membuf_clear(&sdo->buf);
1328  if (!membuf_reserve(&sdo->buf, n))
1329  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_MEM);
1330  membuf_write(&sdo->buf, msg->data + 1, n);
1331  sdo->req.buf = membuf_begin(&sdo->buf);
1332  sdo->req.offset += sdo->req.nbyte;
1333  sdo->req.nbyte = membuf_size(&sdo->buf);
1334  }
1335 
1336  // If this is the last segment in the block, send a confirmation.
1337  if (seqno == sdo->blksize || last) {
1339  sdo->ackseq = 0;
1340  }
1341 
1342  if (sdo->timeout)
1343  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1345 }
1346 
1347 static co_ssdo_state_t *
1348 co_ssdo_blk_dn_end_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1349 {
1350  return co_ssdo_abort_res(sdo, ac);
1351 }
1352 
1353 static co_ssdo_state_t *
1354 co_ssdo_blk_dn_end_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1355 {
1356  (void)tp;
1357 
1358  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1359 }
1360 
1361 static co_ssdo_state_t *
1363 {
1364  assert(sdo);
1365  assert(msg);
1366 
1367  if (msg->len < 1)
1368  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1369  co_unsigned8_t cs = msg->data[0];
1370 
1371  // Check the client command specifier.
1372  switch (cs & CO_SDO_CS_MASK) {
1373  case CO_SDO_CCS_BLK_DN_REQ: break;
1374  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1375  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1376  }
1377 
1378  // Check the client subcommand.
1379  if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_END_BLK)
1380  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1381 
1382  // Check the total length.
1383  if (sdo->req.size != sdo->req.offset + sdo->req.nbyte)
1385 
1386  // Check the number of bytes in the last segment.
1387  co_unsigned8_t n = sdo->req.size ? (sdo->req.size - 1) % 7 + 1 : 0;
1388  if (CO_SDO_BLK_SIZE_GET(cs) != n)
1389  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1390 
1391  // Check the CRC.
1392  if (sdo->gencrc) {
1393  sdo->crc = co_crc(sdo->crc, sdo->req.buf, sdo->req.nbyte);
1394  co_unsigned16_t crc = ldle_u16(msg->data + 1);
1395  if (sdo->crc != crc)
1396  return co_ssdo_abort_res(sdo, CO_SDO_AC_BLK_CRC);
1397  }
1398 
1399  co_unsigned32_t ac = co_ssdo_dn_ind(sdo);
1400  if (ac)
1401  return co_ssdo_abort_res(sdo, ac);
1402 
1403  // Finalize the transfer.
1405  return co_ssdo_abort_ind(sdo);
1406 }
1407 
1408 static co_ssdo_state_t *
1410 {
1411  assert(sdo);
1412  assert(msg);
1413 
1414  assert(msg->len > 0);
1415  co_unsigned8_t cs = msg->data[0];
1416 
1417  // Check the client subcommand.
1418  if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_INI_BLK)
1419  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1420 
1421  // Check if the client supports generating a CRC.
1422  sdo->gencrc = !!(cs & CO_SDO_BLK_CRC);
1423 
1424  // Load the object index and sub-index from the CAN frame.
1425  if (msg->len < 3)
1426  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_OBJ);
1427  sdo->idx = ldle_u16(msg->data + 1);
1428  if (msg->len < 4)
1429  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_SUB);
1430  sdo->subidx = msg->data[3];
1431 
1432  trace("SSDO: %04X:%02X: received block upload request", sdo->idx,
1433  sdo->subidx);
1434 
1435  // Load the number of segments per block.
1436  if (msg->len < 5)
1438  sdo->blksize = msg->data[4];
1439  if (!sdo->blksize || sdo->blksize > CO_SSDO_MAX_SEQNO)
1441 
1442  // Load the protocol switch threshold (PST).
1443  co_unsigned8_t pst = msg->len > 5 ? msg->data[5] : 0;
1444 
1445  // Perform access checks and start serializing the value.
1446  co_sdo_req_clear(&sdo->req);
1447  co_unsigned32_t ac = co_ssdo_up_ind(sdo);
1448  if (ac)
1449  return co_ssdo_abort_res(sdo, ac);
1450 
1451  if (pst && sdo->req.size <= pst) {
1452  // If the PST is non-zero, and the number of bytes is smaller
1453  // than or equal to the PST, switch to the SDO upload protocol.
1454  if (sdo->req.size <= 4) {
1455  // Perform an expedited transfer.
1456  if ((ac = co_ssdo_up_buf(sdo, sdo->req.size)) != 0)
1457  return co_ssdo_abort_res(sdo, ac);
1459  return co_ssdo_abort_ind(sdo);
1460  } else {
1462  if (sdo->timeout)
1463  can_timer_timeout(sdo->timer, sdo->net,
1464  sdo->timeout);
1465  return co_ssdo_up_seg_state;
1466  }
1467  } else {
1469  if (sdo->timeout)
1470  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1471  return co_ssdo_blk_up_sub_state;
1472  }
1473 }
1474 
1475 static co_ssdo_state_t *
1476 co_ssdo_blk_up_sub_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1477 {
1478  return co_ssdo_abort_res(sdo, ac);
1479 }
1480 
1481 static co_ssdo_state_t *
1482 co_ssdo_blk_up_sub_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1483 {
1484  (void)tp;
1485 
1486  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1487 }
1488 
1489 static co_ssdo_state_t *
1491 {
1492  assert(sdo);
1493  assert(msg);
1494 
1495  if (msg->len < 1)
1496  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1497  co_unsigned8_t cs = msg->data[0];
1498 
1499  // Check the client command specifier.
1500  switch (cs & CO_SDO_CS_MASK) {
1501  case CO_SDO_CCS_BLK_UP_REQ: break;
1502  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1503  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1504  }
1505 
1506  // Check the client subcommand.
1507  switch (cs & CO_SDO_SC_MASK) {
1508  case CO_SDO_SC_BLK_RES:
1509  if (co_sdo_req_first(&sdo->req) && !sdo->nbyte)
1510  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1511 
1512  if (msg->len < 3)
1513  return co_ssdo_abort_res(sdo, CO_SDO_AC_BLK_SEQ);
1514 
1515  // Flush the successfully sent segments from the buffer.
1516  co_unsigned8_t ackseq = msg->data[1];
1517  membuf_flush(&sdo->buf, ackseq * 7);
1518 
1519  // Read the number of segments in the next block.
1520  sdo->blksize = msg->data[2];
1521  if (!sdo->blksize || sdo->blksize > CO_SSDO_MAX_SEQNO)
1523 
1524  break;
1525  case CO_SDO_SC_START_UP:
1526  if (!(co_sdo_req_first(&sdo->req) && !sdo->nbyte))
1527  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1528  break;
1529  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1530  }
1531 
1532  ptrdiff_t n = sdo->blksize * 7 - membuf_size(&sdo->buf);
1533  if (n > 0) {
1534  if (!membuf_reserve(&sdo->buf, n))
1535  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_MEM);
1536  co_unsigned32_t ac = co_ssdo_up_buf(sdo, n);
1537  if (ac)
1538  return co_ssdo_abort_res(sdo, ac);
1539  sdo->blksize = (co_unsigned8_t)(
1540  (membuf_size(&sdo->buf) + 6) / 7);
1541  }
1542  int last = co_sdo_req_last(&sdo->req) && sdo->nbyte == sdo->req.nbyte;
1543 
1544  if (sdo->timeout)
1545  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1546 
1547  if (sdo->blksize) {
1548  // Send all segments in the current block.
1549  co_ssdo_send_blk_up_sub_res(sdo, last);
1550  return co_ssdo_blk_up_sub_state;
1551  } else {
1553  return co_ssdo_blk_up_end_state;
1554  }
1555 }
1556 
1557 static co_ssdo_state_t *
1558 co_ssdo_blk_up_end_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1559 {
1560  return co_ssdo_abort_res(sdo, ac);
1561 }
1562 
1563 static co_ssdo_state_t *
1564 co_ssdo_blk_up_end_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1565 {
1566  (void)tp;
1567 
1568  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1569 }
1570 
1571 static co_ssdo_state_t *
1573 {
1574  assert(sdo);
1575  assert(msg);
1576 
1577  if (msg->len < 1)
1578  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1579  co_unsigned8_t cs = msg->data[0];
1580 
1581  // Check the client command specifier.
1582  switch (cs & CO_SDO_CS_MASK) {
1583  case CO_SDO_CCS_BLK_UP_REQ: break;
1584  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1585  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1586  }
1587 
1588  // Check the client subcommand.
1589  if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_END_BLK)
1590  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1591 
1592  return co_ssdo_abort_ind(sdo);
1593 }
1594 
1595 static co_ssdo_state_t *
1597 {
1598  assert(sdo);
1599 
1600  if (sdo->timeout)
1601  can_timer_stop(sdo->timer);
1602 
1603  sdo->idx = 0;
1604  sdo->subidx = 0;
1605 
1606  sdo->toggle = 0;
1607  sdo->blksize = 0;
1608  sdo->ackseq = 0;
1609  sdo->gencrc = 0;
1610  sdo->crc = 0;
1611 
1612  co_sdo_req_clear(&sdo->req);
1613  membuf_clear(&sdo->buf);
1614  sdo->nbyte = 0;
1615 
1616  return co_ssdo_wait_state;
1617 }
1618 
1619 static co_ssdo_state_t *
1620 co_ssdo_abort_res(co_ssdo_t *sdo, co_unsigned32_t ac)
1621 {
1622 #if !LELY_NO_STDIO
1623  trace("SSDO: abort code %08" PRIX32 " (%s)", ac, co_sdo_ac2str(ac));
1624 #endif
1625  co_ssdo_send_abort(sdo, ac);
1626  return co_ssdo_abort_ind(sdo);
1627 }
1628 
1629 static co_unsigned32_t
1631 {
1632  assert(sdo);
1633 
1634  // Find the object in the object dictionary.
1635  co_obj_t *obj = co_dev_find_obj(sdo->dev, sdo->idx);
1636  if (!obj)
1637  return CO_SDO_AC_NO_OBJ;
1638 
1639  // Find the sub-object.
1640  co_sub_t *sub = co_obj_find_sub(obj, sdo->subidx);
1641  if (!sub)
1642  return CO_SDO_AC_NO_SUB;
1643 
1644  return co_sub_dn_ind(sub, &sdo->req);
1645 }
1646 
1647 static co_unsigned32_t
1649 {
1650  assert(sdo);
1651 
1652  // Find the object in the object dictionary.
1653  const co_obj_t *obj = co_dev_find_obj(sdo->dev, sdo->idx);
1654  if (!obj)
1655  return CO_SDO_AC_NO_OBJ;
1656 
1657  // Find the sub-object.
1658  const co_sub_t *sub = co_obj_find_sub(obj, sdo->subidx);
1659  if (!sub)
1660  return CO_SDO_AC_NO_SUB;
1661 
1662  // If the object is an array, check whether the element exists.
1663  if (co_obj_get_code(obj) == CO_OBJECT_ARRAY
1664  && sdo->subidx > co_obj_get_val_u8(obj, 0x00))
1665  return CO_SDO_AC_NO_DATA;
1666 
1667  sdo->nbyte = 0;
1668  return co_sub_up_ind(sub, &sdo->req);
1669 }
1670 
1671 static co_unsigned32_t
1672 co_ssdo_up_buf(co_ssdo_t *sdo, size_t nbyte)
1673 {
1674  co_unsigned32_t ac = 0;
1675 
1676  if (nbyte && !membuf_reserve(&sdo->buf, nbyte))
1677  return CO_SDO_AC_NO_MEM;
1678 
1679  while (nbyte) {
1680  if (sdo->nbyte >= sdo->req.nbyte) {
1681  if (co_sdo_req_last(&sdo->req)
1682  || (ac = co_ssdo_up_ind(sdo)))
1683  break;
1684  sdo->nbyte = 0;
1685  }
1686  const uint_least8_t *src = (const uint_least8_t *)sdo->req.buf
1687  + sdo->nbyte;
1688  size_t n = MIN(nbyte, sdo->req.nbyte - sdo->nbyte);
1689 
1690  if (sdo->gencrc)
1691  sdo->crc = co_crc(sdo->crc, src, n);
1692 
1693  membuf_write(&sdo->buf, src, n);
1694  nbyte -= n;
1695  sdo->nbyte += n;
1696  }
1697 
1698  return ac;
1699 }
1700 
1701 static void
1702 co_ssdo_send_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1703 {
1704  assert(sdo);
1705 
1706  struct can_msg msg;
1708  stle_u32(msg.data + 4, ac);
1709  can_net_send(sdo->net, &msg);
1710 }
1711 
1712 static void
1714 {
1715  assert(sdo);
1716 
1717  co_unsigned8_t cs = CO_SDO_SCS_DN_INI_RES;
1718 
1719  struct can_msg msg;
1720  co_ssdo_init_ini_res(sdo, &msg, cs);
1721  can_net_send(sdo->net, &msg);
1722 }
1723 
1724 static void
1726 {
1727  assert(sdo);
1728 
1729  co_unsigned8_t cs = CO_SDO_SCS_DN_SEG_RES | sdo->toggle;
1730  sdo->toggle ^= CO_SDO_SEG_TOGGLE;
1731 
1732  struct can_msg msg;
1733  co_ssdo_init_seg_res(sdo, &msg, cs);
1734  can_net_send(sdo->net, &msg);
1735 }
1736 
1737 static void
1739 {
1740  assert(sdo);
1741  assert(sdo->req.size && sdo->req.size <= 4);
1742 
1743  const char *buf = membuf_begin(&sdo->buf);
1744  size_t nbyte = membuf_size(&sdo->buf);
1745  assert(nbyte == sdo->req.size);
1746 
1747  co_unsigned8_t cs =
1749 
1750  struct can_msg msg;
1751  co_ssdo_init_ini_res(sdo, &msg, cs);
1752  memcpy(msg.data + 4, buf, nbyte);
1753  can_net_send(sdo->net, &msg);
1754 }
1755 
1756 static void
1758 {
1759  assert(sdo);
1760  assert(!sdo->req.size || sdo->req.size > 4);
1761 
1762  co_unsigned8_t cs = CO_SDO_SCS_UP_INI_RES | CO_SDO_INI_SIZE_IND;
1763 
1764  struct can_msg msg;
1765  co_ssdo_init_ini_res(sdo, &msg, cs);
1766  stle_u32(msg.data + 4, sdo->req.size);
1767  can_net_send(sdo->net, &msg);
1768 }
1769 
1770 static void
1772 {
1773  assert(sdo);
1774  assert(!sdo->req.size || sdo->req.size > 4);
1775 
1776  const char *buf = membuf_begin(&sdo->buf);
1777  size_t nbyte = membuf_size(&sdo->buf);
1778  assert(nbyte <= 7);
1779 
1780  co_unsigned8_t cs = CO_SDO_SCS_UP_SEG_RES | sdo->toggle
1781  | CO_SDO_SEG_SIZE_SET(nbyte);
1782  sdo->toggle ^= CO_SDO_SEG_TOGGLE;
1783  if (last)
1784  cs |= CO_SDO_SEG_LAST;
1785 
1786  struct can_msg msg;
1787  co_ssdo_init_seg_res(sdo, &msg, cs);
1788  memcpy(msg.data + 1, buf, nbyte);
1789  can_net_send(sdo->net, &msg);
1790 }
1791 
1792 static void
1794 {
1795  assert(sdo);
1796 
1797  co_unsigned8_t cs = CO_SDO_SCS_BLK_DN_RES | CO_SDO_SC_INI_BLK;
1798  if (sdo->gencrc)
1799  cs |= CO_SDO_BLK_CRC;
1800 
1801  struct can_msg msg;
1802  co_ssdo_init_ini_res(sdo, &msg, cs);
1803  msg.data[4] = sdo->blksize;
1804  can_net_send(sdo->net, &msg);
1805 }
1806 
1807 static void
1809 {
1810  assert(sdo);
1811 
1812  co_unsigned8_t cs = CO_SDO_SCS_BLK_DN_RES | CO_SDO_SC_BLK_RES;
1813 
1814  struct can_msg msg;
1815  co_ssdo_init_seg_res(sdo, &msg, cs);
1816  msg.data[1] = sdo->ackseq;
1817  msg.data[2] = sdo->blksize;
1818  can_net_send(sdo->net, &msg);
1819 }
1820 
1821 static void
1823 {
1824  assert(sdo);
1825 
1826  co_unsigned8_t cs = CO_SDO_SCS_BLK_DN_RES | CO_SDO_SC_END_BLK;
1827 
1828  struct can_msg msg;
1829  co_ssdo_init_seg_res(sdo, &msg, cs);
1830  can_net_send(sdo->net, &msg);
1831 }
1832 
1833 static void
1835 {
1836  assert(sdo);
1837 
1838  co_unsigned8_t cs = CO_SDO_SCS_BLK_UP_RES | CO_SDO_BLK_SIZE_IND
1840  if (sdo->gencrc)
1841  cs |= CO_SDO_BLK_CRC;
1842 
1843  struct can_msg msg;
1844  co_ssdo_init_ini_res(sdo, &msg, cs);
1845  stle_u32(msg.data + 4, sdo->req.size);
1846  can_net_send(sdo->net, &msg);
1847 }
1848 
1849 static void
1851 {
1852  assert(sdo);
1853 
1854  const char *buf = membuf_begin(&sdo->buf);
1855  size_t nbyte = membuf_size(&sdo->buf);
1856 
1857  for (co_unsigned8_t seqno = 1; seqno <= sdo->blksize;
1858  seqno++, buf += 7, nbyte -= 7) {
1859  co_unsigned8_t cs = seqno;
1860  if (last && nbyte <= 7)
1861  cs |= CO_SDO_SEQ_LAST;
1862 
1863  struct can_msg msg;
1864  co_ssdo_init_seg_res(sdo, &msg, cs);
1865  memcpy(msg.data + 1, buf, MIN(nbyte, 7));
1866  can_net_send(sdo->net, &msg);
1867  }
1868 }
1869 
1870 static void
1872 {
1873  assert(sdo);
1874 
1875  // Compute the number of bytes in the last segment containing data.
1876  co_unsigned8_t n = sdo->req.size ? (sdo->req.size - 1) % 7 + 1 : 0;
1877 
1878  co_unsigned8_t cs = CO_SDO_SCS_BLK_UP_RES | CO_SDO_SC_END_BLK
1879  | CO_SDO_BLK_SIZE_SET(n);
1880 
1881  struct can_msg msg;
1882  co_ssdo_init_seg_res(sdo, &msg, cs);
1883  stle_u16(msg.data + 1, sdo->crc);
1884  can_net_send(sdo->net, &msg);
1885 }
1886 
1887 static void
1888 co_ssdo_init_ini_res(co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
1889 {
1890  assert(sdo);
1891  assert(msg);
1892 
1893  *msg = (struct can_msg)CAN_MSG_INIT;
1894  msg->id = sdo->par.cobid_res;
1895  if (sdo->par.cobid_res & CO_SDO_COBID_FRAME) {
1896  msg->id &= CAN_MASK_EID;
1897  msg->flags |= CAN_FLAG_IDE;
1898  } else {
1899  msg->id &= CAN_MASK_BID;
1900  }
1901  msg->len = CAN_MAX_LEN;
1902  msg->data[0] = cs;
1903  stle_u16(msg->data + 1, sdo->idx);
1904  msg->data[3] = sdo->subidx;
1905 }
1906 
1907 static void
1908 co_ssdo_init_seg_res(co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
1909 {
1910  assert(sdo);
1911  assert(msg);
1912 
1913  *msg = (struct can_msg)CAN_MSG_INIT;
1914  msg->id = sdo->par.cobid_res;
1915  if (sdo->par.cobid_res & CO_SDO_COBID_FRAME) {
1916  msg->id &= CAN_MASK_EID;
1917  msg->flags |= CAN_FLAG_IDE;
1918  } else {
1919  msg->id &= CAN_MASK_BID;
1920  }
1921  msg->len = CAN_MAX_LEN;
1922  msg->data[0] = cs;
1923 }
@ 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_unsigned8_t co_dev_get_id(const co_dev_t *dev)
Returns the node-ID of a CANopen device.
Definition: dev.c:197
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
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
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
int co_sdo_req_first(const struct co_sdo_req *req)
Returns 1 if the specified request includes the first segment, and 0 otherwise.
Definition: sdo.h:343
#define CO_SDO_AC_TYPE_LEN_LO
SDO abort code: Data type does not match, length of service parameter too low.
Definition: sdo.h:129
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_AC_TYPE_LEN_HI
SDO abort code: Data type does not match, length of service parameter too high.
Definition: sdo.h:123
#define CO_SDO_AC_NO_CS
SDO abort code: Client/server command specifier not valid or unknown.
Definition: sdo.h:69
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_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
const char * co_sdo_ac2str(co_unsigned32_t ac)
Returns a string describing an SDO abort code.
Definition: sdo.c:57
#define CO_SDO_AC_PARAM_VAL
SDO abort code: Invalid value for parameter (download only).
Definition: sdo.h:135
void co_sdo_req_init(struct co_sdo_req *req)
Initializes a CANopen SDO upload/download request.
Definition: sdo.c:109
#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
void * membuf_begin(const struct membuf *buf)
Returns a pointer to the first byte in a memory buffer.
Definition: membuf.h:161
size_t membuf_reserve(struct membuf *buf, size_t size)
Resizes a memory buffer, if necessary, to make room for at least an additional size bytes.
Definition: membuf.c:52
void membuf_fini(struct membuf *buf)
Finalizes a memory buffer.
Definition: membuf.c:40
void membuf_flush(struct membuf *buf, size_t size)
Flushes size bytes from the beginning of a memory buffer.
Definition: membuf.c:88
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_clear(struct membuf *buf)
Clears a memory buffer.
Definition: membuf.h:169
size_t membuf_size(const struct membuf *buf)
Returns the total number of bytes written to a memory buffer.
Definition: membuf.h:177
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
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
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:533
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
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:376
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
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
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
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
#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_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
co_dev_t * co_ssdo_get_dev(const co_ssdo_t *sdo)
Returns a pointer to the CANopen device of a Server-SDO.
Definition: ssdo.c:760
int co_ssdo_start(co_ssdo_t *sdo)
Starts a Server-SDO service.
Definition: ssdo.c:687
static co_ssdo_state_t * co_ssdo_wait_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'waiting' state.
Definition: ssdo.c:989
void co_ssdo_stop(co_ssdo_t *sdo)
Stops a Server-SDO service.
Definition: ssdo.c:721
static co_ssdo_state_t * co_ssdo_wait_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'waiting' state.
Definition: ssdo.c:998
static void co_ssdo_send_blk_up_ini_res(co_ssdo_t *sdo)
Sends a Server-SDO 'block upload initiate' response.
Definition: ssdo.c:1834
static co_ssdo_state_t * co_ssdo_up_seg_on_time(co_ssdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'upload segment' state.
Definition: ssdo.c:1181
void co_ssdo_destroy(co_ssdo_t *ssdo)
Destroys a CANopen Server-SDO service.
Definition: ssdo.c:677
static void co_ssdo_send_dn_seg_res(co_ssdo_t *sdo)
Sends a Server-SDO 'download segment' response.
Definition: ssdo.c:1725
static co_ssdo_state_t * co_ssdo_up_seg_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'upload segment' state.
Definition: ssdo.c:1189
static co_ssdo_state_t * co_ssdo_blk_dn_end_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'block download end' state.
Definition: ssdo.c:1348
co_unsigned8_t co_ssdo_get_num(const co_ssdo_t *sdo)
Returns the SDO number of a Server-SDO.
Definition: ssdo.c:768
static co_ssdo_state_t *const co_ssdo_blk_dn_end_state
The 'block download end' state.
Definition: ssdo.c:347
static void co_ssdo_init_ini_res(co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
Initializes a Server-SDO download/upload initiate response CAN frame.
Definition: ssdo.c:1888
static void co_ssdo_send_blk_up_sub_res(co_ssdo_t *sdo, int last)
Sends a Server-SDO 'block upload sub-block' response.
Definition: ssdo.c:1850
static co_ssdo_state_t * co_ssdo_blk_dn_sub_on_time(co_ssdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block download sub-block' state.
Definition: ssdo.c:1282
static co_ssdo_state_t * co_ssdo_blk_dn_end_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block download end' state.
Definition: ssdo.c:1362
static co_unsigned32_t co_ssdo_up_ind(co_ssdo_t *sdo)
Processes an upload indication of a Server-SDO by checking access to the requested sub-object and wri...
Definition: ssdo.c:1648
static co_ssdo_state_t * co_ssdo_dn_seg_on_time(co_ssdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'download segment' state.
Definition: ssdo.c:1078
static void co_ssdo_send_blk_up_end_res(co_ssdo_t *sdo)
Sends a Server-SDO 'block upload end' response.
Definition: ssdo.c:1871
static co_ssdo_state_t * co_ssdo_blk_up_end_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block upload end' state.
Definition: ssdo.c:1572
static co_ssdo_state_t * co_ssdo_blk_up_sub_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
The 'block upload initiate' state.
Definition: ssdo.c:1476
static void co_ssdo_send_up_seg_res(co_ssdo_t *sdo, int last)
Sends a Server-SDO 'upload segment' response.
Definition: ssdo.c:1771
static co_unsigned32_t co_ssdo_dn_ind(co_ssdo_t *sdo)
Processes a download indication of a Server-SDO by checking access to the requested sub-object and re...
Definition: ssdo.c:1630
static void co_ssdo_send_blk_dn_sub_res(co_ssdo_t *sdo)
Sends a Server-SDO 'block upload sub-block' response.
Definition: ssdo.c:1808
static co_ssdo_state_t * co_ssdo_blk_dn_sub_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block download sub-block' state.
Definition: ssdo.c:1290
void co_ssdo_set_timeout(co_ssdo_t *sdo, int timeout)
Sets the timeout of a Server-SDO.
Definition: ssdo.c:792
static int co_ssdo_update(co_ssdo_t *sdo)
Updates and (de)activates a Server-SDO service.
Definition: ssdo.c:803
static void co_ssdo_emit_time(co_ssdo_t *sdo, const struct timespec *tp)
Invokes the 'timeout' transition function of the current state of a Server-SDO service.
Definition: ssdo.c:969
static co_ssdo_state_t *const co_ssdo_blk_up_sub_state
The 'block upload sub-block' state.
Definition: ssdo.c:383
static co_unsigned32_t co_1200_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
The download indication function for (all sub-objects of) CANopen objects 1200..127F (SDO server para...
Definition: ssdo.c:830
static co_ssdo_state_t * co_ssdo_blk_dn_ini_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block download initiate' state.
Definition: ssdo.c:1228
static co_ssdo_state_t *const co_ssdo_blk_up_end_state
The 'block upload end' state.
Definition: ssdo.c:406
static int co_ssdo_recv(const struct can_msg *msg, void *data)
The CAN receive callback function for a Server-SDO service.
Definition: ssdo.c:916
static int co_ssdo_timer(const struct timespec *tp, void *data)
The CAN timer callback function for a Server-SDO service.
Definition: ssdo.c:938
static co_ssdo_state_t *const co_ssdo_dn_seg_state
The 'download segment' state.
Definition: ssdo.c:257
static co_ssdo_state_t * co_ssdo_up_ini_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'upload initiate' state.
Definition: ssdo.c:1138
static co_ssdo_state_t * co_ssdo_up_seg_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
The 'upload initiate' state.
Definition: ssdo.c:1175
static void co_ssdo_emit_recv(co_ssdo_t *sdo, const struct can_msg *msg)
Invokes the 'CAN frame received' transition function of the current state of a Server-SDO service.
Definition: ssdo.c:979
static co_unsigned32_t co_ssdo_up_buf(co_ssdo_t *sdo, size_t nbyte)
Copies at most nbyte bytes from a CANopen SDO upload request, obtaining more bytes with co_ssdo_up_in...
Definition: ssdo.c:1672
static void co_ssdo_send_dn_ini_res(co_ssdo_t *sdo)
Sends a Server-SDO 'download initiate' response.
Definition: ssdo.c:1713
static co_ssdo_state_t * co_ssdo_abort_res(co_ssdo_t *sdo, co_unsigned32_t ac)
Sends an abort transfer request and aborts any ongoing transfer by invoking co_ssdo_abort_ind().
Definition: ssdo.c:1620
static co_ssdo_state_t * co_ssdo_dn_ini_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'download initiate' state.
Definition: ssdo.c:1018
static void co_ssdo_send_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
Sends an abort transfer request.
Definition: ssdo.c:1702
static void co_ssdo_send_blk_dn_end_res(co_ssdo_t *sdo)
Sends a Server-SDO 'block download end' response.
Definition: ssdo.c:1822
static co_ssdo_state_t * co_ssdo_blk_dn_end_on_time(co_ssdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block download end' state.
Definition: ssdo.c:1354
static co_ssdo_state_t * co_ssdo_dn_seg_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
The 'download initiate' state.
Definition: ssdo.c:1072
static co_ssdo_state_t *const co_ssdo_blk_dn_sub_state
The 'block download sub-block' state.
Definition: ssdo.c:323
static co_ssdo_state_t * co_ssdo_dn_seg_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'download segment' state.
Definition: ssdo.c:1086
static void co_ssdo_init_seg_res(co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
Initializes a Server-SDO download/upload segment response CAN frame.
Definition: ssdo.c:1908
static void co_ssdo_send_blk_dn_ini_res(co_ssdo_t *sdo)
Sends a Server-SDO 'block download initiate' response.
Definition: ssdo.c:1793
static co_ssdo_state_t *const co_ssdo_up_seg_state
The 'upload segment' state.
Definition: ssdo.c:287
static co_ssdo_state_t * co_ssdo_blk_up_sub_on_time(co_ssdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block upload sub-block' state.
Definition: ssdo.c:1482
static co_ssdo_state_t * co_ssdo_blk_up_sub_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block upload sub-block' state.
Definition: ssdo.c:1490
int co_ssdo_get_timeout(const co_ssdo_t *sdo)
Returns the timeout (in milliseconds) of a Server-SDO.
Definition: ssdo.c:784
static co_ssdo_state_t * co_ssdo_blk_up_end_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
The 'abort' transition function of the 'block upload end' state.
Definition: ssdo.c:1558
static co_ssdo_state_t * co_ssdo_blk_up_ini_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
The 'CAN frame received' transition function of the 'block upload initiate' state.
Definition: ssdo.c:1409
static co_ssdo_state_t *const co_ssdo_stopped_state
The 'stopped' state.
Definition: ssdo.c:207
static co_ssdo_state_t * co_ssdo_abort_ind(co_ssdo_t *sdo)
Processes an abort transfer indication by aborting any ongoing transfer of a Server-SDO and returning...
Definition: ssdo.c:1596
const struct co_sdo_par * co_ssdo_get_par(const co_ssdo_t *sdo)
Returns a pointer to the SDO parameter record of a Server-SDO.
Definition: ssdo.c:776
can_net_t * co_ssdo_get_net(const co_ssdo_t *sdo)
Returns a pointer to the CAN network of a Server-SDO.
Definition: ssdo.c:752
static void co_ssdo_enter(co_ssdo_t *sdo, co_ssdo_state_t *next)
Enters the specified state of a Server-SDO service.
Definition: ssdo.c:950
static co_ssdo_state_t * co_ssdo_blk_dn_sub_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
The 'block download initiate' state.
Definition: ssdo.c:1276
static co_ssdo_state_t * co_ssdo_blk_up_end_on_time(co_ssdo_t *sdo, const struct timespec *tp)
The 'timeout' transition function of the 'block upload end' state.
Definition: ssdo.c:1564
static void co_ssdo_send_up_exp_res(co_ssdo_t *sdo)
Sends a Server-SDO 'upload initiate' (expedited) response.
Definition: ssdo.c:1738
static co_ssdo_state_t *const co_ssdo_wait_state
The 'waiting' state.
Definition: ssdo.c:222
static void co_ssdo_emit_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
Invokes the 'abort' transition function of the current state of a Server-SDO service.
Definition: ssdo.c:959
static void co_ssdo_send_up_ini_res(co_ssdo_t *sdo)
Sends a Server-SDO 'upload initiate' response.
Definition: ssdo.c:1757
int co_ssdo_is_stopped(const co_ssdo_t *sdo)
Retuns 1 if the specified Server-SDO service is stopped, and 0 if not.
Definition: ssdo.c:744
co_ssdo_t * co_ssdo_create(can_net_t *net, co_dev_t *dev, co_unsigned8_t num)
Creates a new CANopen Server-SDO service.
Definition: ssdo.c:650
This header file is part of the CANopen library; it contains the Server-SDO declarations.
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 device.
Definition: dev.h:30
A CANopen object.
Definition: obj.h:31
A CANopen Server-SDO state.
Definition: ssdo.c:171
co_ssdo_state_t *(* on_recv)(co_ssdo_t *sdo, const struct can_msg *msg)
A pointer to the transition function invoked when a CAN frame has been received.
Definition: ssdo.c:200
co_ssdo_state_t *(* on_time)(co_ssdo_t *sdo, const struct timespec *tp)
A pointer to the transition function invoked when a timeout occurs.
Definition: ssdo.c:190
co_ssdo_state_t *(* on_abort)(co_ssdo_t *sdo, co_unsigned32_t ac)
A pointer to the transition function invoked when an abort code has been received.
Definition: ssdo.c:181
A CANopen Server-SDO.
Definition: ssdo.c:63
struct co_sdo_par par
The SDO parameter record.
Definition: ssdo.c:71
co_unsigned16_t idx
The current object index.
Definition: ssdo.c:81
co_ssdo_state_t * state
A pointer to the current state.
Definition: ssdo.c:79
can_net_t * net
A pointer to a CAN network interface.
Definition: ssdo.c:65
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: ssdo.c:73
int timeout
The SDO timeout (in milliseconds).
Definition: ssdo.c:75
co_dev_t * dev
A pointer to a CANopen device.
Definition: ssdo.c:67
unsigned gencrc
A flag indicating whether a CRC should be generated.
Definition: ssdo.c:91
struct membuf buf
The buffer.
Definition: ssdo.c:97
can_timer_t * timer
A pointer to the CAN timer.
Definition: ssdo.c:77
co_unsigned16_t crc
The generated CRC.
Definition: ssdo.c:93
co_unsigned8_t subidx
The current object sub-index.
Definition: ssdo.c:83
struct co_sdo_req req
The SDO request.
Definition: ssdo.c:95
co_unsigned8_t blksize
The number of segments per block.
Definition: ssdo.c:87
co_unsigned8_t toggle
The current value of the toggle bit.
Definition: ssdo.c:85
co_unsigned8_t num
The SDO number.
Definition: ssdo.c:69
size_t nbyte
The number of bytes in req already copied to buf.
Definition: ssdo.c:99
co_unsigned8_t ackseq
The sequence number of the last successfully received segment.
Definition: ssdo.c:89
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
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 offset
The offset of the bytes at buf.
Definition: sdo.h:196
size_t size
The total size (in bytes) of the value to be uploaded/downloaded.
Definition: sdo.h:187
const void * buf
A pointer to the next bytes to be uploaded/downloaded.
Definition: sdo.h:189
size_t nbyte
The number of bytes available at buf.
Definition: sdo.h:191
A memory buffer.
Definition: membuf.h:36
char * begin
A pointer to the first byte in the buffer.
Definition: membuf.h:40
int co_type_is_array(co_unsigned16_t type)
Returns 1 if the specified (static) data type is an array, and 0 if not.
Definition: type.c:40
#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.