void recv_msg(void *arg)
{
int ret;
struct msghdr msg;
struct iovec iov;
struct sockaddr_ll addr;
while (1) {
iov.iov_base = buffer_in;
iov.iov_len = sizeof(buffer_in);
memset(&msg, 0, sizeof(msg));
msg.msg_name = &addr;
msg.msg_namelen = sizeof(addr);
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
ret = rt_dev_recvmsg(sock, &msg, 0);
if (ret <= 0) {
printk(" rt_dev_recvmsg() = %d\n", ret);
return;
} else {
printk("received packet from %02X:%02X:%02X:%02X:%02X:%02X, "
"length: %d,\ncontent: \"%s\"\n",
addr.sll_addr[0], addr.sll_addr[1], addr.sll_addr[2],
addr.sll_addr[3], addr.sll_addr[4], addr.sll_addr[5],
ret, buffer_in);
}
}
}
/**
* @brief Receive a CAN message
*
* The @a recv() function receives a CAN message mit a given timeout set
* with @a setRxTimeout().
*
* If timestamp support is enabled the pointer to the timestamp buffer have
* to be set. The format of the returned timestamp is @a rack_time_t, thus
* it is a global time value.
*
* @param recv_frame Pointer to a CAN frame
* @param timestamp Pointer to a timestamp variable
*
* @return 0 on success, otherwise negative error code
*
* Environments:
*
* This service can be called from:
*
* - User-space task (non-RT, RT)
*
* Rescheduling: possible.
*/
int CanPort::recv(rtcan_frame_t *recv_frame, rack_time_t *timestamp)
{
int ret;
uint64_t timestamp_ns;
struct iovec iov =
{
iov_base : recv_frame,
iov_len : sizeof(rtcan_frame_t)
};
struct msghdr msg =
{
msg_name : NULL,
msg_namelen : 0,
msg_iov : &iov,
msg_iovlen : 1,
};
if (timestamp != NULL)
{
msg.msg_control = ×tamp_ns;
msg.msg_controllen = sizeof(uint64_t);
}
else
{
msg.msg_control = NULL;
msg.msg_controllen = 0;
}
ret = rt_dev_recvmsg(fd, &msg, 0);
if (ret != sizeof(rtcan_frame_t))
{
return ret;
}
if (timestamp != NULL)
{
*timestamp = module->rackTime.fromNano(timestamp_ns);
}
return 0;
}
void recv_msg(void *arg)
{
int ret;
struct msghdr msg;
struct iovec iov[2];
unsigned short msgsize = size;
struct sockaddr_in addr;
while(1) {
iov[0].iov_base = &msgsize;
iov[0].iov_len = sizeof(msgsize);
iov[1].iov_base = buffer_in;
iov[1].iov_len = size;
memset(&msg, 0, sizeof(msg));
msg.msg_name = &addr;
msg.msg_namelen = sizeof(addr);
msg.msg_iov = iov;
msg.msg_iovlen = 2;
ret = rt_dev_recvmsg(sock, &msg, 0);
if (ret <= 0) {
printk(" rt_dev_recvmsg() = %d\n", ret);
return;
} else {
unsigned long ip = ntohl(addr.sin_addr.s_addr);
printk("received packet from %lu.%lu.%lu.%lu, length: %zd+2, "
"encoded length: %d,\n flags: %X, content %s\n", ip >> 24,
(ip >> 16) & 0xFF, (ip >> 8) & 0xFF, ip & 0xFF,
ret-sizeof(msgsize), msgsize, msg.msg_flags,
(memcmp(buffer_in, buffer_out, ret-sizeof(msgsize)) == 0) ?
"ok" : "corrupted");
}
}
}
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++;
}
}
