int PipeBase::recvInRTRawData(void* pData, int size)
{
#ifdef UNIX
if (pData == nullptr)
{
throw std::runtime_error("RecvRTfromNRT:Invalid pointer");
}
return rt_dev_recvfrom(pImp->FD_RT, pData, size, MSG_DONTWAIT, NULL, 0);
#endif
#ifdef WIN32
return 0;
#endif
}
static void receiver(long nothing)
{
fd_set rxfds;
RTIME timeout = 0;
int ready = 0;
socklen_t fromlen = sizeof(rx_addr);
rt_printk("RtnetTest: Receiver task initialised\n");
while(!end) {
FD_ZERO(&rxfds);
FD_SET(sock, &rxfds);
ready = 0;
ready = rt_dev_select(sock + 1, &rxfds, NULL, NULL, timeout);
if (ready > 0 && FD_ISSET(sock, &rxfds)) {
rlen = rt_dev_recvfrom(sock, buffer_in, sizeof(buffer_in), 0, (struct sockaddr*) &rx_addr, &fromlen);
if (rlen > 0) {
rx_samp.cnt++;
// rx_samp.tx = simple_strtoll(buffer_in, NULL, 10);
sscanf(buffer_in, "%lld", &rx_samp.tx);
rx_samp.rx = rt_get_real_time_ns();
rt_mbx_send_if(mbx, &rx_samp, sizeof(rx_samp));
memset(buffer_in, 0, sizeof(buffer_in));
}
} else if (EPERM == -ready) {
rt_printk("RtnetTest: Failed to rt_dev_select on socket\n");
end = 1;
break;
} else if (EINTR == -ready) {
rt_printk("RtnetTest: rt_dev_select was interrupted\n");
end = 1;
break;
}
}
return;
}
static void rt_task(void)
{
int i, ret, count = 0;
struct can_frame frame;
struct sockaddr_can addr;
socklen_t addrlen = sizeof(addr);
struct msghdr msg;
struct iovec iov;
nanosecs_abs_t timestamp, timestamp_prev = 0;
if (with_timestamp) {
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_name = (void *)&addr;
msg.msg_namelen = sizeof(struct sockaddr_can);
msg.msg_control = (void *)×tamp;
msg.msg_controllen = sizeof(nanosecs_abs_t);
}
while (1) {
if (with_timestamp) {
iov.iov_base = (void *)&frame;
iov.iov_len = sizeof(can_frame_t);
ret = rt_dev_recvmsg(s, &msg, 0);
} else
ret = rt_dev_recvfrom(s, (void *)&frame, sizeof(can_frame_t), 0,
(struct sockaddr *)&addr, &addrlen);
if (ret < 0) {
switch (ret) {
case -ETIMEDOUT:
if (verbose)
printf("rt_dev_recv: timed out");
continue;
case -EBADF:
if (verbose)
printf("rt_dev_recv: aborted because socket was closed");
break;
default:
fprintf(stderr, "rt_dev_recv: %s\n", strerror(-ret));
}
break;
}
if (print && (count % print) == 0) {
printf("#%d: (%d) ", count, addr.can_ifindex);
if (with_timestamp && msg.msg_controllen) {
if (timestamp_rel) {
printf("%lldns ", (long long)(timestamp - timestamp_prev));
timestamp_prev = timestamp;
} else
printf("%lldns ", (long long)timestamp);
}
if (frame.can_id & CAN_ERR_FLAG)
printf("!0x%08x!", frame.can_id & CAN_ERR_MASK);
else if (frame.can_id & CAN_EFF_FLAG)
printf("<0x%08x>", frame.can_id & CAN_EFF_MASK);
else
printf("<0x%03x>", frame.can_id & CAN_SFF_MASK);
printf(" [%d]", frame.can_dlc);
if (!(frame.can_id & CAN_RTR_FLAG))
for (i = 0; i < frame.can_dlc; i++) {
printf(" %02x", frame.data[i]);
}
if (frame.can_id & CAN_ERR_FLAG) {
printf(" ERROR ");
if (frame.can_id & CAN_ERR_BUSOFF)
printf("bus-off");
if (frame.can_id & CAN_ERR_CRTL)
printf("controller problem");
} else if (frame.can_id & CAN_RTR_FLAG)
printf(" remote request");
printf("\n");
}
count++;
}
}
