CANDev::CANDev(std::string dev_name, uint32_t can_id, uint32_t can_mask) {
struct sockaddr_can addr;
struct ifreq ifr;
if((dev = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
std::cout<< "Error while opening socket" << std::endl;
dev = -1;
return;
}
strcpy(ifr.ifr_name, dev_name.c_str());
rt_dev_ioctl(dev, SIOCGIFINDEX, &ifr);
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
struct can_filter rfilter[1];
rfilter[0].can_id = can_id;
rfilter[0].can_mask = can_mask;
if (rt_dev_setsockopt(dev, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter,
sizeof(struct can_filter)) < 0)
{
std::cout << "Error: filter" << std::endl;
rt_dev_close(dev);
dev = -1;
return;
}
if(rt_dev_bind(dev, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
std::cout << "Error in socket bind" << std::endl;
rt_dev_close(dev);
dev = -1;
}
}
void InitRT(int port)
{
#ifdef UNIX
if ((FD_RT = rt_dev_socket(AF_RTIPC, SOCK_DGRAM, IPCPROTO_XDDP)) < 0)
{
throw std::runtime_error(std::string("RT data communication failed! Port:") + std::to_string(port));
}
struct timeval tv;
tv.tv_sec = 1; /* 30 Secs Timeout */
tv.tv_usec = 0; // Not Init'ing this can cause strange errors
if (rt_dev_setsockopt(FD_RT, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, sizeof(struct timeval)))
{
throw std::runtime_error(std::string("RT data communication failed! Port:") + std::to_string(port));
}
/*
* Set a local 16k pool for the RT endpoint. Memory needed to
* convey datagrams will be pulled from this pool, instead of
* Xenomai's system pool.
*/
const std::size_t poolsz = 16384; /* bytes */
if (rt_dev_setsockopt(FD_RT, SOL_XDDP, XDDP_POOLSZ, &poolsz, sizeof(poolsz)))
{
throw std::runtime_error(std::string("RT data communication failed! Port:") + std::to_string(port));
}
/*
* Bind the socket to the port, to setup a proxy to channel
* traffic to/from the Linux domain.
*
* saddr.sipc_port specifies the port number to use.
*/
struct sockaddr_ipc saddr;
memset(&saddr, 0, sizeof(saddr));
saddr.sipc_family = AF_RTIPC;
saddr.sipc_port = port;
if (rt_dev_bind(FD_RT, (struct sockaddr *)&saddr, sizeof(saddr)))
{
throw std::runtime_error(std::string("RT pipe Init failed! Port:") + std::to_string(port));
}
#endif
}
void CANSocket::open(int port) throw(std::logic_error, std::runtime_error)
{
if (isOpen()) {
(logMessage("CANSocket::%s(): This object is already associated with a CAN port.")
% __func__).raise<std::logic_error>();
}
logMessage("CANSocket::open(%d)") % port;
int ret;
ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (ret < 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_socket(): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
handle = ret;
struct ifreq ifr;
char devname[10];
sprintf(devname, "rtcan%d", port);
strncpy(ifr.ifr_name, devname, IFNAMSIZ);
ret = rt_dev_ioctl(handle, SIOCGCANSTATE, &ifr);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(SIOCGCANSTATE): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
} else {
// Note: The Xenomai documentation says that "ifr_ifru will be filled
// with an instance of can_mode_t." This is a documentation bug. In
// ksrc/drivers/can/rtcan_raw_dev.c ifr_ifru actually gets filled with
// in instance of can_state_t, which makes a lot more sense.
can_state_t* state = (can_state_t*) &ifr.ifr_ifru;
if (*state != CAN_STATE_ACTIVE) {
const int NUM_STATES = 8;
const char canStateStrs[NUM_STATES][18] = {
"active",
"warning",
"passive",
"bus-off",
"scanning-baudrate",
"stopped",
"sleeping",
"unknown state"
};
int index = *state;
if (index < 0 || index >= NUM_STATES) {
index = NUM_STATES - 1;
}
logMessage(" WARNING: CAN_STATE is %s (%d)") % canStateStrs[index] % *state;
if (*state == CAN_STATE_BUS_OFF) {
logMessage(" Setting CAN_MODE = CAN_MODE_START");
can_mode_t* mode = (can_mode_t*) &ifr.ifr_ifru;
*mode = CAN_MODE_START;
ret = rt_dev_ioctl(handle, SIOCSCANMODE, &ifr);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(SIOCSCANMODE): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
// According to Xenomai documentation, the transition from
// bus-off to active takes at least 128 * 11 * 1e-6 = 0.0014
// seconds and there's no way to detect when it's done :(
btsleep(0.1);
}
}
}
struct can_filter recvFilter[1];
recvFilter[0].can_id = (Puck::HOST_ID << Puck::NODE_ID_WIDTH) | CAN_INV_FILTER;
recvFilter[0].can_mask = Puck::FROM_MASK;
ret = rt_dev_setsockopt(handle, SOL_CAN_RAW, CAN_RAW_FILTER, &recvFilter, sizeof(recvFilter));
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_setsockopt(CAN_RAW_FILTER): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
// Note: This must be done after the rt_dev_ioctl(SIOCGCANSTATE) call above,
// otherwise rt_dev_send() will fail with ret = -6 (No such device or
// address). The ifr.ifr_index gets overwritten because it is actually a
// member of the ifr.ifr_ifru union.
ret = rt_dev_ioctl(handle, SIOCGIFINDEX, &ifr);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(SIOCGIFINDEX): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
struct sockaddr_can toAddr;
memset(&toAddr, 0, sizeof(toAddr));
toAddr.can_ifindex = ifr.ifr_ifindex;
toAddr.can_family = AF_CAN;
ret = rt_dev_bind(handle, (struct sockaddr *) &toAddr, sizeof(toAddr));
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_bind(): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
nanosecs_rel_t timeout = (nanosecs_rel_t) CommunicationsBus::TIMEOUT;
ret = rt_dev_ioctl(handle, RTCAN_RTIOC_RCV_TIMEOUT, &timeout);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(RCV_TIMEOUT): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
ret = rt_dev_ioctl(handle, RTCAN_RTIOC_SND_TIMEOUT, &timeout);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(SND_TIMEOUT): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
}
int main(int argc, char **argv)
{
int opt, ret;
u_int32_t id, mask;
u_int32_t err_mask = 0;
struct ifreq ifr;
char *ptr;
char name[32];
struct option long_options[] = {
{ "help", no_argument, 0, 'h' },
{ "verbose", no_argument, 0, 'v'},
{ "filter", required_argument, 0, 'f'},
{ "error", required_argument, 0, 'e'},
{ "timeout", required_argument, 0, 't'},
{ "timestamp", no_argument, 0, 'T'},
{ "timestamp-rel", no_argument, 0, 'R'},
{ 0, 0, 0, 0},
};
mlockall(MCL_CURRENT | MCL_FUTURE);
signal(SIGTERM, cleanup_and_exit);
signal(SIGINT, cleanup_and_exit);
while ((opt = getopt_long(argc, argv, "hve:f:t:p:RT",
long_options, NULL)) != -1) {
switch (opt) {
case 'h':
print_usage(argv[0]);
exit(0);
case 'p':
print = strtoul(optarg, NULL, 0);
break;
case 'v':
verbose = 1;
break;
case 'e':
err_mask = strtoul(optarg, NULL, 0);
break;
case 'f':
ptr = optarg;
while (1) {
id = strtoul(ptr, NULL, 0);
ptr = strchr(ptr, ':');
if (!ptr) {
fprintf(stderr, "filter must be applied in the form id:mask[:id:mask]...\n");
exit(1);
}
ptr++;
mask = strtoul(ptr, NULL, 0);
ptr = strchr(ptr, ':');
add_filter(id, mask);
if (!ptr)
break;
ptr++;
}
break;
case 't':
timeout = (nanosecs_rel_t)strtoul(optarg, NULL, 0) * 1000000;
break;
case 'R':
timestamp_rel = 1;
case 'T':
with_timestamp = 1;
break;
default:
fprintf(stderr, "Unknown option %c\n", opt);
break;
}
}
ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (ret < 0) {
fprintf(stderr, "rt_dev_socket: %s\n", strerror(-ret));
return -1;
}
s = ret;
if (argv[optind] == NULL) {
if (verbose)
printf("interface all\n");
ifr.ifr_ifindex = 0;
} else {
if (verbose)
printf("interface %s\n", argv[optind]);
strncpy(ifr.ifr_name, argv[optind], IFNAMSIZ);
if (verbose)
printf("s=%d, ifr_name=%s\n", s, ifr.ifr_name);
ret = rt_dev_ioctl(s, SIOCGIFINDEX, &ifr);
if (ret < 0) {
fprintf(stderr, "rt_dev_ioctl GET_IFINDEX: %s\n", strerror(-ret));
goto failure;
}
}
if (err_mask) {
ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
&err_mask, sizeof(err_mask));
if (ret < 0) {
fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
goto failure;
}
if (verbose)
printf("Using err_mask=%#x\n", err_mask);
}
if (filter_count) {
ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER,
&recv_filter, filter_count *
sizeof(struct can_filter));
if (ret < 0) {
fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
goto failure;
}
}
recv_addr.can_family = AF_CAN;
recv_addr.can_ifindex = ifr.ifr_ifindex;
ret = rt_dev_bind(s, (struct sockaddr *)&recv_addr,
sizeof(struct sockaddr_can));
if (ret < 0) {
fprintf(stderr, "rt_dev_bind: %s\n", strerror(-ret));
goto failure;
}
if (timeout) {
if (verbose)
printf("Timeout: %lld ns\n", (long long)timeout);
ret = rt_dev_ioctl(s, RTCAN_RTIOC_RCV_TIMEOUT, &timeout);
if (ret) {
fprintf(stderr, "rt_dev_ioctl RCV_TIMEOUT: %s\n", strerror(-ret));
goto failure;
}
}
if (with_timestamp) {
ret = rt_dev_ioctl(s, RTCAN_RTIOC_TAKE_TIMESTAMP, RTCAN_TAKE_TIMESTAMPS);
if (ret) {
fprintf(stderr, "rt_dev_ioctl TAKE_TIMESTAMP: %s\n", strerror(-ret));
goto failure;
}
}
snprintf(name, sizeof(name), "rtcanrecv-%d", getpid());
ret = rt_task_shadow(&rt_task_desc, name, 0, 0);
if (ret) {
fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret));
goto failure;
}
rt_task();
/* never returns */
failure:
cleanup();
return -1;
}
CANDev::CANDev(const std::string &dev_name, const std::string &name,
const std::vector<CANDev::FilterElement> &filter_vec)
: frame_buf(1000),
buf_size(0),
dev_name_(dev_name),
name_(name) {
struct sockaddr_can addr;
struct ifreq ifr;
if ((dev = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
std::cout << "ERROR: CANDev::CANDev(" << dev_name_ << ", " << name_
<< "): error in opening socket" << std::endl;
dev = -1;
return;
}
strcpy(ifr.ifr_name, dev_name.c_str()); // NOLINT
rt_dev_ioctl(dev, SIOCGIFINDEX, &ifr);
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
int nfilters = filter_vec.size();
if (nfilters > 0) {
struct can_filter *rfilter = new can_filter[nfilters];
for (int i = 0; i < nfilters; i++) {
rfilter[i].can_id = filter_vec[i].can_id_;
rfilter[i].can_mask = filter_vec[i].can_mask_;
}
if (rt_dev_setsockopt(dev, SOL_CAN_RAW, CAN_RAW_FILTER, rfilter,
sizeof(struct can_filter) * nfilters) < 0) {
std::cout << "ERROR: CANDev::CANDev(" << dev_name_ << ", " << name_
<< "): filter" << std::endl;
rt_dev_close(dev);
delete[] rfilter;
dev = -1;
return;
}
delete[] rfilter;
}
// #if !defined(HAVE_RTNET)
// TODO(my_username): check if it is working in RT
{
struct timeval timeout;
timeout.tv_sec = 1;
timeout.tv_usec = 0;
if (setsockopt(dev, SOL_SOCKET, SO_RCVTIMEO, &timeout,
sizeof(struct timeval)) < 0) {
std::cout << "ERROR: CANDev::CANDev: setsockopt SO_RCVTIMEO" << std::endl;
rt_dev_close(dev);
dev = -1;
return;
}
if (setsockopt(dev, SOL_SOCKET, SO_SNDTIMEO, &timeout,
sizeof(struct timeval)) < 0) {
std::cout << "ERROR: CANDev::CANDev: setsockopt SO_SNDTIMEO" << std::endl;
rt_dev_close(dev);
dev = -1;
return;
}
}
// #else
// // one second timeout for receive and send
// rt_dev_ioctl(dev, RTCAN_RTIOC_RCV_TIMEOUT, 1000000000UL);
// rt_dev_ioctl(dev, RTCAN_RTIOC_SND_TIMEOUT, 1000000000UL);
// #endif
if (rt_dev_bind(dev, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
std::cout << "ERROR: CANDev::CANDev(" << dev_name_ << ", " << name_
<< "): error in socket bind" << std::endl;
rt_dev_close(dev);
dev = -1;
}
}
int main(int argc, char **argv)
{
int i, opt, ret;
struct ifreq ifr;
char name[32];
struct option long_options[] = {
{ "help", no_argument, 0, 'h' },
{ "identifier", required_argument, 0, 'i'},
{ "rtr", no_argument, 0, 'r'},
{ "extended", no_argument, 0, 'e'},
{ "verbose", no_argument, 0, 'v'},
{ "count", no_argument, 0, 'c'},
{ "print", required_argument, 0, 'p'},
{ "loop", required_argument, 0, 'l'},
{ "delay", required_argument, 0, 'd'},
{ "send", no_argument, 0, 's'},
{ "timeout", required_argument, 0, 't'},
{ "loopback", required_argument, 0, 'L'},
{ 0, 0, 0, 0},
};
mlockall(MCL_CURRENT | MCL_FUTURE);
signal(SIGTERM, cleanup_and_exit);
signal(SIGINT, cleanup_and_exit);
frame.can_id = 1;
while ((opt = getopt_long(argc, argv, "hvi:l:red:t:cp:sL:",
long_options, NULL)) != -1) {
switch (opt) {
case 'h':
print_usage(argv[0]);
exit(0);
case 'p':
print = strtoul(optarg, NULL, 0);
case 'v':
verbose = 1;
break;
case 'c':
count = 1;
break;
case 'l':
loops = strtoul(optarg, NULL, 0);
break;
case 'i':
frame.can_id = strtoul(optarg, NULL, 0);
break;
case 'r':
rtr = 1;
break;
case 'e':
extended = 1;
break;
case 'd':
delay = strtoul(optarg, NULL, 0) * 1000000LL;
break;
case 's':
use_send = 1;
break;
case 't':
timeout = strtoul(optarg, NULL, 0) * 1000000LL;
break;
case 'L':
loopback = strtoul(optarg, NULL, 0);
break;
default:
fprintf(stderr, "Unknown option %c\n", opt);
break;
}
}
if (optind == argc) {
print_usage(argv[0]);
exit(0);
}
if (argv[optind] == NULL) {
fprintf(stderr, "No Interface supplied\n");
exit(-1);
}
if (verbose)
printf("interface %s\n", argv[optind]);
ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (ret < 0) {
fprintf(stderr, "rt_dev_socket: %s\n", strerror(-ret));
return -1;
}
s = ret;
if (loopback >= 0) {
ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_LOOPBACK,
&loopback, sizeof(loopback));
if (ret < 0) {
fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
goto failure;
}
if (verbose)
printf("Using loopback=%d\n", loopback);
}
strncpy(ifr.ifr_name, argv[optind], IFNAMSIZ);
if (verbose)
printf("s=%d, ifr_name=%s\n", s, ifr.ifr_name);
ret = rt_dev_ioctl(s, SIOCGIFINDEX, &ifr);
if (ret < 0) {
fprintf(stderr, "rt_dev_ioctl: %s\n", strerror(-ret));
goto failure;
}
memset(&to_addr, 0, sizeof(to_addr));
to_addr.can_ifindex = ifr.ifr_ifindex;
to_addr.can_family = AF_CAN;
if (use_send) {
/* Suppress definiton of a default receive filter list */
ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
if (ret < 0) {
fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret));
goto failure;
}
ret = rt_dev_bind(s, (struct sockaddr *)&to_addr, sizeof(to_addr));
if (ret < 0) {
fprintf(stderr, "rt_dev_bind: %s\n", strerror(-ret));
goto failure;
}
}
if (count)
frame.can_dlc = sizeof(int);
else {
for (i = optind + 1; i < argc; i++) {
frame.data[dlc] = strtoul(argv[i], NULL, 0);
dlc++;
if( dlc == 8 )
break;
}
frame.can_dlc = dlc;
}
if (rtr)
frame.can_id |= CAN_RTR_FLAG;
if (extended)
frame.can_id |= CAN_EFF_FLAG;
if (timeout) {
if (verbose)
printf("Timeout: %lld ns\n", (long long)timeout);
ret = rt_dev_ioctl(s, RTCAN_RTIOC_SND_TIMEOUT, &timeout);
if (ret) {
fprintf(stderr, "rt_dev_ioctl SND_TIMEOUT: %s\n", strerror(-ret));
goto failure;
}
}
snprintf(name, sizeof(name), "rtcansend-%d", getpid());
ret = rt_task_shadow(&rt_task_desc, name, 1, 0);
if (ret) {
fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret));
goto failure;
}
rt_task();
cleanup();
return 0;
failure:
cleanup();
return -1;
}
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);
}
static int _init(void)
{
int broadcast = 1;
int64_t timeout = -1;
rt_printk("RtnetTest: Module initialisation started\n");
memset(buffer_in, 0, sizeof(buffer_in));
memset(&loc_addr, 0, sizeof (struct sockaddr_in));
memset(buffer_out, 0, sizeof(buffer_out));
memset(&tx_addr, 0, sizeof (struct sockaddr_in));
loc_addr.sin_family = AF_INET;
loc_addr.sin_port = htons(UDPPORT);
loc_addr.sin_addr.s_addr = INADDR_ANY;
tx_addr.sin_family = AF_INET;
tx_addr.sin_port = htons(UDPPORT);
tx_addr.sin_addr.s_addr = rt_inet_aton("127.0.0.1");
if (((mbx = rt_typed_named_mbx_init("MYMBX", 2000*sizeof(struct sample), FIFO_Q))) == NULL) {
rt_printk("RtnetTest: Cannot create the mailbox\n");
return -1;
}
if (((sock = rt_dev_socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))) < 0) {
rt_printk("RtnetTest: Error opening UDP/IP socket: %d\n", sock);
return -1;
}
if (rt_dev_setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) == -1) {
rt_printk("RtnetTest: Can't set broadcast options\n");
goto close_socks;
}
rt_dev_ioctl(sock, RTNET_RTIOC_TIMEOUT, &timeout);
if (rt_dev_bind(sock, (struct sockaddr *) &loc_addr, sizeof(struct sockaddr_in)) < 0) {
rt_printk("RtnetTest: Can't bind the network socket");
goto close_socks;
}
rt_set_periodic_mode();
period = start_rt_timer(nano2count(WORKCYCLE));
if (rt_task_init_cpuid(&rx_task, receiver, 0, STKSIZ, 0, 0, 0, CPU) < 0) {
rt_printk("RtnetTest: Can't initialise the receiver task");
goto close_socks;
}
if (rt_task_init_cpuid(&tx_task, sender, 0, STKSIZ, 0, 0, 0, CPU) < 0) {
rt_printk("RtnetTest: Can't initialise the transmitter task");
goto close_socks;
}
if (0 != rt_task_make_periodic(&tx_task, rt_get_time() + 20*period, period)) {
rt_printk("RtnetTest: Make sender periodic failed\n");
goto close_socks;
}
if (rt_task_resume(&rx_task) < 0) {
rt_printk("RtnetTest: Can't start the receiver task");
goto close_socks;
}
rt_printk("RtnetTest: Module initialisation completed\n");
return 0;
close_socks:
rt_dev_close(sock);
rt_dev_shutdown(sock, SHUT_RDWR);
return -1;
}
