Lely core libraries  2.2.5
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 #include <inttypes.h>
36 #include <stdlib.h>
37 
38 struct __co_ssdo_state;
40 typedef const struct __co_ssdo_state co_ssdo_state_t;
41 
43 struct __co_ssdo {
49  co_unsigned8_t num;
51  struct co_sdo_par par;
55  int timeout;
61  co_unsigned16_t idx;
63  co_unsigned8_t subidx;
65  co_unsigned8_t toggle;
67  co_unsigned8_t blksize;
69  co_unsigned8_t ackseq;
71  unsigned gencrc : 1;
73  co_unsigned16_t crc;
75  struct co_sdo_req req;
77  struct membuf buf;
79  size_t nbyte;
80 };
81 
88 static int co_ssdo_update(co_ssdo_t *sdo);
89 
96 static co_unsigned32_t co_1200_dn_ind(
97  co_sub_t *sub, struct co_sdo_req *req, void *data);
98 
104 static int co_ssdo_recv(const struct can_msg *msg, void *data);
105 
111 static int co_ssdo_timer(const struct timespec *tp, void *data);
112 
114 static inline void co_ssdo_enter(co_ssdo_t *sdo, co_ssdo_state_t *next);
115 
123 static inline void co_ssdo_emit_abort(co_ssdo_t *sdo, co_unsigned32_t ac);
124 
132 static inline void co_ssdo_emit_time(co_ssdo_t *sdo, const struct timespec *tp);
133 
141 static inline void co_ssdo_emit_recv(co_ssdo_t *sdo, const struct can_msg *msg);
142 
154  co_ssdo_state_t *(*on_abort)(co_ssdo_t *sdo, co_unsigned32_t ac);
163  co_ssdo_state_t *(*on_time)(co_ssdo_t *sdo, const struct timespec *tp);
173  co_ssdo_state_t *(*on_recv)(co_ssdo_t *sdo, const struct can_msg *msg);
174 };
175 
176 #define LELY_CO_DEFINE_STATE(name, ...) \
177  static co_ssdo_state_t *const name = &(co_ssdo_state_t){ __VA_ARGS__ };
178 
181  co_ssdo_t *sdo, co_unsigned32_t ac);
182 
185  co_ssdo_t *sdo, const struct can_msg *msg);
186 
188 // clang-format off
189 LELY_CO_DEFINE_STATE(co_ssdo_wait_state,
190  .on_abort = &co_ssdo_wait_on_abort,
191  .on_recv = &co_ssdo_wait_on_recv
192 )
193 // clang-format on
194 
195 
200  co_ssdo_t *sdo, const struct can_msg *msg);
201 
203 // LELY_CO_DEFINE_STATE(co_ssdo_dn_ini_state,
204 // .on_recv = &co_ssdo_dn_ini_on_recv
205 //)
206 
209  co_ssdo_t *sdo, co_unsigned32_t ac);
210 
213  co_ssdo_t *sdo, const struct timespec *tp);
214 
219  co_ssdo_t *sdo, const struct can_msg *msg);
220 
222 // clang-format off
223 LELY_CO_DEFINE_STATE(co_ssdo_dn_seg_state,
224  .on_abort = &co_ssdo_dn_seg_on_abort,
225  .on_time = &co_ssdo_dn_seg_on_time,
226  .on_recv = &co_ssdo_dn_seg_on_recv
227 )
228 // clang-format on
229 
232  co_ssdo_t *sdo, const struct can_msg *msg);
233 
235 // LELY_CO_DEFINE_STATE(co_ssdo_up_ini_state,
236 // .on_recv = &co_ssdo_up_ini_on_recv
237 //)
238 
241  co_ssdo_t *sdo, co_unsigned32_t ac);
242 
245  co_ssdo_t *sdo, const struct timespec *tp);
246 
249  co_ssdo_t *sdo, const struct can_msg *msg);
250 
252 // clang-format off
253 LELY_CO_DEFINE_STATE(co_ssdo_up_seg_state,
254  .on_abort = &co_ssdo_up_seg_on_abort,
255  .on_time = &co_ssdo_up_seg_on_time,
256  .on_recv = &co_ssdo_up_seg_on_recv
257 )
258 // clang-format on
259 
265  co_ssdo_t *sdo, const struct can_msg *msg);
266 
268 // LELY_CO_DEFINE_STATE(co_ssdo_blk_dn_ini_state,
269 // .on_recv = &co_ssdo_blk_dn_ini_on_recv
270 //)
271 
274  co_ssdo_t *sdo, co_unsigned32_t ac);
275 
278  co_ssdo_t *sdo, const struct timespec *tp);
279 
285  co_ssdo_t *sdo, const struct can_msg *msg);
286 
288 // clang-format off
289 LELY_CO_DEFINE_STATE(co_ssdo_blk_dn_sub_state,
290  .on_abort = &co_ssdo_blk_dn_sub_on_abort,
291  .on_time = &co_ssdo_blk_dn_sub_on_time,
292  .on_recv = &co_ssdo_blk_dn_sub_on_recv
293 )
294 // clang-format on
295 
298  co_ssdo_t *sdo, co_unsigned32_t ac);
299 
302  co_ssdo_t *sdo, const struct timespec *tp);
303 
309  co_ssdo_t *sdo, const struct can_msg *msg);
310 
312 // clang-format off
313 LELY_CO_DEFINE_STATE(co_ssdo_blk_dn_end_state,
314  .on_abort = &co_ssdo_blk_dn_end_on_abort,
315  .on_time = &co_ssdo_blk_dn_end_on_time,
316  .on_recv = &co_ssdo_blk_dn_end_on_recv
317 )
318 // clang-format on
319 
325  co_ssdo_t *sdo, const struct can_msg *msg);
326 
328 // LELY_CO_DEFINE_STATE(co_ssdo_blk_up_ini_state,
329 // .on_recv = &co_ssdo_blk_up_ini_on_recv
330 //)
331 
334  co_ssdo_t *sdo, co_unsigned32_t ac);
335 
338  co_ssdo_t *sdo, const struct timespec *tp);
339 
345  co_ssdo_t *sdo, const struct can_msg *msg);
346 
348 // clang-format off
349 LELY_CO_DEFINE_STATE(co_ssdo_blk_up_sub_state,
350  .on_abort = &co_ssdo_blk_up_sub_on_abort,
351  .on_time = &co_ssdo_blk_up_sub_on_time,
352  .on_recv = &co_ssdo_blk_up_sub_on_recv
353 )
354 // clang-format on
355 
358  co_ssdo_t *sdo, co_unsigned32_t ac);
359 
362  co_ssdo_t *sdo, const struct timespec *tp);
363 
368  co_ssdo_t *sdo, const struct can_msg *msg);
369 
371 // clang-format off
372 LELY_CO_DEFINE_STATE(co_ssdo_blk_up_end_state,
373  .on_abort = &co_ssdo_blk_up_end_on_abort,
374  .on_time = &co_ssdo_blk_up_end_on_time,
375  .on_recv = &co_ssdo_blk_up_end_on_recv
376 )
377 // clang-format on
378 
379 #undef LELY_CO_DEFINE_STATE
380 
388 
400 static co_ssdo_state_t *co_ssdo_abort_res(co_ssdo_t *sdo, co_unsigned32_t ac);
401 
408 static co_unsigned32_t co_ssdo_dn_ind(co_ssdo_t *sdo);
409 
416 static co_unsigned32_t co_ssdo_up_ind(co_ssdo_t *sdo);
417 
424 static co_unsigned32_t co_ssdo_up_buf(co_ssdo_t *sdo, size_t nbyte);
425 
432 static void co_ssdo_send_abort(co_ssdo_t *sdo, co_unsigned32_t ac);
433 
435 static void co_ssdo_send_dn_ini_res(co_ssdo_t *sdo);
436 
438 static void co_ssdo_send_dn_seg_res(co_ssdo_t *sdo);
439 
441 static void co_ssdo_send_up_exp_res(co_ssdo_t *sdo);
442 
444 static void co_ssdo_send_up_ini_res(co_ssdo_t *sdo);
445 
452 static void co_ssdo_send_up_seg_res(co_ssdo_t *sdo, int last);
453 
455 static void co_ssdo_send_blk_dn_ini_res(co_ssdo_t *sdo);
456 
458 static void co_ssdo_send_blk_dn_sub_res(co_ssdo_t *sdo);
459 
461 static void co_ssdo_send_blk_dn_end_res(co_ssdo_t *sdo);
462 
464 static void co_ssdo_send_blk_up_ini_res(co_ssdo_t *sdo);
465 
472 static void co_ssdo_send_blk_up_sub_res(co_ssdo_t *sdo, int last);
473 
475 static void co_ssdo_send_blk_up_end_res(co_ssdo_t *sdo);
476 
484 static void co_ssdo_init_ini_res(
485  co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs);
486 
494 static void co_ssdo_init_seg_res(
495  co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs);
496 
497 void *
498 __co_ssdo_alloc(void)
499 {
500  void *ptr = malloc(sizeof(struct __co_ssdo));
501  if (!ptr)
502  set_errc(errno2c(errno));
503  return ptr;
504 }
505 
506 void
507 __co_ssdo_free(void *ptr)
508 {
509  free(ptr);
510 }
511 
512 struct __co_ssdo *
513 __co_ssdo_init(struct __co_ssdo *sdo, can_net_t *net, co_dev_t *dev,
514  co_unsigned8_t num)
515 {
516  assert(sdo);
517  assert(net);
518  assert(dev);
519 
520  int errc = 0;
521 
522  if (!num || num > 128) {
523  errc = errnum2c(ERRNUM_INVAL);
524  goto error_param;
525  }
526 
527  // Find the SDO server parameter in the object dictionary. The default
528  // SDO (1200) is optional.
529  co_obj_t *obj_1200 = co_dev_find_obj(dev, 0x1200 + num - 1);
530  if (num != 1 && !obj_1200) {
531  errc = errnum2c(ERRNUM_INVAL);
532  goto error_param;
533  }
534 
535  sdo->net = net;
536  sdo->dev = dev;
537  sdo->num = num;
538 
539  // Initialize the SDO parameter record with the default values.
540  sdo->par.n = 3;
541  sdo->par.id = co_dev_get_id(dev);
542  sdo->par.cobid_req = 0x600 + sdo->par.id;
543  sdo->par.cobid_res = 0x580 + sdo->par.id;
544 
545  if (obj_1200) {
546  // Copy the SDO parameter record.
547  size_t size = co_obj_sizeof_val(obj_1200);
548  memcpy(&sdo->par, co_obj_addressof_val(obj_1200),
549  MIN(size, sizeof(sdo->par)));
550  }
551 
552  sdo->recv = can_recv_create();
553  if (!sdo->recv) {
554  errc = get_errc();
555  goto error_create_recv;
556  }
557  can_recv_set_func(sdo->recv, &co_ssdo_recv, sdo);
558 
559  sdo->timeout = 0;
560 
561  sdo->timer = can_timer_create();
562  if (!sdo->timer) {
563  errc = get_errc();
564  goto error_create_timer;
565  }
567 
568  sdo->state = co_ssdo_wait_state;
569 
570  sdo->idx = 0;
571  sdo->subidx = 0;
572 
573  sdo->toggle = 0;
574  sdo->blksize = 0;
575  sdo->ackseq = 0;
576  sdo->gencrc = 0;
577  sdo->crc = 0;
578 
579  co_sdo_req_init(&sdo->req);
580  membuf_init(&sdo->buf);
581  sdo->nbyte = 0;
582 
583  // Set the download indication function for the SDO parameter record.
584  if (obj_1200)
585  co_obj_set_dn_ind(obj_1200, &co_1200_dn_ind, sdo);
586 
587  if (co_ssdo_update(sdo) == -1) {
588  errc = get_errc();
589  goto error_update;
590  }
591 
592  return sdo;
593 
594 error_update:
595  if (obj_1200)
596  co_obj_set_dn_ind(obj_1200, NULL, NULL);
597  can_timer_destroy(sdo->timer);
598 error_create_timer:
599  can_recv_destroy(sdo->recv);
600 error_create_recv:
601 error_param:
602  set_errc(errc);
603  return NULL;
604 }
605 
606 void
607 __co_ssdo_fini(struct __co_ssdo *sdo)
608 {
609  assert(sdo);
610  assert(sdo->num >= 1 && sdo->num <= 128);
611 
612  // Remove the download indication functions for the SDO parameter
613  // record.
614  co_obj_t *obj_1200 = co_dev_find_obj(sdo->dev, 0x1200 + sdo->num - 1);
615  if (obj_1200)
616  co_obj_set_dn_ind(obj_1200, NULL, NULL);
617 
618  // Abort any ongoing transfer.
620 
621  membuf_fini(&sdo->buf);
622  co_sdo_req_fini(&sdo->req);
623 
624  can_timer_destroy(sdo->timer);
625 
626  can_recv_destroy(sdo->recv);
627 }
628 
629 co_ssdo_t *
631 {
632  trace("creating Server-SDO %d", num);
633 
634  int errc = 0;
635 
636  co_ssdo_t *sdo = __co_ssdo_alloc();
637  if (!sdo) {
638  errc = get_errc();
639  goto error_alloc_sdo;
640  }
641 
642  if (!__co_ssdo_init(sdo, net, dev, num)) {
643  errc = get_errc();
644  goto error_init_sdo;
645  }
646 
647  return sdo;
648 
649 error_init_sdo:
650  __co_ssdo_free(sdo);
651 error_alloc_sdo:
652  set_errc(errc);
653  return NULL;
654 }
655 
656 void
658 {
659  if (ssdo) {
660  trace("destroying Server-SDO %d", ssdo->num);
661  __co_ssdo_fini(ssdo);
662  __co_ssdo_free(ssdo);
663  }
664 }
665 
666 can_net_t *
668 {
669  assert(sdo);
670 
671  return sdo->net;
672 }
673 
674 co_dev_t *
676 {
677  assert(sdo);
678 
679  return sdo->dev;
680 }
681 
682 co_unsigned8_t
684 {
685  assert(sdo);
686 
687  return sdo->num;
688 }
689 
690 const struct co_sdo_par *
692 {
693  assert(sdo);
694 
695  return &sdo->par;
696 }
697 
698 int
700 {
701  assert(sdo);
702 
703  return sdo->timeout;
704 }
705 
706 void
707 co_ssdo_set_timeout(co_ssdo_t *sdo, int timeout)
708 {
709  assert(sdo);
710 
711  if (sdo->timeout && timeout <= 0)
712  can_timer_stop(sdo->timer);
713 
714  sdo->timeout = MAX(0, timeout);
715 }
716 
717 static int
719 {
720  assert(sdo);
721 
722  // Abort any ongoing transfer.
724 
725  int valid_req = !(sdo->par.cobid_req & CO_SDO_COBID_VALID);
726  int valid_res = !(sdo->par.cobid_res & CO_SDO_COBID_VALID);
727  if (valid_req && valid_res) {
728  uint_least32_t id = sdo->par.cobid_req;
729  uint_least8_t flags = 0;
730  if (id & CO_SDO_COBID_FRAME) {
731  id &= CAN_MASK_EID;
732  flags |= CAN_FLAG_IDE;
733  } else {
734  id &= CAN_MASK_BID;
735  }
736  can_recv_start(sdo->recv, sdo->net, id, flags);
737  } else {
738  can_recv_stop(sdo->recv);
739  }
740 
741  return 0;
742 }
743 
744 static co_unsigned32_t
745 co_1200_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
746 {
747  assert(sub);
748  assert(req);
749  co_ssdo_t *sdo = data;
750  assert(sdo);
751  assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1200 + sdo->num - 1);
752 
753  co_unsigned32_t ac = 0;
754 
755  co_unsigned16_t type = co_sub_get_type(sub);
756  union co_val val;
757  if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
758  return ac;
759 
760  switch (co_sub_get_subidx(sub)) {
761  case 0: ac = CO_SDO_AC_NO_WRITE; goto error;
762  case 1: {
763  assert(type == CO_DEFTYPE_UNSIGNED32);
764  co_unsigned32_t cobid = val.u32;
765  co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
766  if (cobid == cobid_old)
767  goto error;
768 
769  // The CAN-ID cannot be changed when the SDO is and remains
770  // valid.
771  int valid = !(cobid & CO_SDO_COBID_VALID);
772  int valid_old = !(cobid_old & CO_SDO_COBID_VALID);
773  uint_least32_t canid = cobid & CAN_MASK_EID;
774  uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
775  if (valid && valid_old && canid != canid_old) {
776  ac = CO_SDO_AC_PARAM_VAL;
777  goto error;
778  }
779 
780  // A 29-bit CAN-ID is only valid if the frame bit is set.
781  if (!(cobid & CO_SDO_COBID_FRAME)
782  && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID))) {
783  ac = CO_SDO_AC_PARAM_VAL;
784  goto error;
785  }
786 
787  sdo->par.cobid_req = cobid;
788  break;
789  }
790  case 2: {
791  assert(type == CO_DEFTYPE_UNSIGNED32);
792  co_unsigned32_t cobid = val.u32;
793  co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
794  if (cobid == cobid_old)
795  goto error;
796 
797  // The CAN-ID cannot be changed when the SDO is and remains
798  // valid.
799  int valid = !(cobid & CO_SDO_COBID_VALID);
800  int valid_old = !(cobid_old & CO_SDO_COBID_VALID);
801  uint_least32_t canid = cobid & CAN_MASK_EID;
802  uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
803  if (valid && valid_old && canid != canid_old) {
804  ac = CO_SDO_AC_PARAM_VAL;
805  goto error;
806  }
807 
808  // A 29-bit CAN-ID is only valid if the frame bit is set.
809  if (!(cobid & CO_SDO_COBID_FRAME)
810  && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID))) {
811  ac = CO_SDO_AC_PARAM_VAL;
812  goto error;
813  }
814 
815  sdo->par.cobid_res = cobid;
816  break;
817  }
818  case 3: {
819  assert(type == CO_DEFTYPE_UNSIGNED8);
820  co_unsigned8_t id = val.u8;
821  co_unsigned8_t id_old = co_sub_get_val_u8(sub);
822  if (id == id_old)
823  goto error;
824 
825  sdo->par.id = id;
826  break;
827  }
828  default: ac = CO_SDO_AC_NO_SUB; goto error;
829  }
830 
831  co_sub_dn(sub, &val);
832  co_val_fini(type, &val);
833 
834  co_ssdo_update(sdo);
835  return 0;
836 
837 error:
838  co_val_fini(type, &val);
839  return ac;
840 }
841 
842 static int
843 co_ssdo_recv(const struct can_msg *msg, void *data)
844 {
845  assert(msg);
846  co_ssdo_t *sdo = data;
847  assert(sdo);
848 
849  // Ignore remote frames.
850  if (msg->flags & CAN_FLAG_RTR)
851  return 0;
852 
853 #ifndef LELY_NO_CANFD
854  // Ignore CAN FD format frames.
855  if (msg->flags & CAN_FLAG_EDL)
856  return 0;
857 #endif
858 
859  co_ssdo_emit_recv(sdo, msg);
860 
861  return 0;
862 }
863 
864 static int
865 co_ssdo_timer(const struct timespec *tp, void *data)
866 {
867  assert(tp);
868  co_ssdo_t *sdo = data;
869  assert(sdo);
870 
871  co_ssdo_emit_time(sdo, tp);
872 
873  return 0;
874 }
875 
876 static inline void
878 {
879  assert(sdo);
880 
881  if (next)
882  sdo->state = next;
883 }
884 
885 static inline void
886 co_ssdo_emit_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
887 {
888  assert(sdo);
889  assert(sdo->state);
890  assert(sdo->state->on_recv);
891 
892  co_ssdo_enter(sdo, sdo->state->on_abort(sdo, ac));
893 }
894 
895 static inline void
896 co_ssdo_emit_time(co_ssdo_t *sdo, const struct timespec *tp)
897 {
898  assert(sdo);
899  assert(sdo->state);
900  assert(sdo->state->on_time);
901 
902  co_ssdo_enter(sdo, sdo->state->on_time(sdo, tp));
903 }
904 
905 static inline void
906 co_ssdo_emit_recv(co_ssdo_t *sdo, const struct can_msg *msg)
907 {
908  assert(sdo);
909  assert(sdo->state);
910  assert(sdo->state->on_recv);
911 
912  co_ssdo_enter(sdo, sdo->state->on_recv(sdo, msg));
913 }
914 
915 static co_ssdo_state_t *
916 co_ssdo_wait_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
917 {
918  (void)sdo;
919  (void)ac;
920 
921  return NULL;
922 }
923 
924 static co_ssdo_state_t *
925 co_ssdo_wait_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
926 {
927  assert(sdo);
928  assert(msg);
929 
930  if (msg->len < 1)
931  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
932  co_unsigned8_t cs = msg->data[0];
933 
934  switch (cs & CO_SDO_CS_MASK) {
935  case CO_SDO_CCS_DN_INI_REQ: return co_ssdo_dn_ini_on_recv(sdo, msg);
936  case CO_SDO_CCS_UP_INI_REQ: return co_ssdo_up_ini_on_recv(sdo, msg);
937  case CO_SDO_CCS_BLK_DN_REQ: return co_ssdo_blk_dn_ini_on_recv(sdo, msg);
938  case CO_SDO_CCS_BLK_UP_REQ: return co_ssdo_blk_up_ini_on_recv(sdo, msg);
939  case CO_SDO_CS_ABORT: return NULL;
940  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
941  }
942 }
943 
944 static co_ssdo_state_t *
945 co_ssdo_dn_ini_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
946 {
947  assert(sdo);
948  assert(msg);
949 
950  assert(msg->len > 0);
951  co_unsigned8_t cs = msg->data[0];
952 
953  // Load the object index and sub-index from the CAN frame.
954  if (msg->len < 3)
955  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_OBJ);
956  sdo->idx = ldle_u16(msg->data + 1);
957  if (msg->len < 4)
958  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_SUB);
959  sdo->subidx = msg->data[3];
960 
961  trace("SSDO: %04X:%02X: received download request", sdo->idx,
962  sdo->subidx);
963 
964  // Obtain the size from the command specifier.
965  co_sdo_req_clear(&sdo->req);
966  int exp = !!(cs & CO_SDO_INI_SIZE_EXP);
967  if (exp) {
968  if (cs & CO_SDO_INI_SIZE_IND)
969  sdo->req.size = CO_SDO_INI_SIZE_EXP_GET(cs);
970  else
971  sdo->req.size = msg->len - 4;
972  } else if (cs & CO_SDO_INI_SIZE_IND) {
973  // 0-pad the data bytes to handle clients which send CAN frames
974  // less than 8 bytes.
975  uint_least8_t data[4] = { 0 };
976  memcpy(data, msg->data + 4, msg->len - 4);
977  sdo->req.size = ldle_u32(data);
978  }
979 
980  if (exp) {
981  // Perform an expedited transfer.
982  sdo->req.buf = msg->data + 4;
983  sdo->req.nbyte = sdo->req.size;
984  co_unsigned32_t ac = co_ssdo_dn_ind(sdo);
985  if (ac)
986  return co_ssdo_abort_res(sdo, ac);
987  // Finalize the transfer.
989  return co_ssdo_abort_ind(sdo);
990  } else {
992  if (sdo->timeout)
993  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
994  return co_ssdo_dn_seg_state;
995  }
996 }
997 
998 static co_ssdo_state_t *
999 co_ssdo_dn_seg_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1000 {
1001  return co_ssdo_abort_res(sdo, ac);
1002 }
1003 
1004 static co_ssdo_state_t *
1005 co_ssdo_dn_seg_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1006 {
1007  (void)tp;
1008 
1009  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1010 }
1011 
1012 static co_ssdo_state_t *
1013 co_ssdo_dn_seg_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
1014 {
1015  assert(sdo);
1016  assert(msg);
1017 
1018  if (msg->len < 1)
1019  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1020  co_unsigned8_t cs = msg->data[0];
1021 
1022  // Check the client command specifier.
1023  switch (cs & CO_SDO_CS_MASK) {
1024  case CO_SDO_CCS_DN_SEG_REQ: break;
1025  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1026  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1027  }
1028 
1029  // Check the value of the toggle bit.
1030  if ((cs & CO_SDO_SEG_TOGGLE) != sdo->toggle)
1031  return co_ssdo_dn_seg_state;
1032 
1033  // Obtain the size of the segment.
1034  size_t n = CO_SDO_SEG_SIZE_GET(cs);
1035  if (msg->len < 1 + n)
1036  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1037  int last = !!(cs & CO_SDO_SEG_LAST);
1038 
1039  sdo->req.buf = msg->data + 1;
1040  sdo->req.offset += sdo->req.nbyte;
1041  sdo->req.nbyte = n;
1042 
1043  if (last && !co_sdo_req_last(&sdo->req))
1045 
1046  co_unsigned32_t ac = co_ssdo_dn_ind(sdo);
1047  if (ac)
1048  return co_ssdo_abort_res(sdo, ac);
1049 
1051 
1052  if (last) {
1053  return co_ssdo_abort_ind(sdo);
1054  } else {
1055  if (sdo->timeout)
1056  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1057  return co_ssdo_dn_seg_state;
1058  }
1059 }
1060 
1061 static co_ssdo_state_t *
1062 co_ssdo_up_ini_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
1063 {
1064  assert(sdo);
1065  assert(msg);
1066 
1067  // Load the object index and sub-index from the CAN frame.
1068  if (msg->len < 3)
1069  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_OBJ);
1070  sdo->idx = ldle_u16(msg->data + 1);
1071  if (msg->len < 4)
1072  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_SUB);
1073  sdo->subidx = msg->data[3];
1074 
1075  trace("SSDO: %04X:%02X: received upload request", sdo->idx,
1076  sdo->subidx);
1077 
1078  // Perform access checks and start serializing the value.
1079  co_sdo_req_clear(&sdo->req);
1080  co_unsigned32_t ac = co_ssdo_up_ind(sdo);
1081  if (ac)
1082  return co_ssdo_abort_res(sdo, ac);
1083 
1084  if (sdo->req.size && sdo->req.size <= 4) {
1085  // Perform an expedited transfer.
1086  if ((ac = co_ssdo_up_buf(sdo, sdo->req.size)) != 0)
1087  return co_ssdo_abort_res(sdo, ac);
1089  return co_ssdo_abort_ind(sdo);
1090  } else {
1092  if (sdo->timeout)
1093  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1094  return co_ssdo_up_seg_state;
1095  }
1096 }
1097 
1098 static co_ssdo_state_t *
1099 co_ssdo_up_seg_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1100 {
1101  return co_ssdo_abort_res(sdo, ac);
1102 }
1103 
1104 static co_ssdo_state_t *
1105 co_ssdo_up_seg_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1106 {
1107  (void)tp;
1108 
1109  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1110 }
1111 
1112 static co_ssdo_state_t *
1113 co_ssdo_up_seg_on_recv(co_ssdo_t *sdo, const struct can_msg *msg)
1114 {
1115  assert(sdo);
1116  assert(msg);
1117 
1118  if (msg->len < 1)
1119  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1120  co_unsigned8_t cs = msg->data[0];
1121 
1122  // Check the client command specifier.
1123  switch (cs & CO_SDO_CS_MASK) {
1124  case CO_SDO_CCS_UP_SEG_REQ: break;
1125  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1126  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1127  }
1128 
1129  // Check the value of the toggle bit.
1130  if ((cs & CO_SDO_SEG_TOGGLE) != sdo->toggle)
1131  return co_ssdo_abort_res(sdo, CO_SDO_AC_TOGGLE);
1132 
1133  membuf_clear(&sdo->buf);
1134  co_unsigned32_t ac = co_ssdo_up_buf(sdo, 7);
1135  if (ac)
1136  return co_ssdo_abort_res(sdo, ac);
1137 
1138  int last = co_sdo_req_last(&sdo->req) && sdo->nbyte == sdo->req.nbyte;
1139  co_ssdo_send_up_seg_res(sdo, last);
1140 
1141  if (last) {
1142  // Finalize the transfer.
1143  return co_ssdo_abort_ind(sdo);
1144  } else {
1145  if (sdo->timeout)
1146  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1147  return co_ssdo_up_seg_state;
1148  }
1149 }
1150 
1151 static co_ssdo_state_t *
1153 {
1154  assert(sdo);
1155  assert(msg);
1156 
1157  assert(msg->len > 0);
1158  co_unsigned8_t cs = msg->data[0];
1159 
1160  // Check the client subcommand.
1161  if ((cs & 0x01) != CO_SDO_SC_INI_BLK)
1162  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1163 
1164  // Check if the client supports generating a CRC.
1165  sdo->gencrc = !!(cs & CO_SDO_BLK_CRC);
1166 
1167  // Load the object index and sub-index from the CAN frame.
1168  if (msg->len < 3)
1169  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_OBJ);
1170  sdo->idx = ldle_u16(msg->data + 1);
1171  if (msg->len < 4)
1172  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_SUB);
1173  sdo->subidx = msg->data[3];
1174 
1175  trace("SSDO: %04X:%02X: received block download request", sdo->idx,
1176  sdo->subidx);
1177 
1178  // Obtain the data set size.
1179  co_sdo_req_clear(&sdo->req);
1180  if (cs & CO_SDO_BLK_SIZE_IND) {
1181  // 0-pad the data bytes to handle clients which send CAN frames
1182  // less than 8 bytes.
1183  uint_least8_t data[4] = { 0 };
1184  memcpy(data, msg->data + 4, msg->len - 4);
1185  sdo->req.size = ldle_u32(data);
1186  }
1187 
1188  // Use the maximum block size by default.
1189  sdo->blksize = CO_SDO_MAX_SEQNO;
1190  sdo->ackseq = 0;
1191 
1193 
1194  if (sdo->timeout)
1195  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1196  return co_ssdo_blk_dn_sub_state;
1197 }
1198 
1199 static co_ssdo_state_t *
1200 co_ssdo_blk_dn_sub_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1201 {
1202  return co_ssdo_abort_res(sdo, ac);
1203 }
1204 
1205 static co_ssdo_state_t *
1206 co_ssdo_blk_dn_sub_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1207 {
1208  (void)tp;
1209 
1210  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1211 }
1212 
1213 static co_ssdo_state_t *
1215 {
1216  assert(sdo);
1217  assert(msg);
1218 
1219  if (msg->len < 1)
1220  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1221  co_unsigned8_t cs = msg->data[0];
1222 
1223  if (cs == CO_SDO_CS_ABORT)
1224  return co_ssdo_abort_ind(sdo);
1225 
1226  co_unsigned8_t seqno = cs & ~CO_SDO_SEQ_LAST;
1227  int last = !!(cs & CO_SDO_SEQ_LAST);
1228 
1229  if (!seqno || seqno > sdo->blksize)
1230  return co_ssdo_abort_res(sdo, CO_SDO_AC_BLK_SEQ);
1231 
1232  // Only accept sequential segments. Dropped segments will be resent
1233  // after the confirmation message.
1234  if (seqno == sdo->ackseq + 1) {
1235  sdo->ackseq++;
1236  // Update the CRC.
1237  if (sdo->gencrc)
1238  sdo->crc = co_crc(
1239  sdo->crc, sdo->req.buf, sdo->req.nbyte);
1240  // Pass the previous frame to the download indication function.
1241  co_unsigned32_t ac = co_ssdo_dn_ind(sdo);
1242  if (ac)
1243  return co_ssdo_abort_res(sdo, ac);
1244  // Copy the new frame to the SDO request.
1245  membuf_clear(&sdo->buf);
1246  if (!membuf_reserve(&sdo->buf, 7))
1247  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_MEM);
1248  membuf_write(&sdo->buf, msg->data + 1, 7);
1249  sdo->req.buf = membuf_begin(&sdo->buf);
1250  sdo->req.offset += sdo->req.nbyte;
1251  sdo->req.nbyte = membuf_size(&sdo->buf);
1252  }
1253 
1254  // If this is the last segment in the block, send a confirmation.
1255  if (seqno == sdo->blksize || last) {
1257  sdo->ackseq = 0;
1258  }
1259 
1260  if (sdo->timeout)
1261  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1263 }
1264 
1265 static co_ssdo_state_t *
1266 co_ssdo_blk_dn_end_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1267 {
1268  return co_ssdo_abort_res(sdo, ac);
1269 }
1270 
1271 static co_ssdo_state_t *
1272 co_ssdo_blk_dn_end_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1273 {
1274  (void)tp;
1275 
1276  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1277 }
1278 
1279 static co_ssdo_state_t *
1281 {
1282  assert(sdo);
1283  assert(msg);
1284 
1285  if (msg->len < 1)
1286  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1287  co_unsigned8_t cs = msg->data[0];
1288 
1289  // Check the client command specifier.
1290  switch (cs & CO_SDO_CS_MASK) {
1291  case CO_SDO_CCS_BLK_DN_REQ: break;
1292  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1293  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1294  }
1295 
1296  // Check the client subcommand.
1297  if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_END_BLK)
1298  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1299 
1300  // Discard the bytes in the last segment that did not contain data.
1301  sdo->req.nbyte -= 7 - CO_SDO_BLK_SIZE_GET(cs);
1302 
1303  // Check the CRC.
1304  if (sdo->gencrc) {
1305  sdo->crc = co_crc(sdo->crc, sdo->req.buf, sdo->req.nbyte);
1306  co_unsigned16_t crc = ldle_u16(msg->data + 1);
1307  if (sdo->crc != crc)
1308  return co_ssdo_abort_res(sdo, CO_SDO_AC_BLK_CRC);
1309  }
1310 
1311  co_unsigned32_t ac = co_ssdo_dn_ind(sdo);
1312  if (ac)
1313  return co_ssdo_abort_res(sdo, ac);
1314 
1315  // Finalize the transfer.
1317  return co_ssdo_abort_ind(sdo);
1318 }
1319 
1320 static co_ssdo_state_t *
1322 {
1323  assert(sdo);
1324  assert(msg);
1325 
1326  assert(msg->len > 0);
1327  co_unsigned8_t cs = msg->data[0];
1328 
1329  // Check the client subcommand.
1330  if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_INI_BLK)
1331  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1332 
1333  // Check if the client supports generating a CRC.
1334  sdo->gencrc = !!(cs & CO_SDO_BLK_CRC);
1335 
1336  // Load the object index and sub-index from the CAN frame.
1337  if (msg->len < 3)
1338  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_OBJ);
1339  sdo->idx = ldle_u16(msg->data + 1);
1340  if (msg->len < 4)
1341  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_SUB);
1342  sdo->subidx = msg->data[3];
1343 
1344  trace("SSDO: %04X:%02X: received block upload request", sdo->idx,
1345  sdo->subidx);
1346 
1347  // Load the number of segments per block.
1348  if (msg->len < 5)
1350  sdo->blksize = msg->data[4];
1351  if (!sdo->blksize || sdo->blksize > CO_SDO_MAX_SEQNO)
1353 
1354  // Load the protocol switch threshold (PST).
1355  co_unsigned8_t pst = msg->len > 5 ? msg->data[5] : 0;
1356 
1357  // Perform access checks and start serializing the value.
1358  co_sdo_req_clear(&sdo->req);
1359  co_unsigned32_t ac = co_ssdo_up_ind(sdo);
1360  if (ac)
1361  return co_ssdo_abort_res(sdo, ac);
1362 
1363  if (pst && sdo->req.size <= pst) {
1364  // If the PST is non-zero, and the number of bytes is smaller
1365  // than or equal to the PST, switch to the SDO upload protocol.
1366  if (sdo->req.size <= 4) {
1367  // Perform an expedited transfer.
1368  if ((ac = co_ssdo_up_buf(sdo, sdo->req.size)) != 0)
1369  return co_ssdo_abort_res(sdo, ac);
1371  return co_ssdo_abort_ind(sdo);
1372  } else {
1374  if (sdo->timeout)
1375  can_timer_timeout(sdo->timer, sdo->net,
1376  sdo->timeout);
1377  return co_ssdo_up_seg_state;
1378  }
1379  } else {
1381  if (sdo->timeout)
1382  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1383  return co_ssdo_blk_up_sub_state;
1384  }
1385 }
1386 
1387 static co_ssdo_state_t *
1388 co_ssdo_blk_up_sub_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1389 {
1390  return co_ssdo_abort_res(sdo, ac);
1391 }
1392 
1393 static co_ssdo_state_t *
1394 co_ssdo_blk_up_sub_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1395 {
1396  (void)tp;
1397 
1398  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1399 }
1400 
1401 static co_ssdo_state_t *
1403 {
1404  assert(sdo);
1405  assert(msg);
1406 
1407  if (msg->len < 1)
1408  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1409  co_unsigned8_t cs = msg->data[0];
1410 
1411  // Check the client command specifier.
1412  switch (cs & CO_SDO_CS_MASK) {
1413  case CO_SDO_CCS_BLK_UP_REQ: break;
1414  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1415  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1416  }
1417 
1418  // Check the client subcommand.
1419  switch (cs & CO_SDO_SC_MASK) {
1420  case CO_SDO_SC_BLK_RES:
1421  if (co_sdo_req_first(&sdo->req) && !sdo->nbyte)
1422  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1423 
1424  if (msg->len < 3)
1425  return co_ssdo_abort_res(sdo, CO_SDO_AC_BLK_SEQ);
1426 
1427  // Flush the successfully sent segments from the buffer.
1428  co_unsigned8_t ackseq = msg->data[1];
1429  membuf_flush(&sdo->buf, ackseq * 7);
1430 
1431  // Read the number of segments in the next block.
1432  sdo->blksize = msg->data[2];
1433  if (!sdo->blksize || sdo->blksize > CO_SDO_MAX_SEQNO)
1435 
1436  break;
1437  case CO_SDO_SC_START_UP:
1438  if (!(co_sdo_req_first(&sdo->req) && !sdo->nbyte))
1439  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1440  break;
1441  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1442  }
1443 
1444  ptrdiff_t n = sdo->blksize * 7 - membuf_size(&sdo->buf);
1445  if (n > 0) {
1446  if (!membuf_reserve(&sdo->buf, n))
1447  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_MEM);
1448  co_unsigned32_t ac = co_ssdo_up_buf(sdo, n);
1449  if (ac)
1450  return co_ssdo_abort_res(sdo, ac);
1451  sdo->blksize = (co_unsigned8_t)(
1452  (membuf_size(&sdo->buf) + 6) / 7);
1453  }
1454  int last = co_sdo_req_last(&sdo->req) && sdo->nbyte == sdo->req.nbyte;
1455 
1456  if (sdo->timeout)
1457  can_timer_timeout(sdo->timer, sdo->net, sdo->timeout);
1458 
1459  if (sdo->blksize) {
1460  // Send all segments in the current block.
1461  co_ssdo_send_blk_up_sub_res(sdo, last);
1462  return co_ssdo_blk_up_sub_state;
1463  } else {
1465  return co_ssdo_blk_up_end_state;
1466  }
1467 }
1468 
1469 static co_ssdo_state_t *
1470 co_ssdo_blk_up_end_on_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1471 {
1472  return co_ssdo_abort_res(sdo, ac);
1473 }
1474 
1475 static co_ssdo_state_t *
1476 co_ssdo_blk_up_end_on_time(co_ssdo_t *sdo, const struct timespec *tp)
1477 {
1478  (void)tp;
1479 
1480  return co_ssdo_abort_res(sdo, CO_SDO_AC_TIMEOUT);
1481 }
1482 
1483 static co_ssdo_state_t *
1485 {
1486  assert(sdo);
1487  assert(msg);
1488 
1489  if (msg->len < 1)
1490  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1491  co_unsigned8_t cs = msg->data[0];
1492 
1493  // Check the client command specifier.
1494  switch (cs & CO_SDO_CS_MASK) {
1495  case CO_SDO_CCS_BLK_UP_REQ: break;
1496  case CO_SDO_CS_ABORT: return co_ssdo_abort_ind(sdo);
1497  default: return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1498  }
1499 
1500  // Check the client subcommand.
1501  if ((cs & CO_SDO_SC_MASK) != CO_SDO_SC_END_BLK)
1502  return co_ssdo_abort_res(sdo, CO_SDO_AC_NO_CS);
1503 
1504  return co_ssdo_abort_ind(sdo);
1505 }
1506 
1507 static co_ssdo_state_t *
1509 {
1510  assert(sdo);
1511 
1512  if (sdo->timeout)
1513  can_timer_stop(sdo->timer);
1514 
1515  sdo->idx = 0;
1516  sdo->subidx = 0;
1517 
1518  sdo->toggle = 0;
1519  sdo->blksize = 0;
1520  sdo->ackseq = 0;
1521  sdo->gencrc = 0;
1522  sdo->crc = 0;
1523 
1524  co_sdo_req_clear(&sdo->req);
1525  membuf_clear(&sdo->buf);
1526  sdo->nbyte = 0;
1527 
1528  return co_ssdo_wait_state;
1529 }
1530 
1531 static co_ssdo_state_t *
1532 co_ssdo_abort_res(co_ssdo_t *sdo, co_unsigned32_t ac)
1533 {
1534  trace("SSDO: abort code %08" PRIX32 " (%s)", ac, co_sdo_ac2str(ac));
1535  co_ssdo_send_abort(sdo, ac);
1536  return co_ssdo_abort_ind(sdo);
1537 }
1538 
1539 static co_unsigned32_t
1541 {
1542  assert(sdo);
1543 
1544  // Find the object in the object dictionary.
1545  co_obj_t *obj = co_dev_find_obj(sdo->dev, sdo->idx);
1546  if (!obj)
1547  return CO_SDO_AC_NO_OBJ;
1548 
1549  // Find the sub-object.
1550  co_sub_t *sub = co_obj_find_sub(obj, sdo->subidx);
1551  if (!sub)
1552  return CO_SDO_AC_NO_SUB;
1553 
1554  return co_sub_dn_ind(sub, &sdo->req);
1555 }
1556 
1557 static co_unsigned32_t
1559 {
1560  assert(sdo);
1561 
1562  // Find the object in the object dictionary.
1563  const co_obj_t *obj = co_dev_find_obj(sdo->dev, sdo->idx);
1564  if (!obj)
1565  return CO_SDO_AC_NO_OBJ;
1566 
1567  // Find the sub-object.
1568  const co_sub_t *sub = co_obj_find_sub(obj, sdo->subidx);
1569  if (!sub)
1570  return CO_SDO_AC_NO_SUB;
1571 
1572  // If the object is an array, check whether the element exists.
1573  if (co_obj_get_code(obj) == CO_OBJECT_ARRAY
1574  && sdo->subidx > co_obj_get_val_u8(obj, 0x00))
1575  return CO_SDO_AC_NO_DATA;
1576 
1577  sdo->nbyte = 0;
1578  return co_sub_up_ind(sub, &sdo->req);
1579 }
1580 
1581 static co_unsigned32_t
1582 co_ssdo_up_buf(co_ssdo_t *sdo, size_t nbyte)
1583 {
1584  co_unsigned32_t ac = 0;
1585 
1586  if (nbyte && !membuf_reserve(&sdo->buf, nbyte))
1587  return CO_SDO_AC_NO_MEM;
1588 
1589  while (nbyte) {
1590  if (sdo->nbyte >= sdo->req.nbyte) {
1591  if (co_sdo_req_last(&sdo->req)
1592  || (ac = co_ssdo_up_ind(sdo)))
1593  break;
1594  sdo->nbyte = 0;
1595  }
1596  const uint_least8_t *src = (const uint_least8_t *)sdo->req.buf
1597  + sdo->nbyte;
1598  size_t n = MIN(nbyte, sdo->req.nbyte - sdo->nbyte);
1599 
1600  if (sdo->gencrc)
1601  sdo->crc = co_crc(sdo->crc, src, n);
1602 
1603  membuf_write(&sdo->buf, src, n);
1604  nbyte -= n;
1605  sdo->nbyte += n;
1606  }
1607 
1608  return ac;
1609 }
1610 
1611 static void
1612 co_ssdo_send_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
1613 {
1614  assert(sdo);
1615 
1616  struct can_msg msg;
1618  stle_u32(msg.data + 4, ac);
1619  can_net_send(sdo->net, &msg);
1620 }
1621 
1622 static void
1624 {
1625  assert(sdo);
1626 
1627  co_unsigned8_t cs = CO_SDO_SCS_DN_INI_RES;
1628 
1629  struct can_msg msg;
1630  co_ssdo_init_ini_res(sdo, &msg, cs);
1631  can_net_send(sdo->net, &msg);
1632 }
1633 
1634 static void
1636 {
1637  assert(sdo);
1638 
1639  co_unsigned8_t cs = CO_SDO_SCS_DN_SEG_RES | sdo->toggle;
1640  sdo->toggle ^= CO_SDO_SEG_TOGGLE;
1641 
1642  struct can_msg msg;
1643  co_ssdo_init_seg_res(sdo, &msg, cs);
1644  can_net_send(sdo->net, &msg);
1645 }
1646 
1647 static void
1649 {
1650  assert(sdo);
1651  assert(sdo->req.size && sdo->req.size <= 4);
1652 
1653  const char *buf = membuf_begin(&sdo->buf);
1654  size_t nbyte = membuf_size(&sdo->buf);
1655  assert(nbyte == sdo->req.size);
1656 
1657  co_unsigned8_t cs =
1659 
1660  struct can_msg msg;
1661  co_ssdo_init_ini_res(sdo, &msg, cs);
1662  memcpy(msg.data + 4, buf, nbyte);
1663  can_net_send(sdo->net, &msg);
1664 }
1665 
1666 static void
1668 {
1669  assert(sdo);
1670  assert(!sdo->req.size || sdo->req.size > 4);
1671 
1672  co_unsigned8_t cs = CO_SDO_SCS_UP_INI_RES | CO_SDO_INI_SIZE_IND;
1673 
1674  struct can_msg msg;
1675  co_ssdo_init_ini_res(sdo, &msg, cs);
1676  stle_u32(msg.data + 4, sdo->req.size);
1677  can_net_send(sdo->net, &msg);
1678 }
1679 
1680 static void
1682 {
1683  assert(sdo);
1684  assert(!sdo->req.size || sdo->req.size > 4);
1685 
1686  const char *buf = membuf_begin(&sdo->buf);
1687  size_t nbyte = membuf_size(&sdo->buf);
1688  assert(nbyte <= 7);
1689 
1690  co_unsigned8_t cs = CO_SDO_SCS_UP_SEG_RES | sdo->toggle
1691  | CO_SDO_SEG_SIZE_SET(nbyte);
1692  sdo->toggle ^= CO_SDO_SEG_TOGGLE;
1693  if (last)
1694  cs |= CO_SDO_SEG_LAST;
1695 
1696  struct can_msg msg;
1697  co_ssdo_init_seg_res(sdo, &msg, cs);
1698  memcpy(msg.data + 1, buf, nbyte);
1699  can_net_send(sdo->net, &msg);
1700 }
1701 
1702 static void
1704 {
1705  assert(sdo);
1706 
1707  co_unsigned8_t cs = CO_SDO_SCS_BLK_DN_RES | CO_SDO_BLK_CRC
1709 
1710  struct can_msg msg;
1711  co_ssdo_init_ini_res(sdo, &msg, cs);
1712  msg.data[4] = sdo->blksize;
1713  can_net_send(sdo->net, &msg);
1714 }
1715 
1716 static void
1718 {
1719  assert(sdo);
1720 
1721  co_unsigned8_t cs = CO_SDO_SCS_BLK_DN_RES | CO_SDO_SC_BLK_RES;
1722 
1723  struct can_msg msg;
1724  co_ssdo_init_seg_res(sdo, &msg, cs);
1725  msg.data[1] = sdo->ackseq;
1726  msg.data[2] = sdo->blksize;
1727  can_net_send(sdo->net, &msg);
1728 }
1729 
1730 static void
1732 {
1733  assert(sdo);
1734 
1735  co_unsigned8_t cs = CO_SDO_SCS_BLK_DN_RES | CO_SDO_SC_END_BLK;
1736 
1737  struct can_msg msg;
1738  co_ssdo_init_seg_res(sdo, &msg, cs);
1739  can_net_send(sdo->net, &msg);
1740 }
1741 
1742 static void
1744 {
1745  assert(sdo);
1746 
1747  co_unsigned8_t cs = CO_SDO_SCS_BLK_UP_RES | CO_SDO_BLK_CRC
1749 
1750  struct can_msg msg;
1751  co_ssdo_init_ini_res(sdo, &msg, cs);
1752  stle_u32(msg.data + 4, sdo->req.size);
1753  can_net_send(sdo->net, &msg);
1754 }
1755 
1756 static void
1758 {
1759  assert(sdo);
1760 
1761  const char *buf = membuf_begin(&sdo->buf);
1762  size_t nbyte = membuf_size(&sdo->buf);
1763 
1764  for (co_unsigned8_t seqno = 1; seqno <= sdo->blksize;
1765  seqno++, buf += 7, nbyte -= 7) {
1766  co_unsigned8_t cs = seqno;
1767  if (last && nbyte <= 7)
1768  cs |= CO_SDO_SEQ_LAST;
1769 
1770  struct can_msg msg;
1771  co_ssdo_init_seg_res(sdo, &msg, cs);
1772  memcpy(msg.data + 1, buf, MIN(nbyte, 7));
1773  can_net_send(sdo->net, &msg);
1774  }
1775 }
1776 
1777 static void
1779 {
1780  assert(sdo);
1781 
1782  // Compute the number of bytes in the last segment containing data.
1783  co_unsigned8_t n = sdo->req.size ? (sdo->req.size - 1) % 7 + 1 : 0;
1784 
1785  co_unsigned8_t cs = CO_SDO_SCS_BLK_UP_RES | CO_SDO_SC_END_BLK
1786  | CO_SDO_BLK_SIZE_SET(n);
1787 
1788  struct can_msg msg;
1789  co_ssdo_init_seg_res(sdo, &msg, cs);
1790  stle_u16(msg.data + 1, sdo->crc);
1791  can_net_send(sdo->net, &msg);
1792 }
1793 
1794 static void
1795 co_ssdo_init_ini_res(co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
1796 {
1797  assert(sdo);
1798  assert(msg);
1799 
1800  *msg = (struct can_msg)CAN_MSG_INIT;
1801  msg->id = sdo->par.cobid_res;
1802  if (sdo->par.cobid_res & CO_SDO_COBID_FRAME) {
1803  msg->id &= CAN_MASK_EID;
1804  msg->flags |= CAN_FLAG_IDE;
1805  } else {
1806  msg->id &= CAN_MASK_BID;
1807  }
1808  msg->len = CAN_MAX_LEN;
1809  msg->data[0] = cs;
1810  stle_u16(msg->data + 1, sdo->idx);
1811  msg->data[3] = sdo->subidx;
1812 }
1813 
1814 static void
1815 co_ssdo_init_seg_res(co_ssdo_t *sdo, struct can_msg *msg, co_unsigned8_t cs)
1816 {
1817  assert(sdo);
1818  assert(msg);
1819 
1820  *msg = (struct can_msg)CAN_MSG_INIT;
1821  msg->id = sdo->par.cobid_res;
1822  if (sdo->par.cobid_res & CO_SDO_COBID_FRAME) {
1823  msg->id &= CAN_MASK_EID;
1824  msg->flags |= CAN_FLAG_IDE;
1825  } else {
1826  msg->id &= CAN_MASK_BID;
1827  }
1828  msg->len = CAN_MAX_LEN;
1829  msg->data[0] = cs;
1830 }
@ 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:207
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:288
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:586
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:506
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:494
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:572
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:825
@ ERRNUM_INVAL
Invalid argument.
Definition: errnum.h:129
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
Definition: errnum.c:947
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:957
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:43
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:528
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:916
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
Definition: obj.c:154
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:1019
void * co_obj_addressof_val(const co_obj_t *obj)
Returns the address of the value of a CANopen object.
Definition: obj.c:311
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:954
co_unsigned8_t co_obj_get_code(const co_obj_t *obj)
Returns the object code of a CANopen object.
Definition: obj.c:286
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:225
#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:372
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:520
size_t co_obj_sizeof_val(const co_obj_t *obj)
Returns size (in bytes) of the value of a CANopen object.
Definition: obj.c:317
co_unsigned16_t co_sub_get_type(const co_sub_t *sub)
Returns the data type of a CANopen sub-object.
Definition: obj.c:570
int co_sdo_req_first(const struct co_sdo_req *req)
Returns 1 if the specified request includes the first segment, and 0 otherwise.
Definition: sdo.h:340
int co_sdo_req_dn_val(struct co_sdo_req *req, co_unsigned16_t type, void *val, co_unsigned32_t *pac)
Copies the next segment of the specified CANopen SDO download request to the internal buffer and,...
Definition: sdo.c:165
#define CO_SDO_AC_TOGGLE
SDO abort code: Toggle bit not altered.
Definition: sdo.h:63
#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_TYPE_LEN
SDO abort code: Data type does not match, length of service parameter does not match.
Definition: sdo.h:117
#define CO_SDO_AC_BLK_SEQ
SDO abort code: Invalid sequence number (block mode only).
Definition: sdo.h:75
#define CO_SDO_AC_NO_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:346
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_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:144
size_t membuf_reserve(struct membuf *buf, size_t size)
Resizes a memory buffer, if necessary, to make room for at least an additional size bytes.
Definition: membuf.c:46
void membuf_fini(struct membuf *buf)
Finalizes a memory buffer.
Definition: membuf.c:38
void membuf_flush(struct membuf *buf, size_t size)
Flushes size bytes from the beginning of a memory buffer.
Definition: membuf.c:75
size_t membuf_write(struct membuf *buf, const void *ptr, size_t size)
Writes data to a memory buffer.
Definition: membuf.h:192
void membuf_clear(struct membuf *buf)
Clears a memory buffer.
Definition: membuf.h:150
void membuf_init(struct membuf *buf)
Initializes a memory buffer.
Definition: membuf.h:138
size_t membuf_size(const struct membuf *buf)
Returns the total number of bytes written to a memory buffer.
Definition: membuf.h:156
void can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
Definition: net.c:468
int can_net_send(can_net_t *net, const struct can_msg *msg)
Sends a CAN frame from a network interface.
Definition: net.c:308
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:428
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:537
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:613
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:582
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:562
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:382
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:484
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:591
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:407
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
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:675
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:916
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:925
static void co_ssdo_send_blk_up_ini_res(co_ssdo_t *sdo)
Sends a Server-SDO 'block upload initiate' response.
Definition: ssdo.c:1743
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:1105
void co_ssdo_destroy(co_ssdo_t *ssdo)
Destroys a CANopen Server-SDO service.
Definition: ssdo.c:657
static void co_ssdo_send_dn_seg_res(co_ssdo_t *sdo)
Sends a Server-SDO 'download segment' response.
Definition: ssdo.c:1635
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:1113
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:1266
co_unsigned8_t co_ssdo_get_num(const co_ssdo_t *sdo)
Returns the SDO number of a Server-SDO.
Definition: ssdo.c:683
static co_ssdo_state_t *const co_ssdo_blk_dn_end_state
The 'block download end' state.
Definition: ssdo.c:317
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:1795
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:1757
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:1206
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:1280
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:1558
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:1005
static void co_ssdo_send_blk_up_end_res(co_ssdo_t *sdo)
Sends a Server-SDO 'block upload end' response.
Definition: ssdo.c:1778
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:1484
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:1388
static void co_ssdo_send_up_seg_res(co_ssdo_t *sdo, int last)
Sends a Server-SDO 'upload segment' response.
Definition: ssdo.c:1681
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:1540
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:1717
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:1214
void co_ssdo_set_timeout(co_ssdo_t *sdo, int timeout)
Sets the timeout of a Server-SDO.
Definition: ssdo.c:707
static int co_ssdo_update(co_ssdo_t *sdo)
Updates and (de)activates a Server-SDO service.
Definition: ssdo.c:718
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:896
static co_ssdo_state_t *const co_ssdo_blk_up_sub_state
The 'block upload sub-block' state.
Definition: ssdo.c:353
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:745
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:1152
static co_ssdo_state_t *const co_ssdo_blk_up_end_state
The 'block upload end' state.
Definition: ssdo.c:376
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:843
static int co_ssdo_timer(const struct timespec *tp, void *data)
The CAN timer callback function for a Server-SDO service.
Definition: ssdo.c:865
static co_ssdo_state_t *const co_ssdo_dn_seg_state
The 'download segment' state.
Definition: ssdo.c:227
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:1062
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:1099
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:906
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:1582
static void co_ssdo_send_dn_ini_res(co_ssdo_t *sdo)
Sends a Server-SDO 'download initiate' response.
Definition: ssdo.c:1623
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:1532
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:945
static void co_ssdo_send_abort(co_ssdo_t *sdo, co_unsigned32_t ac)
Sends an abort transfer request.
Definition: ssdo.c:1612
static void co_ssdo_send_blk_dn_end_res(co_ssdo_t *sdo)
Sends a Server-SDO 'block download end' response.
Definition: ssdo.c:1731
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:1272
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:999
static co_ssdo_state_t *const co_ssdo_blk_dn_sub_state
The 'block download sub-block' state.
Definition: ssdo.c:293
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:1013
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:1815
static void co_ssdo_send_blk_dn_ini_res(co_ssdo_t *sdo)
Sends a Server-SDO 'block download initiate' response.
Definition: ssdo.c:1703
static co_ssdo_state_t *const co_ssdo_up_seg_state
The 'upload segment' state.
Definition: ssdo.c:257
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:1394
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:1402
int co_ssdo_get_timeout(const co_ssdo_t *sdo)
Returns the timeout (in milliseconds) of a Server-SDO.
Definition: ssdo.c:699
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:1470
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:1321
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:1508
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:691
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:667
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:877
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:1200
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:1476
static void co_ssdo_send_up_exp_res(co_ssdo_t *sdo)
Sends a Server-SDO 'upload initiate' (expedited) response.
Definition: ssdo.c:1648
static co_ssdo_state_t *const co_ssdo_wait_state
The 'waiting' state.
Definition: ssdo.c:192
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:886
static void co_ssdo_send_up_ini_res(co_ssdo_t *sdo)
Sends a Server-SDO 'upload initiate' response.
Definition: ssdo.c:1667
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:630
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....
A CAN network interface.
Definition: net.c:37
A CAN frame receiver.
Definition: net.c:99
A CAN timer.
Definition: net.c:63
A CANopen device.
Definition: dev.c:41
A CANopen object.
Definition: obj.h:32
A CANopen Server-SDO state.
Definition: ssdo.c:144
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:173
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:163
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:154
A CANopen Server-SDO.
Definition: ssdo.c:43
struct co_sdo_par par
The SDO parameter record.
Definition: ssdo.c:51
co_unsigned16_t idx
The current object index.
Definition: ssdo.c:61
co_ssdo_state_t * state
A pointer to the current state.
Definition: ssdo.c:59
can_net_t * net
A pointer to a CAN network interface.
Definition: ssdo.c:45
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: ssdo.c:53
int timeout
The SDO timeout (in milliseconds).
Definition: ssdo.c:55
co_dev_t * dev
A pointer to a CANopen device.
Definition: ssdo.c:47
unsigned gencrc
A flag indicating whether a CRC should be generated.
Definition: ssdo.c:71
struct membuf buf
The buffer.
Definition: ssdo.c:77
can_timer_t * timer
A pointer to the CAN timer.
Definition: ssdo.c:57
co_unsigned16_t crc
The generated CRC.
Definition: ssdo.c:73
co_unsigned8_t subidx
The current object sub-index.
Definition: ssdo.c:63
struct co_sdo_req req
The SDO request.
Definition: ssdo.c:75
co_unsigned8_t blksize
The number of segments per block.
Definition: ssdo.c:67
co_unsigned8_t toggle
The current value of the toggle bit.
Definition: ssdo.c:65
co_unsigned8_t num
The SDO number.
Definition: ssdo.c:49
size_t nbyte
The number of bytes in req already copied to buf.
Definition: ssdo.c:79
co_unsigned8_t ackseq
The sequence number of the last successfully received segment.
Definition: ssdo.c:69
A CANopen sub-object.
Definition: obj.h:54
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:178
size_t offset
The offset of the bytes at buf.
Definition: sdo.h:193
size_t size
The total size (in bytes) of the value to be uploaded/downloaded.
Definition: sdo.h:184
const void * buf
A pointer to the next bytes to be uploaded/downloaded.
Definition: sdo.h:186
size_t nbyte
The number of bytes available at buf.
Definition: sdo.h:188
A memory buffer.
Definition: membuf.h:35
#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:163
This header file is part of the CANopen library; it contains the CANopen value declarations.
void co_val_fini(co_unsigned16_t type, void *val)
Finalizes a value of the specified data type.
Definition: val.c:275