Lely core libraries  2.2.5
time.c
Go to the documentation of this file.
1 
24 #include "co.h"
25 
26 #ifndef LELY_NO_CO_TIME
27 
28 #include <lely/co/dev.h>
29 #include <lely/co/obj.h>
30 #include <lely/co/sdo.h>
31 #include <lely/co/time.h>
32 #include <lely/co/val.h>
33 #include <lely/util/endian.h>
34 #include <lely/util/errnum.h>
35 #include <lely/util/time.h>
36 
37 #include <assert.h>
38 #include <stdlib.h>
39 
41 struct __co_time {
47  co_unsigned32_t cobid;
55  struct timespec start;
59  void *data;
60 };
61 
69 static int co_time_update(co_time_t *time);
70 
77 static co_unsigned32_t co_1012_dn_ind(
78  co_sub_t *sub, struct co_sdo_req *req, void *data);
79 
85 static int co_time_recv(const struct can_msg *msg, void *data);
86 
92 static int co_time_timer(const struct timespec *tp, void *data);
93 
94 void
95 co_time_of_day_get(const co_time_of_day_t *tod, struct timespec *tp)
96 {
97  assert(tod);
98  assert(tp);
99 
100  // Convert the CANopen time (seconds since January 1, 1984) to the Unix
101  // epoch (seconds since January 1, 1970). This is a difference of 14
102  // years and 3 leap days.
103  co_time_diff_get((const co_time_diff_t *)tod, tp);
104  tp->tv_sec += (14 * 365 + 3) * 24 * 60 * 60;
105 }
106 
107 void
108 co_time_of_day_set(co_time_of_day_t *tod, const struct timespec *tp)
109 {
110  assert(tod);
111  assert(tp);
112 
113  // Convert the Unix epoch (seconds since January 1, 1970) to the CANopen
114  // time (seconds since January 1, 1984). This is a difference of 14
115  // years and 3 leap days.
116  // clang-format off
117  co_time_diff_set((co_time_diff_t *)tod, &(struct timespec){
118  tp->tv_sec - (14 * 365 + 3) * 24 * 60 * 60,
119  tp->tv_nsec });
120  // clang-format on
121 }
122 
123 void
124 co_time_diff_get(const co_time_diff_t *td, struct timespec *tp)
125 {
126  assert(td);
127  assert(tp);
128 
129  tp->tv_sec = (time_t)td->days * 24 * 60 * 60 + td->ms / 1000;
130  tp->tv_nsec = (long)(td->ms % 1000) * 1000000;
131 }
132 
133 void
134 co_time_diff_set(co_time_diff_t *td, const struct timespec *tp)
135 {
136  assert(td);
137  assert(tp);
138 
139  // Compute the number of milliseconds since midnight.
140  td->ms = (tp->tv_sec % (24 * 60 * 60)) * 1000 + tp->tv_nsec / 1000000;
141  // Compute the number of days.
142  td->days = (co_unsigned16_t)(tp->tv_sec / (24 * 60 * 60));
143 }
144 
145 void *
146 __co_time_alloc(void)
147 {
148  void *ptr = malloc(sizeof(struct __co_time));
149  if (!ptr)
150  set_errc(errno2c(errno));
151  return ptr;
152 }
153 
154 void
155 __co_time_free(void *ptr)
156 {
157  free(ptr);
158 }
159 
160 struct __co_time *
161 __co_time_init(struct __co_time *time, can_net_t *net, co_dev_t *dev)
162 {
163  assert(time);
164  assert(net);
165  assert(dev);
166 
167  int errc = 0;
168 
169  time->net = net;
170  time->dev = dev;
171 
172  // Retrieve the TIME COB-ID.
173  co_obj_t *obj_1012 = co_dev_find_obj(time->dev, 0x1012);
174  if (!obj_1012) {
175  errc = errnum2c(ERRNUM_NOSYS);
176  goto error_obj_1012;
177  }
178 
179  time->cobid = co_obj_get_val_u32(obj_1012, 0x00);
180  time->sub_1013_00 = co_dev_find_sub(time->dev, 0x1013, 0x00);
181 
182  time->recv = NULL;
183  time->timer = NULL;
184 
185  can_net_get_time(time->net, &time->start);
186 
187  time->ind = NULL;
188  time->data = NULL;
189 
190  // Set the download indication function for the TIME COB-ID object.
191  co_obj_set_dn_ind(obj_1012, &co_1012_dn_ind, time);
192 
193  if (co_time_update(time) == -1) {
194  errc = get_errc();
195  goto error_update;
196  }
197 
198  return time;
199 
200 error_update:
201  co_obj_set_dn_ind(obj_1012, NULL, NULL);
202 error_obj_1012:
203  set_errc(errc);
204  return NULL;
205 }
206 
207 void
208 __co_time_fini(struct __co_time *time)
209 {
210  assert(time);
211 
212  // Remove the download indication function for the TIME COB-ID object.
213  co_obj_t *obj_1012 = co_dev_find_obj(time->dev, 0x1012);
214  assert(obj_1012);
215  co_obj_set_dn_ind(obj_1012, NULL, NULL);
216 
217  can_timer_destroy(time->timer);
218 
219  can_recv_destroy(time->recv);
220 }
221 
222 co_time_t *
224 {
225  trace("creating TIME service");
226 
227  int errc = 0;
228 
229  co_time_t *time = __co_time_alloc();
230  if (!time) {
231  errc = get_errc();
232  goto error_alloc_time;
233  }
234 
235  if (!__co_time_init(time, net, dev)) {
236  errc = get_errc();
237  goto error_init_time;
238  }
239 
240  return time;
241 
242 error_init_time:
243  __co_time_free(time);
244 error_alloc_time:
245  set_errc(errc);
246  return NULL;
247 }
248 
249 void
251 {
252  if (time) {
253  trace("destroying TIME service");
254  __co_time_fini(time);
255  __co_time_free(time);
256  }
257 }
258 
259 can_net_t *
261 {
262  assert(time);
263 
264  return time->net;
265 }
266 
267 co_dev_t *
269 {
270  assert(time);
271 
272  return time->dev;
273 }
274 
275 void
276 co_time_get_ind(const co_time_t *time, co_time_ind_t **pind, void **pdata)
277 {
278  assert(time);
279 
280  if (pind)
281  *pind = time->ind;
282  if (pdata)
283  *pdata = time->data;
284 }
285 
286 void
288 {
289  assert(time);
290 
291  time->ind = ind;
292  time->data = data;
293 }
294 
295 void
296 co_time_start(co_time_t *time, const struct timespec *start,
297  const struct timespec *interval)
298 {
299  assert(time);
300 
301  if (time->timer)
302  can_timer_start(time->timer, time->net, start, interval);
303 }
304 
305 void
307 {
308  assert(time);
309 
310  if (time->timer)
311  can_timer_stop(time->timer);
312 }
313 
314 static int
316 {
317  assert(time);
318 
319  if (time->cobid & CO_TIME_COBID_CONSUMER) {
320  if (!time->recv) {
321  time->recv = can_recv_create();
322  if (!time->recv)
323  return -1;
324  can_recv_set_func(time->recv, &co_time_recv, time);
325  }
326  // Register the receiver under the specified CAN-ID.
327  uint_least32_t id = time->cobid;
328  uint_least8_t flags = 0;
329  if (id & CO_TIME_COBID_FRAME) {
330  id &= CAN_MASK_EID;
331  flags |= CAN_FLAG_IDE;
332  } else {
333  id &= CAN_MASK_BID;
334  }
335  can_recv_start(time->recv, time->net, id, flags);
336  } else if (time->recv) {
337  can_recv_destroy(time->recv);
338  time->recv = NULL;
339  }
340 
341  if (time->cobid & CO_TIME_COBID_PRODUCER) {
342  if (!time->timer) {
343  time->timer = can_timer_create();
344  if (!time->timer)
345  return -1;
346  can_timer_set_func(time->timer, &co_time_timer, time);
347  }
348  } else if (time->timer) {
349  can_timer_destroy(time->timer);
350  time->timer = NULL;
351  }
352 
353  return 0;
354 }
355 
356 static co_unsigned32_t
357 co_1012_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
358 {
359  assert(sub);
360  assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1012);
361  assert(req);
362  co_time_t *time = data;
363  assert(time);
364 
365  co_unsigned32_t ac = 0;
366 
367  co_unsigned16_t type = co_sub_get_type(sub);
368  union co_val val;
369  if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
370  return ac;
371 
372  if (co_sub_get_subidx(sub)) {
373  ac = CO_SDO_AC_NO_SUB;
374  goto error;
375  }
376 
377  assert(type == CO_DEFTYPE_UNSIGNED32);
378  co_unsigned32_t cobid = val.u32;
379  co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
380  if (cobid == cobid_old)
381  goto error;
382 
383  // The CAN-ID cannot be changed while the producer or consumer is and
384  // remains active.
385  int active = (cobid & CO_TIME_COBID_PRODUCER)
386  || (cobid & CO_TIME_COBID_CONSUMER);
387  int active_old = (cobid_old & CO_TIME_COBID_PRODUCER)
388  || (cobid_old & CO_TIME_COBID_CONSUMER);
389  uint_least32_t canid = cobid & CAN_MASK_EID;
390  uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
391  if (active && active_old && canid != canid_old) {
392  ac = CO_SDO_AC_PARAM_VAL;
393  goto error;
394  }
395 
396  // A 29-bit CAN-ID is only valid if the frame bit is set.
397  if (!(cobid & CO_TIME_COBID_FRAME)
398  && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID))) {
399  ac = CO_SDO_AC_PARAM_VAL;
400  goto error;
401  }
402 
403  time->cobid = cobid;
404 
405  co_sub_dn(sub, &val);
406  co_val_fini(type, &val);
407 
408  co_time_update(time);
409  return 0;
410 
411 error:
412  co_val_fini(type, &val);
413  return ac;
414 }
415 
416 static int
417 co_time_recv(const struct can_msg *msg, void *data)
418 {
419  assert(msg);
420  co_time_t *time = data;
421  assert(time);
422 
423  // Ignore remote frames.
424  if (msg->flags & CAN_FLAG_RTR)
425  return 0;
426 
427 #ifndef LELY_NO_CANFD
428  // Ignore CAN FD format frames.
429  if (msg->flags & CAN_FLAG_EDL)
430  return 0;
431 #endif
432 
433  if (msg->len < 6)
434  return 0;
435 
436  co_time_of_day_t tod;
437  tod.ms = ldle_u32(msg->data) & UINT32_C(0x0fffffff);
438  tod.days = ldle_u16(msg->data + 4);
439 
440  struct timespec tv;
441  co_time_of_day_get(&tod, &tv);
442 
443  if (time->ind)
444  time->ind(time, &tv, time->data);
445 
446  return 0;
447 }
448 
449 static int
450 co_time_timer(const struct timespec *tp, void *data)
451 {
452  assert(tp);
453  co_time_t *time = data;
454  assert(time);
455 
456  // Update the high-resolution time stamp, if it exists.
457  if (time->sub_1013_00)
458  co_sub_set_val_u32(time->sub_1013_00,
459  (co_unsigned32_t)timespec_diff_usec(
460  tp, &time->start));
461 
462  // Convert the time to a TIME_OF_DAY value.
463  co_time_of_day_t tod = { 0, 0 };
464  co_time_of_day_set(&tod, tp);
465 
466  struct can_msg msg = CAN_MSG_INIT;
467  msg.id = time->cobid;
468  if (time->cobid & CO_TIME_COBID_FRAME) {
469  msg.id &= CAN_MASK_EID;
470  msg.flags |= CAN_FLAG_IDE;
471  } else {
472  msg.id &= CAN_MASK_BID;
473  }
474  msg.len = 6;
475  stle_u32(msg.data, tod.ms & UINT32_C(0x0fffffff));
476  stle_u16(msg.data + 4, tod.days);
477  can_net_send(time->net, &msg);
478 
479  return 0;
480 }
481 
482 #endif // !LELY_NO_CO_TIME
can_recv_destroy
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:562
co_sub_dn
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
can_msg::flags
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
can_msg::data
uint_least8_t data[CAN_MSG_MAX_LEN]
The frame payload (in case of a data frame).
Definition: msg.h:102
CAN_MASK_EID
#define CAN_MASK_EID
The mask used to extract the 29-bit Extended Identifier from a CAN frame.
Definition: msg.h:34
dev.h
can_net_get_time
void can_net_get_time(const can_net_t *net, struct timespec *tp)
Retrieves the current time of a CAN network interface.
Definition: net.c:204
val.h
co_val_fini
void co_val_fini(co_unsigned16_t type, void *val)
Finalizes a value of the specified data type.
Definition: val.c:275
CO_DEFTYPE_UNSIGNED32
#define CO_DEFTYPE_UNSIGNED32
The data type (and object index) of a 32-bit unsigned integer.
Definition: type.h:50
CAN_MASK_BID
#define CAN_MASK_BID
The mask used to extract the 11-bit Base Identifier from a CAN frame.
Definition: msg.h:31
co_time_start
void co_time_start(co_time_t *time, const struct timespec *start, const struct timespec *interval)
Starts a CANopen TIME producer.
Definition: time.c:296
co_time_of_day_get
void co_time_of_day_get(const co_time_of_day_t *tod, struct timespec *tp)
Loads the absolute time from a CANopen TIME_OF_DAY value.
Definition: time.c:95
__co_time
A CANopen TIME producer/consumer service.
Definition: time.c:41
can_msg
A CAN or CAN FD format frame.
Definition: msg.h:87
co_time_update
static int co_time_update(co_time_t *time)
Updates and (de)activates a TIME producer/consumer service.
Definition: time.c:315
can_msg::len
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
can_timer_set_func
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_timer_stop
void can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
Definition: net.c:468
co_obj_get_idx
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
Definition: obj.c:154
ldle_u32
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
__can_recv
A CAN frame receiver.
Definition: net.c:99
can_recv_set_func
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
__co_time::net
can_net_t * net
A pointer to a CAN network interface.
Definition: time.c:43
get_errc
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
Definition: errnum.c:947
CO_TIME_COBID_FRAME
#define CO_TIME_COBID_FRAME
The bit in the TIME COB-ID specifying whether to use an 11-bit (0) or 29-bit (1) CAN-ID.
Definition: time.h:38
timespec_diff_usec
int_least64_t timespec_diff_usec(const struct timespec *t1, const struct timespec *t2)
Returns the time difference (in microseconds) between *t1 and *t2.
Definition: time.h:214
errno2c
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:43
__co_time::cobid
co_unsigned32_t cobid
The TIME COB-ID.
Definition: time.c:47
co.h
time.h
sdo.h
CO_TIME_COBID_CONSUMER
#define CO_TIME_COBID_CONSUMER
The bit in the TIME COB-ID specifying whether the device is a consumer.
Definition: time.h:29
can_timer_start
void can_timer_start(can_timer_t *timer, can_net_t *net, const struct timespec *start, const struct timespec *interval)
Starts a CAN timer and registers it with a network interface.
Definition: net.c:437
CAN_MSG_INIT
#define CAN_MSG_INIT
The static initializer for can_msg.
Definition: msg.h:113
co_1012_dn_ind
static co_unsigned32_t co_1012_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
The download indication function for (all sub-objects of) CANopen object 1012 (COB-ID time stamp obje...
Definition: time.c:357
co_time_diff_set
void co_time_diff_set(co_time_diff_t *td, const struct timespec *tp)
Stores a time difference into a CANopen TIME_DIFFERENCE value.
Definition: time.c:134
co_obj_set_dn_ind
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_time::data
void * data
A pointer to user-specified data for ind.
Definition: time.c:59
__can_timer
A CAN timer.
Definition: net.c:63
set_errc
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:957
__co_time::dev
co_dev_t * dev
A pointer to a CANopen device.
Definition: time.c:45
can_timer_destroy
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:407
CO_SDO_AC_NO_SUB
#define CO_SDO_AC_NO_SUB
SDO abort code: Sub-index does not exist.
Definition: sdo.h:132
errnum2c
int errnum2c(errnum_t errnum)
Transforms a platform-independent error number to a native error code.
Definition: errnum.c:825
co_sdo_req
A CANopen SDO upload/download request.
Definition: sdo.h:178
co_time_recv
static int co_time_recv(const struct can_msg *msg, void *data)
The CAN receive callback function for a TIME consumer service.
Definition: time.c:417
errnum.h
co_time_get_dev
co_dev_t * co_time_get_dev(const co_time_t *time)
Returns a pointer to the CANopen device of a TIME producer/consumer service.
Definition: time.c:268
__co_time::recv
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition: time.c:51
stle_u32
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
co_time_ind_t
void co_time_ind_t(co_time_t *time, const struct timespec *tp, void *data)
The type of a CANopen TIME indication function, invoked when a time stamp is received.
Definition: time.h:52
time.h
__co_time::ind
co_time_ind_t * ind
A pointer to the indication function.
Definition: time.c:57
co_time_of_day_set
void co_time_of_day_set(co_time_of_day_t *tod, const struct timespec *tp)
Stores the absolute time into a CANopen TIME_OF_DAY value.
Definition: time.c:108
co_val
A union of the CANopen static data types.
Definition: val.h:163
co_time_get_ind
void co_time_get_ind(const co_time_t *time, co_time_ind_t **pind, void **pdata)
Retrieves the indication function invoked when a CANopen time stamp is received.
Definition: time.c:276
can_recv_start
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
co_sdo_req_dn_val
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
__co_obj
A CANopen object.
Definition: obj.h:32
CO_SDO_AC_PARAM_VAL
#define CO_SDO_AC_PARAM_VAL
SDO abort code: Invalid value for parameter (download only).
Definition: sdo.h:135
co_time_timer
static int co_time_timer(const struct timespec *tp, void *data)
The CAN timer callback function for a TIME producer service.
Definition: time.c:450
CAN_FLAG_RTR
@ CAN_FLAG_RTR
The Remote Transmission Request (RTR) flag (unavailable in CAN FD format frames).
Definition: msg.h:48
__co_dev
A CANopen device.
Definition: dev.c:41
co_sub_get_obj
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
can_net_send
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
__co_time::start
struct timespec start
The creation time of the service.
Definition: time.c:55
co_time_set_ind
void co_time_set_ind(co_time_t *time, co_time_ind_t *ind, void *data)
Sets the indication function invoked when a CANopen time stamp is received.
Definition: time.c:287
__co_time::sub_1013_00
co_sub_t * sub_1013_00
A pointer to the high-resolution time stamp sub-object.
Definition: time.c:49
co_time_create
co_time_t * co_time_create(can_net_t *net, co_dev_t *dev)
Creates a new CANopen TIME producer/consumer service.
Definition: time.c:223
stle_u16
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
co_time_diff_get
void co_time_diff_get(const co_time_diff_t *td, struct timespec *tp)
Loads a time difference from a CANopen TIME_DIFFERENCE value.
Definition: time.c:124
co_sub_get_subidx
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
ldle_u16
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
stdlib.h
__co_sub
A CANopen sub-object.
Definition: obj.h:54
co_time_stop
void co_time_stop(co_time_t *time)
Stops a CANopen TIME producer.
Definition: time.c:306
co_dev_find_sub
co_sub_t * co_dev_find_sub(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
Finds a sub-object in the object dictionary of a CANopen device.
Definition: dev.c:299
CO_TIME_COBID_PRODUCER
#define CO_TIME_COBID_PRODUCER
The bit in the TIME COB-ID specifying whether the device is a producer.
Definition: time.h:32
co_sub_get_type
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
__co_time::timer
can_timer_t * timer
A pointer to the CAN timer.
Definition: time.c:53
co_time_get_net
can_net_t * co_time_get_net(const co_time_t *time)
Returns a pointer to the CAN network of a TIME producer/consumer service.
Definition: time.c:260
CAN_FLAG_IDE
@ CAN_FLAG_IDE
The Identifier Extension (IDE) flag.
Definition: msg.h:43
obj.h
__can_net
A CAN network interface.
Definition: net.c:37
can_recv_create
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:537
can_timer_create
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:382
ERRNUM_NOSYS
@ ERRNUM_NOSYS
Function not supported.
Definition: errnum.h:181
can_msg::id
uint_least32_t id
The identifier (11 or 29 bits, depending on the CAN_FLAG_IDE flag).
Definition: msg.h:89
co_dev_find_obj
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
co_time_destroy
void co_time_destroy(co_time_t *time)
Destroys a CANopen TIME producer/consumer service.
Definition: time.c:250
endian.h