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;
}
}
int main(int argc, char *argv[])
{
int sockfd = 0;
struct ifreq ifr[MAX_RT_DEVICES];
short flags[MAX_RT_DEVICES];
int devices = 0;
struct ifconf ifc;
struct ifreq flags_ifr;
int i, ret;
printf("RTnet, interface lister for RTAI/fusion\n");
/* Create new socket. */
sockfd = rt_dev_socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
printf("Error opening socket: %d\n", sockfd);
return 1;
}
ifc.ifc_len = sizeof(ifr);
ifc.ifc_req = ifr;
ret = rt_dev_ioctl(sockfd, SIOCGIFCONF, &ifc);
if (ret < 0) {
rt_dev_close(sockfd);
printf("Error retrieving device list: %d\n", ret);
return 1;
}
while (ifc.ifc_len >= (int)sizeof(struct ifreq)) {
memcpy(flags_ifr.ifr_name, ifc.ifc_req[devices].ifr_name, IFNAMSIZ);
ret = rt_dev_ioctl(sockfd, SIOCGIFFLAGS, &flags_ifr);
if (ret < 0) {
rt_dev_close(sockfd);
printf("Error retrieving flags for device %s: %d\n",
flags_ifr.ifr_name, ret);
return 1;
}
flags[devices] = flags_ifr.ifr_flags;
ifc.ifc_len -= sizeof(struct ifreq);
devices++;
}
rt_dev_close(sockfd);
for (i = 0; i < devices; i++)
printf("Device %s: IP %d.%d.%d.%d, flags 0x%08X\n", ifr[i].ifr_name,
((struct sockaddr_in *)&ifr[i].ifr_addr)->sin_addr.s_addr & 0xFF,
((struct sockaddr_in *)&ifr[i].ifr_addr)->sin_addr.s_addr >> 8 & 0xFF,
((struct sockaddr_in *)&ifr[i].ifr_addr)->sin_addr.s_addr >> 16 & 0xFF,
((struct sockaddr_in *)&ifr[i].ifr_addr)->sin_addr.s_addr >> 24,
flags[i]);
return 0;
}
/**
* @brief Open a CAN device
*
* This function opens a new socket, gets the interface index and
* binds the socket to the CAN IDs defined in the struct @a sockaddr_can.
*
* @param dev Number of the CAN device (e.g. 2 if you will open rtcan2)
* @param scan Pointer to struct sockaddr_can
* @param scan_size Size @a scan
* @param module Pointer the RACK module. This pointer is needed to access
* the global timestamp.
*
* @return 0 on success, otherwise negative error code
*
* Environments:
*
* This service can be called from:
*
* - User-space task (non-RT)
*
* Rescheduling: possible.
*/
int CanPort::open(int dev, sockaddr_can* scan, int scan_size, RackModule *module)
{
int ret;
struct ifreq ifr;
if (fd != -1) // file is open
{
return -EBUSY;
}
// Prepare CAN socket and controller
fd = rt_dev_socket(PF_CAN, SOCK_RAW, 0);
if (fd < 0)
{
return fd;
}
// get interface index
sprintf(ifr.ifr_name, "rtcan%d", dev);
ret = rt_dev_ioctl(fd, RTCAN_RTIOC_GET_IFINDEX, &ifr);
if (ret)
{
goto exit_error;
}
// Bind socket to default CAN IDs
scan->can_family = AF_CAN;
scan->can_ifindex = ifr.ifr_ifindex;
ret = rt_dev_bind(fd, (struct sockaddr *)scan, scan_size);
if (ret)
{
goto exit_error;
}
this->module = module;
return 0;
exit_error:
if (fd != -1)
close();
return ret;
}
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
}
int tims_socket(void)
{
return rt_dev_socket(PF_TIMS, SOCK_RAW, 0);
}
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 ret;
unsigned int i;
struct sockaddr_in local_addr;
struct in_addr dest_ip;
while (1) {
switch (getopt(argc, argv, "d:s:t")) {
case 'd':
dest_ip_s = optarg;
break;
case 's':
size = atoi(optarg);
break;
case 't':
start_timer = 1;
break;
case -1:
goto end_of_opt;
default:
printf("usage: %s -d <dest_ip> -s <size> -t\n", argv[0]);
return 0;
}
}
end_of_opt:
inet_aton(dest_ip_s, &dest_ip);
if (size > 65505)
size = 65505;
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
signal(SIGHUP, catch_signal);
mlockall(MCL_CURRENT|MCL_FUTURE);
printf("destination ip address %s=%08x\n", dest_ip_s, dest_ip.s_addr);
printf("size %d\n", size);
printf("start timer %d\n", start_timer);
/* fill output buffer with test pattern */
for (i = 0; i < sizeof(buffer_out); i++)
buffer_out[i] = i & 0xFF;
/* create rt-socket */
sock = rt_dev_socket(AF_INET,SOCK_DGRAM,0);
if (sock < 0) {
printf(" rt_dev_socket() = %d!\n", sock);
return sock;
}
/* extend the socket pool */
ret = rt_dev_ioctl(sock, RTNET_RTIOC_EXTPOOL, &add_rtskbs);
if (ret != (int)add_rtskbs) {
printf(" rt_dev_ioctl(RT_IOC_SO_EXTPOOL) = %d\n", ret);
rt_dev_close(sock);
return -1;
}
/* bind the rt-socket to a port */
memset(&local_addr, 0, sizeof(struct sockaddr_in));
local_addr.sin_family = AF_INET;
local_addr.sin_port = htons(PORT);
local_addr.sin_addr.s_addr = INADDR_ANY;
ret = rt_dev_bind(sock, (struct sockaddr *)&local_addr,
sizeof(struct sockaddr_in));
if (ret < 0) {
printf(" rt_dev_bind() = %d!\n", ret);
rt_dev_close(sock);
return ret;
}
/* set destination address */
memset(&dest_addr, 0, sizeof(struct sockaddr_in));
dest_addr.sin_family = AF_INET;
dest_addr.sin_port = htons(PORT);
dest_addr.sin_addr = dest_ip;
if (start_timer) {
rt_timer_start(TM_ONESHOT);
}
ret = rt_task_create(&rt_recv_task, "Receiver", 0, 10, 0);
if (ret != 0) {
printf(" rt_task_create(recv) = %d!\n", ret);
rt_dev_close(sock);
return ret;
}
ret = rt_task_create(&rt_xmit_task, "Sender", 0, 9, 0);
if (ret != 0) {
printf(" rt_task_create(xmit) = %d!\n", ret);
rt_dev_close(sock);
rt_task_delete(&rt_recv_task);
return ret;
}
rt_task_start(&rt_recv_task, recv_msg, NULL);
rt_task_start(&rt_xmit_task, xmit_msg, NULL);
pause();
if (start_timer)
rt_timer_stop();
/* Important: First close the socket! */
while (rt_dev_close(sock) == -EAGAIN) {
printf("frag-ip: Socket busy - waiting...\n");
sleep(1);
}
rt_task_delete(&rt_xmit_task);
rt_task_delete(&rt_recv_task);
return 0;
}
bool GDCNetwork::initialize(char* config_filename)
{
sockaddr_in temp_addr;
int temp_values[4];
int ret;
for(int i=1; i<=MAX_DOFS; i++)
{
ret = read_config_int_array(config_filename, gdc_joint_names_[i], 4, temp_values);
if(ret && temp_values[2])//check if the joint is in the config file and is active
{
printf("Activating Joint %s\n", gdc_joint_names_[i]);
//we create a map between gdc states and dofs
GDCState temp_gdcstate;
temp_gdcstate.dof_number_ = temp_values[3];
temp_gdcstate.card_number_ = temp_values[1];
temp_gdcstate.joint_name_ = std::string(gdc_joint_names_[i]);
gdc_card_states_.push_back(temp_gdcstate);
activeDOF_[i] = true;
//create the addressing structure
memset(&temp_addr, 0, sizeof(temp_addr));
temp_addr.sin_family = AF_INET;
temp_addr.sin_addr.s_addr = generateIPAddress(GDCNetworks_[temp_values[0]], temp_values[1]);
temp_addr.sin_port = htons(GDC_SENDING_PORT);
remote_ip_address_.push_back(temp_addr);
//create the receiving socket
printf("creating socket for port %d\n",i);
int temp_sock = rt_dev_socket(AF_INET, SOCK_DGRAM, 0);
if(temp_sock < 0)
{
printf("cannot create listener sock, port %d error: %d, %s", i, errno, strerror(errno));
return false;
}
//make the socket non blocking
int64_t tout = -1;
if(rt_dev_ioctl(temp_sock, RTNET_RTIOC_TIMEOUT, &tout) < 0)
{
printf("cannot make socket non-blocking, port %d error: %d, %s", i, errno, strerror(errno));
return false;
}
memset(&temp_addr, 0, sizeof(temp_addr));
temp_addr.sin_family = AF_INET;
temp_addr.sin_addr.s_addr = generateIPAddress(GDCNetworks_[temp_values[0]], BASE_HOST_ADDRESS);
temp_addr.sin_port = htons(REMOTE_PORT_BASE + temp_values[1]);
printf("binding\n");
if(rt_dev_bind(temp_sock, (struct sockaddr *)&temp_addr, sizeof(temp_addr)) < 0)
{
printf("cannot bind listener socket, port %d error: %d, %s", i, errno, strerror(errno));
}
receiving_sockets_.push_back(temp_sock);
}
else
activeDOF_[i] = false;
}
printf("dealing with foot sensors\n.");
for(int i=0; i<MAX_FOOT; i++)
{
ret = read_config_int_array(config_filename, gdc_foot_names_[i], 3, temp_values);
if(ret && temp_values[2])//check if the foot is in the config file and is active
{
//create a state
FootSensorState temp_footstate;
temp_footstate.foot_number_ = i;
temp_footstate.card_number_ = temp_values[1];
temp_footstate.foot_name_ = std::string(gdc_foot_names_[i]);
foot_sensor_states_.push_back(temp_footstate);
active_foot_sensor_[i] = true;
//create the addressing structure
memset(&temp_addr, 0, sizeof(temp_addr));
temp_addr.sin_family = AF_INET;
temp_addr.sin_addr.s_addr = generateIPAddress(GDCNetworks_[temp_values[0]], temp_values[1]);
temp_addr.sin_port = htons(GDC_SENDING_PORT);
remote_ip_address_foot_sensor_.push_back(temp_addr);
//create the receiving socket
printf("creating socket for port %d\n",i);
int temp_sock = rt_dev_socket(AF_INET, SOCK_DGRAM, 0);
if(temp_sock < 0)
{
printf("cannot create listener sock, port %d error: %d, %s", i, errno, strerror(errno));
return false;
}
//make the socket non blocking
int64_t tout = -1;
if(rt_dev_ioctl(temp_sock, RTNET_RTIOC_TIMEOUT, &tout) < 0)
{
printf("cannot make socket non-blocking, port %d error: %d, %s", i, errno, strerror(errno));
return false;
}
memset(&temp_addr, 0, sizeof(temp_addr));
temp_addr.sin_family = AF_INET;
temp_addr.sin_addr.s_addr = generateIPAddress(GDCNetworks_[temp_values[0]], BASE_HOST_ADDRESS);
temp_addr.sin_port = htons(REMOTE_PORT_BASE + temp_values[1]);
printf("binding\n");
if(rt_dev_bind(temp_sock, (struct sockaddr *)&temp_addr, sizeof(temp_addr)) < 0)
{
printf("cannot bind listener socket, port %d error: %d, %s", i, errno, strerror(errno));
}
receiving_sockets_foot_sensor_.push_back(temp_sock);
}
}
//create the multicast address
memset(&multi_cast_ip_address_, 0, sizeof(temp_addr));
multi_cast_ip_address_.sin_family = AF_INET;
inet_aton(NETWORK_MULTICAST_ADDRESS, &multi_cast_ip_address_.sin_addr);
multi_cast_ip_address_.sin_port = htons(GDC_SENDING_PORT);
//now we create a socket to send messages
printf("creating sending socket\n");
sender_socket_ = rt_dev_socket(AF_INET, SOCK_DGRAM, 0);
if(sender_socket_ < 0)
{
printf("cannot create sender_socket, error: %d\n", sender_socket_);
return false;
}
memset(&temp_addr, 0, sizeof(temp_addr));
temp_addr.sin_family = AF_INET;
temp_addr.sin_addr.s_addr = generateIPAddress(NETWORK_ADDRESS_1, BASE_HOST_ADDRESS);
temp_addr.sin_port = htons(GDC_SENDING_PORT);
printf("binding socket\n");
if(rt_dev_bind(sender_socket_, (struct sockaddr *)&temp_addr, sizeof(temp_addr)) < 0)
{
printf("cannot bind sender_socket, error: %d, %s\n", errno, strerror(errno));
return false;
}
//init the logging stuff
num_of_received_messages_.resize(receiving_sockets_.size());
num_of_received_foot_messages_.resize(receiving_sockets_foot_sensor_.size());
return true;
}
int main(int argc, char *argv[])
{
RT_TASK *task;
RTIME trp, max = 0, min = 1000000000, avrg = 0;
int sockfd, ret, hard_timer_running, i;
struct sockaddr_in local_addr, server_addr;
struct { long long count; char msg[100]; } msg = { 0LL, "this message was sent using rtnet-rtai." };
signal(SIGTERM, sigh);
signal(SIGINT, sigh);
signal(SIGHUP, sigh);
/* Set address structures to zero. */
memset(&local_addr, 0, sizeof(struct sockaddr_in));
memset(&server_addr, 0, sizeof(struct sockaddr_in));
/* Check arguments and set addresses. */
if (argc == 6) {
local_addr.sin_family = AF_INET;
local_addr.sin_addr.s_addr = INADDR_ANY;
local_addr.sin_port = htons(atoi(argv[1]));
server_addr.sin_family = AF_INET;
inet_aton(argv[2], &server_addr.sin_addr);
server_addr.sin_port = htons(atoi(argv[3]));
NR_TRIPS = atoi(argv[4]);
PERIOD = atoi(argv[5])*1000LL;
} else {
fprintf(stderr,
"Usage: "
"%s <local-port> "
"<server-ip> <server-port> "
"<number-of-trips> <sending-period-us>\n",
argv[0]);
return 1;
}
/* Create new socket. */
sockfd = rt_dev_socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
printf("Error opening socket: %d\n", sockfd);
return 1;
}
/* Link the Linux process to RTAI. */
if (!(hard_timer_running = rt_is_hard_timer_running())) {
start_rt_timer(0);
}
task = rt_thread_init(nam2num("SMPCLT"), 1, 0, SCHED_OTHER, 0xFF);
if (task == NULL) {
rt_dev_close(sockfd);
printf("CANNOT LINK LINUX SIMPLECLIENT PROCESS TO RTAI\n");
return 1;
}
/* Lock allocated memory into RAM. */
printf("RTnet, simpleclient for RTAI (user space).\n");
mlockall(MCL_CURRENT|MCL_FUTURE);
/* Switch over to hard realtime mode. */
rt_make_hard_real_time();
/* Bind socket to local address specified as parameter. */
ret = rt_dev_bind(sockfd, (struct sockaddr *)&local_addr, sizeof(struct sockaddr_in));
/* Specify destination address for socket; needed for rt_socket_send(). */
rt_dev_connect(sockfd, (struct sockaddr *) &server_addr, sizeof(struct sockaddr_in));
/* Send messages */
for (i = 1; i <= NR_TRIPS && !end; i++) {
rt_sleep(nano2count(PERIOD));
msg.count = i;
trp = rt_get_time_ns();
rt_dev_send(sockfd, &msg, sizeof(long long) + strlen(msg.msg) + 1, 0);
rt_dev_recv(sockfd, &msg, sizeof(msg), 0);
trp = (msg.count - trp)/1000;
if (trp < min) min = trp;
if (i > 3 && trp > max) max = trp;
avrg += trp;
printf("Client received: trip time %lld (us), %s\n", trp, msg.msg);
}
msg.count = -1;
rt_dev_send(sockfd, &msg, sizeof(long long) + strlen(msg.msg) + 1, 0);
/* Switch over to soft realtime mode. */
rt_make_soft_real_time();
/* Close socket, must be in soft-mode because socket was created as non-rt. */
rt_dev_close(sockfd);
/* Unlink the Linux process from RTAI. */
if (!hard_timer_running) {
stop_rt_timer();
}
rt_task_delete(task);
printf("Min trip time %lld, Max trip time %lld, Average trip time %lld\n", min, max, avrg/i);
return 0;
}
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);
}
int init_module(void)
{
int ret;
unsigned int i;
struct sockaddr_in local_addr;
unsigned long dest_ip = rt_inet_aton(dest_ip_s);
if (size > 65505)
size = 65505;
printk("destination ip address %s=%08x\n", dest_ip_s,
(unsigned int)dest_ip);
printk("size %d\n", size);
#ifdef CONFIG_RTOS_STARTSTOP_TIMER
printk("start timer %d\n", start_timer);
#endif
/* fill output buffer with test pattern */
for (i = 0; i < sizeof(buffer_out); i++)
buffer_out[i] = i & 0xFF;
/* create rt-socket */
sock = rt_dev_socket(AF_INET,SOCK_DGRAM,0);
if (sock < 0) {
printk(" rt_dev_socket() = %d!\n", sock);
return sock;
}
/* extend the socket pool */
ret = rt_dev_ioctl(sock, RTNET_RTIOC_EXTPOOL, &add_rtskbs);
if (ret != (int)add_rtskbs) {
printk(" rt_dev_ioctl(RT_IOC_SO_EXTPOOL) = %d\n", ret);
rt_dev_close(sock);
return -1;
}
/* bind the rt-socket to a port */
memset(&local_addr, 0, sizeof(struct sockaddr_in));
local_addr.sin_family = AF_INET;
local_addr.sin_port = htons(PORT);
local_addr.sin_addr.s_addr = INADDR_ANY;
ret = rt_dev_bind(sock, (struct sockaddr *)&local_addr,
sizeof(struct sockaddr_in));
if (ret < 0) {
printk(" rt_dev_bind() = %d!\n", ret);
rt_dev_close(sock);
return ret;
}
/* set destination address */
memset(&dest_addr, 0, sizeof(struct sockaddr_in));
dest_addr.sin_family = AF_INET;
dest_addr.sin_port = htons(PORT);
dest_addr.sin_addr.s_addr = dest_ip;
#ifdef CONFIG_RTOS_STARTSTOP_TIMER
if (start_timer) {
rtos_timer_start_oneshot();
}
#endif
ret = rtos_task_init(&rt_recv_task, recv_msg, 0, 9);
if (ret != 0)
{
printk(" rtos_task_init(recv) = %d!\n", ret);
rt_dev_close(sock);
return ret;
}
ret = rtos_task_init_periodic(&rt_xmit_task, send_msg, 0, 10, CYCLE);
if (ret != 0) {
printk(" rtos_task_init_periodic(xmit) = %d!\n", ret);
rt_dev_close(sock);
rtos_task_delete(&rt_recv_task);
return ret;
}
return 0;
}
int init_module(void)
{
int ret;
struct sockaddr_ll local_addr;
/* set destination address */
memset(&dest_addr, 0, sizeof(struct sockaddr_ll));
dest_addr.sll_family = AF_PACKET;
dest_addr.sll_protocol = htons(PROTOCOL);
dest_addr.sll_ifindex = local_if;
dest_addr.sll_halen = 6;
rt_eth_aton(dest_addr.sll_addr, dest_mac_s);
printk("destination mac address: %02X:%02X:%02X:%02X:%02X:%02X\n",
dest_addr.sll_addr[0], dest_addr.sll_addr[1],
dest_addr.sll_addr[2], dest_addr.sll_addr[3],
dest_addr.sll_addr[4], dest_addr.sll_addr[5]);
printk("local interface: %d\n", local_if);
/* create rt-socket */
sock = rt_dev_socket(AF_PACKET, SOCK_DGRAM, htons(PROTOCOL));
if (sock < 0) {
printk(" rt_dev_socket() = %d!\n", sock);
return sock;
}
/* bind the rt-socket to a port */
memset(&local_addr, 0, sizeof(struct sockaddr_ll));
local_addr.sll_family = AF_PACKET;
local_addr.sll_protocol = htons(PROTOCOL);
local_addr.sll_ifindex = local_if;
ret = rt_dev_bind(sock, (struct sockaddr *)&local_addr,
sizeof(struct sockaddr_ll));
if (ret < 0) {
printk(" rt_dev_bind() = %d!\n", ret);
goto cleanup_sock;
}
/* You may have to start the system timer manually
* on older Xenomai versions (2.0.x):
* rt_timer_start(TM_ONESHOT); */
ret = rt_task_create(&rt_recv_task, "recv_task", 0, 9, 0);
if (ret != 0) {
printk(" rt_task_create(rt_recv_task) = %d!\n", ret);
goto cleanup_sock;
}
ret = rt_task_start(&rt_recv_task, recv_msg, NULL);
if (ret != 0) {
printk(" rt_task_start(rt_recv_task) = %d!\n", ret);
goto cleanup_recv_task;
}
ret = rt_task_create(&rt_xmit_task, "xmit_task", 0, 10, 0);
if (ret != 0) {
printk(" rt_task_create(rt_xmit_task) = %d!\n", ret);
goto cleanup_recv_task;
}
ret = rt_task_set_periodic(&rt_xmit_task, TM_INFINITE, CYCLE);
if (ret != 0) {
printk(" rt_task_set_periodic(rt_xmit_task) = %d!\n", ret);
goto cleanup_xmit_task;
}
ret = rt_task_start(&rt_xmit_task, send_msg, NULL);
if (ret != 0) {
printk(" rt_task_start(rt_xmit_task) = %d!\n", ret);
goto cleanup_xmit_task;
}
return 0;
cleanup_xmit_task:
rt_task_delete(&rt_xmit_task);
cleanup_recv_task:
rt_task_delete(&rt_recv_task);
cleanup_sock:
rt_dev_close(sock);
return ret;
}
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;
}
int init_module(void)
{
int ret;
unsigned int i;
struct sockaddr_in local_addr;
unsigned long dest_ip = rt_inet_aton(dest_ip_s);
if (size > 65505)
size = 65505;
printk("destination ip address %s=%08x\n", dest_ip_s,
(unsigned int)dest_ip);
printk("size %d\n", size);
/* fill output buffer with test pattern */
for (i = 0; i < sizeof(buffer_out); i++)
buffer_out[i] = i & 0xFF;
/* create rt-socket */
sock = rt_dev_socket(AF_INET,SOCK_DGRAM,0);
if (sock < 0) {
printk(" rt_dev_socket() = %d!\n", sock);
return sock;
}
/* extend the socket pool */
ret = rt_dev_ioctl(sock, RTNET_RTIOC_EXTPOOL, &add_rtskbs);
if (ret != (int)add_rtskbs) {
printk(" rt_dev_ioctl(RT_IOC_SO_EXTPOOL) = %d\n", ret);
goto cleanup_sock;
}
/* bind the rt-socket to a port */
memset(&local_addr, 0, sizeof(struct sockaddr_in));
local_addr.sin_family = AF_INET;
local_addr.sin_port = htons(PORT);
local_addr.sin_addr.s_addr = INADDR_ANY;
ret = rt_dev_bind(sock, (struct sockaddr *)&local_addr,
sizeof(struct sockaddr_in));
if (ret < 0) {
printk(" rt_dev_bind() = %d!\n", ret);
goto cleanup_sock;
}
/* set destination address */
memset(&dest_addr, 0, sizeof(struct sockaddr_in));
dest_addr.sin_family = AF_INET;
dest_addr.sin_port = htons(PORT);
dest_addr.sin_addr.s_addr = dest_ip;
/* You may have to start the system timer manually
* on older Xenomai versions (2.0.x):
* rt_timer_start(TM_ONESHOT); */
ret = rt_task_create(&rt_recv_task, "recv_task", 0, 9, 0);
if (ret != 0) {
printk(" rt_task_create(rt_recv_task) = %d!\n", ret);
goto cleanup_sock;
}
ret = rt_task_start(&rt_recv_task, recv_msg, NULL);
if (ret != 0) {
printk(" rt_task_start(rt_recv_task) = %d!\n", ret);
goto cleanup_recv_task;
}
ret = rt_task_create(&rt_xmit_task, "xmit_task", 0, 10, 0);
if (ret != 0) {
printk(" rt_task_create(rt_xmit_task) = %d!\n", ret);
goto cleanup_recv_task;
}
ret = rt_task_set_periodic(&rt_xmit_task, TM_INFINITE, CYCLE);
if (ret != 0) {
printk(" rt_task_set_periodic(rt_xmit_task) = %d!\n", ret);
goto cleanup_xmit_task;
}
ret = rt_task_start(&rt_xmit_task, send_msg, NULL);
if (ret != 0) {
printk(" rt_task_start(rt_xmit_task) = %d!\n", ret);
goto cleanup_xmit_task;
}
return 0;
cleanup_xmit_task:
rt_task_delete(&rt_xmit_task);
cleanup_recv_task:
rt_dev_close(sock);
rt_task_delete(&rt_recv_task);
return ret;
cleanup_sock:
rt_dev_close(sock);
return ret;
}
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 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;
}
