Lely core libraries 2.3.4
time.c
Go to the documentation of this file.
1
24#include "co.h"
25
26#if !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
63
69static void co_time_update(co_time_t *time);
70
77static co_unsigned32_t co_1012_dn_ind(
78 co_sub_t *sub, struct co_sdo_req *req, void *data);
79
85static int co_time_recv(const struct can_msg *msg, void *data);
86
92static int co_time_timer(const struct timespec *tp, void *data);
93
94void
95co_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 const co_time_diff_t td = { .ms = tod->ms, .days = tod->days };
104 co_time_diff_get(&td, tp);
105 tp->tv_sec += (14 * 365 + 3) * 24 * 60 * 60;
106}
107
108void
109co_time_of_day_set(co_time_of_day_t *tod, const struct timespec *tp)
110{
111 assert(tod);
112 assert(tp);
113
114 // Convert the Unix epoch (seconds since January 1, 1970) to the CANopen
115 // time (seconds since January 1, 1984). This is a difference of 14
116 // years and 3 leap days.
117 co_time_diff_t td = { 0, 0 };
118 // clang-format off
119 co_time_diff_set(&td, &(struct timespec){
120 tp->tv_sec - (14 * 365 + 3) * 24 * 60 * 60,
121 tp->tv_nsec });
122 // clang-format on
123 tod->ms = td.ms;
124 tod->days = td.days;
125}
126
127void
128co_time_diff_get(const co_time_diff_t *td, struct timespec *tp)
129{
130 assert(td);
131 assert(tp);
132
133 tp->tv_sec = (time_t)td->days * 24 * 60 * 60 + td->ms / 1000;
134 tp->tv_nsec = (long)(td->ms % 1000) * 1000000;
135}
136
137void
138co_time_diff_set(co_time_diff_t *td, const struct timespec *tp)
139{
140 assert(td);
141 assert(tp);
142
143 // Compute the number of milliseconds since midnight.
144 td->ms = (tp->tv_sec % (24 * 60 * 60)) * 1000 + tp->tv_nsec / 1000000;
145 // Compute the number of days.
146 td->days = (co_unsigned16_t)(tp->tv_sec / (24 * 60 * 60));
147}
148
149void *
150__co_time_alloc(void)
151{
152 void *ptr = malloc(sizeof(struct __co_time));
153#if !LELY_NO_ERRNO
154 if (!ptr)
155 set_errc(errno2c(errno));
156#endif
157 return ptr;
158}
159
160void
161__co_time_free(void *ptr)
162{
163 free(ptr);
164}
165
166struct __co_time *
167__co_time_init(struct __co_time *time, can_net_t *net, co_dev_t *dev)
168{
169 assert(time);
170 assert(net);
171 assert(dev);
172
173 int errc = 0;
174
175 time->net = net;
176 time->dev = dev;
177
178 time->stopped = 1;
179
180 co_obj_t *obj_1012 = co_dev_find_obj(time->dev, 0x1012);
181 if (!obj_1012) {
182 errc = errnum2c(ERRNUM_NOSYS);
183 goto error_obj_1012;
184 }
185
186 time->cobid = 0;
187
188 time->sub_1013_00 = co_dev_find_sub(time->dev, 0x1013, 0x00);
189
190 time->recv = can_recv_create();
191 if (!time->recv) {
192 errc = get_errc();
193 goto error_create_recv;
194 }
195 can_recv_set_func(time->recv, &co_time_recv, time);
196
197 time->timer = can_timer_create();
198 if (!time->timer) {
199 errc = get_errc();
200 goto error_create_timer;
201 }
203
204 time->start = (struct timespec){ 0, 0 };
205
206 time->ind = NULL;
207 time->data = NULL;
208
209 if (co_time_start(time) == -1) {
210 errc = get_errc();
211 goto error_start;
212 }
213
214 return time;
215
216 // co_time_stop(time);
217error_start:
219error_create_timer:
220 can_recv_destroy(time->recv);
221error_create_recv:
222 co_obj_set_dn_ind(obj_1012, NULL, NULL);
223error_obj_1012:
224 set_errc(errc);
225 return NULL;
226}
227
228void
229__co_time_fini(struct __co_time *time)
230{
231 assert(time);
232
233 co_time_stop(time);
234
236 can_recv_destroy(time->recv);
237}
238
239co_time_t *
241{
242 trace("creating TIME service");
243
244 int errc = 0;
245
246 co_time_t *time = __co_time_alloc();
247 if (!time) {
248 errc = get_errc();
249 goto error_alloc_time;
250 }
251
252 if (!__co_time_init(time, net, dev)) {
253 errc = get_errc();
254 goto error_init_time;
255 }
256
257 return time;
258
259error_init_time:
260 __co_time_free(time);
261error_alloc_time:
262 set_errc(errc);
263 return NULL;
264}
265
266void
268{
269 if (time) {
270 trace("destroying TIME service");
271 __co_time_fini(time);
272 __co_time_free(time);
273 }
274}
275
276int
278{
279 assert(time);
280
281 if (!time->stopped)
282 return 0;
283
284 co_obj_t *obj_1012 = co_dev_find_obj(time->dev, 0x1012);
285 // Retrieve the TIME COB-ID.
286 time->cobid = co_obj_get_val_u32(obj_1012, 0x00);
287 // Set the download indication function for the TIME COB-ID object.
288 co_obj_set_dn_ind(obj_1012, &co_1012_dn_ind, time);
289
290 can_net_get_time(time->net, &time->start);
291
292 co_time_update(time);
293
294 time->stopped = 1;
295
296 return 0;
297}
298
299void
301{
302 assert(time);
303
304 if (time->stopped)
305 return;
306
307 co_time_stop_prod(time);
308
309 can_timer_stop(time->timer);
310 can_recv_stop(time->recv);
311
312 // Remove the download indication function for the TIME COB-ID object.
313 co_obj_t *obj_1012 = co_dev_find_obj(time->dev, 0x1012);
314 co_obj_set_dn_ind(obj_1012, NULL, NULL);
315
316 time->stopped = 1;
317}
318
319int
321{
322 assert(time);
323
324 return time->stopped;
325}
326
327can_net_t *
329{
330 assert(time);
331
332 return time->net;
333}
334
335co_dev_t *
337{
338 assert(time);
339
340 return time->dev;
341}
342
343void
344co_time_get_ind(const co_time_t *time, co_time_ind_t **pind, void **pdata)
345{
346 assert(time);
347
348 if (pind)
349 *pind = time->ind;
350 if (pdata)
351 *pdata = time->data;
352}
353
354void
356{
357 assert(time);
358
359 time->ind = ind;
360 time->data = data;
361}
362
363void
364co_time_start_prod(co_time_t *time, const struct timespec *start,
365 const struct timespec *interval)
366{
367 assert(time);
368
369 if (time->cobid & CO_TIME_COBID_PRODUCER)
370 can_timer_start(time->timer, time->net, start, interval);
371}
372
373void
375{
376 assert(time);
377
378 if (time->cobid & CO_TIME_COBID_PRODUCER)
379 can_timer_stop(time->timer);
380}
381
382static void
384{
385 assert(time);
386
387 if (time->cobid & CO_TIME_COBID_CONSUMER) {
388 // Register the receiver under the specified CAN-ID.
389 uint_least32_t id = time->cobid;
390 uint_least8_t flags = 0;
391 if (id & CO_TIME_COBID_FRAME) {
392 id &= CAN_MASK_EID;
393 flags |= CAN_FLAG_IDE;
394 } else {
395 id &= CAN_MASK_BID;
396 }
397 can_recv_start(time->recv, time->net, id, flags);
398 } else {
399 // Stop the receiver unless we are a consumer.
400 can_recv_stop(time->recv);
401 }
402
403 if (!(time->cobid & CO_TIME_COBID_PRODUCER))
404 // Stop the timer unless we are a producer.
405 can_timer_stop(time->timer);
406}
407
408static co_unsigned32_t
409co_1012_dn_ind(co_sub_t *sub, struct co_sdo_req *req, void *data)
410{
411 assert(sub);
412 assert(co_obj_get_idx(co_sub_get_obj(sub)) == 0x1012);
413 assert(req);
414 co_time_t *time = data;
415 assert(time);
416
417 co_unsigned16_t type = co_sub_get_type(sub);
418 assert(!co_type_is_array(type));
419
420 union co_val val;
421 co_unsigned32_t ac = 0;
422 if (co_sdo_req_dn_val(req, type, &val, &ac) == -1)
423 return ac;
424
425 if (co_sub_get_subidx(sub))
426 return CO_SDO_AC_NO_SUB;
427
428 assert(type == CO_DEFTYPE_UNSIGNED32);
429 co_unsigned32_t cobid = val.u32;
430 co_unsigned32_t cobid_old = co_sub_get_val_u32(sub);
431 if (cobid == cobid_old)
432 return 0;
433
434 // The CAN-ID cannot be changed while the producer or consumer is and
435 // remains active.
436 int active = (cobid & CO_TIME_COBID_PRODUCER)
437 || (cobid & CO_TIME_COBID_CONSUMER);
438 int active_old = (cobid_old & CO_TIME_COBID_PRODUCER)
439 || (cobid_old & CO_TIME_COBID_CONSUMER);
440 uint_least32_t canid = cobid & CAN_MASK_EID;
441 uint_least32_t canid_old = cobid_old & CAN_MASK_EID;
442 if (active && active_old && canid != canid_old)
443 return CO_SDO_AC_PARAM_VAL;
444
445 // A 29-bit CAN-ID is only valid if the frame bit is set.
446 if (!(cobid & CO_TIME_COBID_FRAME)
447 && (cobid & (CAN_MASK_EID ^ CAN_MASK_BID)))
448 return CO_SDO_AC_PARAM_VAL;
449
450 time->cobid = cobid;
451
452 co_sub_dn(sub, &val);
453
454 co_time_update(time);
455 return 0;
456}
457
458static int
459co_time_recv(const struct can_msg *msg, void *data)
460{
461 assert(msg);
462 co_time_t *time = data;
463 assert(time);
464
465 // Ignore remote frames.
466 if (msg->flags & CAN_FLAG_RTR)
467 return 0;
468
469#if !LELY_NO_CANFD
470 // Ignore CAN FD format frames.
471 if (msg->flags & CAN_FLAG_EDL)
472 return 0;
473#endif
474
475 if (msg->len < 6)
476 return 0;
477
478 co_time_of_day_t tod;
479 tod.ms = ldle_u32(msg->data) & UINT32_C(0x0fffffff);
480 tod.days = ldle_u16(msg->data + 4);
481
482 struct timespec tv;
483 co_time_of_day_get(&tod, &tv);
484
485 if (time->ind)
486 time->ind(time, &tv, time->data);
487
488 return 0;
489}
490
491static int
492co_time_timer(const struct timespec *tp, void *data)
493{
494 assert(tp);
495 co_time_t *time = data;
496 assert(time);
497
498 // Update the high-resolution time stamp, if it exists.
499 if (time->sub_1013_00)
500 co_sub_set_val_u32(time->sub_1013_00,
501 (co_unsigned32_t)timespec_diff_usec(
502 tp, &time->start));
503
504 // Convert the time to a TIME_OF_DAY value.
505 co_time_of_day_t tod = { 0, 0 };
506 co_time_of_day_set(&tod, tp);
507
508 struct can_msg msg = CAN_MSG_INIT;
509 msg.id = time->cobid;
510 if (time->cobid & CO_TIME_COBID_FRAME) {
511 msg.id &= CAN_MASK_EID;
512 msg.flags |= CAN_FLAG_IDE;
513 } else {
514 msg.id &= CAN_MASK_BID;
515 }
516 msg.len = 6;
517 stle_u32(msg.data, tod.ms & UINT32_C(0x0fffffff));
518 stle_u16(msg.data + 4, tod.days);
519 can_net_send(time->net, &msg);
520
521 return 0;
522}
523
524#endif // !LELY_NO_CO_TIME
@ 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_MASK_BID
The mask used to extract the 11-bit Base Identifier from a CAN frame.
Definition msg.h:31
#define CAN_MSG_INIT
The static initializer for can_msg.
Definition msg.h:113
This header file is part of the CANopen library; it contains the device description declarations.
co_obj_t * co_dev_find_obj(const co_dev_t *dev, co_unsigned16_t idx)
Finds an object in the object dictionary of a CANopen device.
Definition dev.c:279
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:290
void co_time_stop(co_time_t *time)
Stops a TIME service.
Definition time.c:300
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:240
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:355
void co_time_stop_prod(co_time_t *time)
Stops a CANopen TIME producer.
Definition time.c:374
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
int co_time_is_stopped(const co_time_t *time)
Retuns 1 if the specified TIME service is stopped, and 0 if not.
Definition time.c:320
void co_time_destroy(co_time_t *time)
Destroys a CANopen TIME producer/consumer service.
Definition time.c:267
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:459
static void co_time_update(co_time_t *time)
Updates and (de)activates a TIME producer/consumer service.
Definition time.c:383
int co_time_start(co_time_t *time)
Starts a TIME service.
Definition time.c:277
void co_time_start_prod(co_time_t *time, const struct timespec *start, const struct timespec *interval)
Starts a CANopen TIME producer.
Definition time.c:364
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:344
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:328
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:128
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:138
static int co_time_timer(const struct timespec *tp, void *data)
The CAN timer callback function for a TIME producer service.
Definition time.c:492
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:109
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:336
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:409
This header file is part of the CANopen library; it contains the time stamp (TIME) object declaration...
#define CO_TIME_COBID_CONSUMER
The bit in the TIME COB-ID specifying whether the device is a consumer.
Definition time.h:29
#define CO_TIME_COBID_PRODUCER
The bit in the TIME COB-ID specifying whether the device is a producer.
Definition time.h:32
#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
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
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_NOSYS
Function not supported.
Definition errnum.h:184
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
This header file is part of the CANopen library; it contains the Service Data Object (SDO) declaratio...
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_NO_SUB
SDO abort code: Sub-index does not exist.
Definition sdo.h:132
#define CO_SDO_AC_PARAM_VAL
SDO abort code: Invalid value for parameter (download only).
Definition sdo.h:135
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_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:431
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition net.c:376
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:196
void can_timer_set_func(can_timer_t *timer, can_timer_func_t *func, void *data)
Sets the callback function invoked when a CAN timer is triggered.
Definition net.c:422
void can_recv_stop(can_recv_t *recv)
Stops a CAN frame receiver from processing frames and unregisters it with the network interface.
Definition net.c:609
void can_recv_set_func(can_recv_t *recv, can_recv_func_t *func, void *data)
Sets the callback function used to process CAN frames with a receiver.
Definition net.c:578
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition net.c:558
void can_recv_start(can_recv_t *recv, can_net_t *net, uint_least32_t id, uint_least8_t flags)
Registers a CAN frame receiver with a network interface and starts processing frames.
Definition net.c:587
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition net.c:533
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition net.c:401
This header file is part of the CANopen library; it contains the object dictionary declarations.
co_unsigned8_t co_sub_get_subidx(const co_sub_t *sub)
Returns the sub-index of a CANopen sub-object.
Definition obj.c:559
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
Definition obj.c:164
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
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 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:86
A CAN timer.
Definition net.c:63
A CANopen device.
Definition dev.h:30
A CANopen object.
Definition obj.h:31
A CANopen sub-object.
Definition obj.h:53
A CANopen TIME producer/consumer service.
Definition time.c:41
can_timer_t * timer
A pointer to the CAN timer.
Definition time.c:55
void * data
A pointer to user-specified data for ind.
Definition time.c:61
can_recv_t * recv
A pointer to the CAN frame receiver.
Definition time.c:53
co_sub_t * sub_1013_00
A pointer to the high-resolution time stamp sub-object.
Definition time.c:51
int stopped
A flag specifying whether the TIME service is stopped.
Definition time.c:47
co_unsigned32_t cobid
The TIME COB-ID.
Definition time.c:49
can_net_t * net
A pointer to a CAN network interface.
Definition time.c:43
co_time_ind_t * ind
A pointer to the indication function.
Definition time.c:59
struct timespec start
The creation time of the service.
Definition time.c:57
co_dev_t * dev
A pointer to a CANopen device.
Definition time.c:45
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
A CANopen SDO upload/download request.
Definition sdo.h:181
A time type with nanosecond resolution.
Definition time.h:88
long tv_nsec
Nanoseconds [0, 999999999].
Definition time.h:92
time_t tv_sec
Whole seconds (>= 0).
Definition time.h:90
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_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 utilities library; it contains the time function declarations.
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:231
This header file is part of the CANopen library; it contains the CANopen value declarations.