void heartbeat2(void *cookie)
{
int state = 0;
while (!end) {
rtdm_task_wait_period();
leds_set2(state++);
}
}
static void sync_task_func(void *arg)
{
int ret;
rtdm_lockctx_t lock_ctx;
nanosecs_abs_t timestamp;
nanosecs_abs_t timestamp_master;
rtser_event_t ser_rx_event;
can_frame_t can_frame = {
.can_id = clock_sync_can_id,
.can_dlc = sizeof(timestamp),
};
struct iovec iov = {
.iov_base = &can_frame,
.iov_len = sizeof(can_frame_t),
};
struct msghdr msg = {
.msg_name = NULL,
.msg_namelen = 0,
.msg_iov = &iov,
.msg_iovlen = 1,
.msg_control = NULL,
.msg_controllen = 0,
};
if (clock_sync_mode == SYNC_CAN_SLAVE) {
msg.msg_control = ×tamp;
msg.msg_controllen = sizeof(timestamp);
}
while (1) {
switch (clock_sync_mode) {
case SYNC_SER_MASTER:
timestamp = cpu_to_be64(rtdm_clock_read());
ret = sync_dev_ctx->ops->write_rt(sync_dev_ctx, NULL,
×tamp, sizeof(timestamp));
if (ret != sizeof(timestamp)) {
tims_error("[CLOCK SYNC]: can't write serial time stamp, "
"code = %d\n", ret);
goto exit_task;
}
rtdm_task_wait_period();
break;
case SYNC_SER_SLAVE:
ret = sync_dev_ctx->ops->ioctl_rt(sync_dev_ctx, NULL,
RTSER_RTIOC_WAIT_EVENT, &ser_rx_event);
if (ret < 0) {
tims_error("[CLOCK SYNC]: can't read serial time stamp, "
"code = %d\n", ret);
goto exit_task;
}
ret = sync_dev_ctx->ops->read_rt(sync_dev_ctx, NULL,
×tamp_master, sizeof(timestamp_master));
if (ret != sizeof(timestamp_master)) {
tims_error("[CLOCK SYNC]: can't read serial time stamp, "
"code = %d\n", ret);
goto exit_task;
}
timestamp_master = be64_to_cpu(timestamp_master);
rtdm_lock_get_irqsave(&sync_lock, lock_ctx);
clock_offset =
timestamp_master - ser_rx_event.rxpend_timestamp;
rtdm_lock_put_irqrestore(&sync_lock, lock_ctx);
break;
case SYNC_CAN_MASTER:
// workaround for kernel working on user data
iov.iov_len = sizeof(can_frame_t);
iov.iov_base = &can_frame;
// workaround end
*(nanosecs_abs_t *)can_frame.data =
cpu_to_be64(rtdm_clock_read());
ret = sync_dev_ctx->ops->sendmsg_rt(sync_dev_ctx, NULL,
&msg, 0);
if (ret < 0) {
tims_error("[CLOCK SYNC]: can't send CAN time stamp, "
"code = %d\n", ret);
goto exit_task;
}
rtdm_task_wait_period();
break;
case SYNC_CAN_SLAVE:
// workaround for kernel working on user data
iov.iov_len = sizeof(can_frame_t);
iov.iov_base = &can_frame;
// workaround end
ret = sync_dev_ctx->ops->recvmsg_rt(sync_dev_ctx, NULL,
&msg, 0);
if (ret < 0) {
tims_error("[CLOCK SYNC]: can't receive CAN time stamp, "
"code = %d\n", ret);
return;
}
timestamp_master =
be64_to_cpu(*(nanosecs_abs_t *)can_frame.data);
rtdm_lock_get_irqsave(&sync_lock, lock_ctx);
clock_offset = timestamp_master - timestamp;
rtdm_lock_put_irqrestore(&sync_lock, lock_ctx);
break;
}
}
exit_task:
rtdm_context_unlock(sync_dev_ctx);
}
static __initdata char *mode_str[] = {
"Local Clock", "RTnet", "CAN Master", "CAN Slave",
"Serial Master", "Serial Slave"
};
static __initdata struct rtser_config sync_serial_config = {
.config_mask = RTSER_SET_BAUD | RTSER_SET_FIFO_DEPTH |
RTSER_SET_TIMESTAMP_HISTORY | RTSER_SET_EVENT_MASK,
.baud_rate = 115200,
.fifo_depth = RTSER_FIFO_DEPTH_8,
.timestamp_history = RTSER_RX_TIMESTAMP_HISTORY,
.event_mask = RTSER_EVENT_RXPEND,
};
int __init tims_clock_init(void)
{
struct can_filter filter;
int nr_filters = 1;
struct ifreq can_ifr;
struct sockaddr_can can_addr;
int ret;
if (clock_sync_mode < SYNC_NONE || clock_sync_mode > SYNC_SER_SLAVE) {
tims_error("invalid clock_sync_mode %d", clock_sync_mode);
return -EINVAL;
}
printk("TIMS: clock sync mode is %s\n", mode_str[clock_sync_mode]);
printk("TIMS: clock sync dev is %s\n", clock_sync_dev);
rtdm_lock_init(&sync_lock);
switch(clock_sync_mode) {
case SYNC_NONE:
return 0;
case SYNC_RTNET:
sync_dev_fd = rt_dev_open(clock_sync_dev, O_RDONLY);
if (sync_dev_fd < 0)
goto sync_dev_error;
set_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags);
break;
case SYNC_CAN_MASTER:
case SYNC_CAN_SLAVE:
sync_dev_fd = rt_dev_socket(PF_CAN, SOCK_RAW, 0);
if (sync_dev_fd < 0) {
tims_error("[CLOCK SYNC]: error opening CAN socket: %d\n",
sync_dev_fd);
return sync_dev_fd;
}
set_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags);
strcpy(can_ifr.ifr_name, clock_sync_dev);
ret = rt_dev_ioctl(sync_dev_fd, SIOCGIFINDEX, &can_ifr);
if (ret) {
tims_info("[CLOCK SYNC]: error resolving CAN interface: %d\n",
ret);
return ret;
}
if (clock_sync_mode == SYNC_CAN_MASTER)
nr_filters = 0;
else {
filter.can_id = clock_sync_can_id;
filter.can_mask = 0xFFFFFFFF;
}
ret = rt_dev_setsockopt(sync_dev_fd, SOL_CAN_RAW, CAN_RAW_FILTER,
&filter, nr_filters*sizeof(can_filter_t));
if (ret < 0)
goto config_error;
/* Bind socket to default CAN ID */
can_addr.can_family = AF_CAN;
can_addr.can_ifindex = can_ifr.ifr_ifindex;
ret = rt_dev_bind(sync_dev_fd, (struct sockaddr *)&can_addr,
sizeof(can_addr));
if (ret < 0)
goto config_error;
/* Enable timestamps for incoming packets */
ret = rt_dev_ioctl(sync_dev_fd, RTCAN_RTIOC_TAKE_TIMESTAMP,
RTCAN_TAKE_TIMESTAMPS);
if (ret < 0)
goto config_error;
/* Calculate transmission delay */
ret = rt_dev_ioctl(sync_dev_fd, SIOCGCANBAUDRATE, &can_ifr);
if (ret < 0)
goto config_error;
/* (47+64 bit) * 1.000.000.000 (ns/sec) / baudrate (bit/s) */
sync_delay = 1000 * (111000000 / can_ifr.ifr_ifru.ifru_ivalue);
break;
case SYNC_SER_MASTER:
case SYNC_SER_SLAVE:
sync_dev_fd = rt_dev_open(clock_sync_dev, O_RDWR);
if (sync_dev_fd < 0)
goto sync_dev_error;
set_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags);
ret = rt_dev_ioctl(sync_dev_fd, RTSER_RTIOC_SET_CONFIG,
&sync_serial_config);
if (ret < 0)
goto config_error;
/* (80 bit) * 1.000.000.000 (ns/sec) / baudrate (bit/s) */
sync_delay = 1000 * (80000000 / sync_serial_config.baud_rate);
break;
}
sync_dev_ctx = rtdm_context_get(sync_dev_fd);
if (clock_sync_mode != SYNC_RTNET) {
ret = rtdm_task_init(&sync_task, "TIMSClockSync", sync_task_func,
NULL, CLOCK_SYNC_PRIORITY, CLOCK_SYNC_PERIOD);
if (ret < 0)
return ret;
set_bit(TIMS_INIT_BIT_SYNC_TASK, &init_flags);
}
return 0;
sync_dev_error:
tims_error("[CLOCK SYNC]: cannot open %s\n", clock_sync_dev);
return sync_dev_fd;
config_error:
tims_info("[CLOCK SYNC]: error configuring sync device: %d\n", ret);
return ret;
}
void tims_clock_cleanup(void)
{
if (test_and_clear_bit(TIMS_INIT_BIT_SYNC_DEV, &init_flags))
rt_dev_close(sync_dev_fd);
if (test_and_clear_bit(TIMS_INIT_BIT_SYNC_TASK, &init_flags))
rtdm_task_join_nrt(&sync_task, 100);
}
