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;
}
void CANSocket::close()
{
if (isOpen()) {
rt_dev_close(handle);
handle = NULL_HANDLE;
}
}
void cleanup(void)
{
time_t actual_duration;
long gmaxj, gminj, gavgj;
if (test_mode == USER_TASK) {
rt_sem_delete(&display_sem);
gavgjitter /= (test_loops > 1 ? test_loops : 2) - 1;
gminj = rt_timer_tsc2ns(gminjitter);
gmaxj = rt_timer_tsc2ns(gmaxjitter);
gavgj = rt_timer_tsc2ns(gavgjitter);
} else {
struct rttst_overall_bench_res overall;
overall.histogram_min = histogram_min;
overall.histogram_max = histogram_max;
overall.histogram_avg = histogram_avg;
rt_dev_ioctl(benchdev, RTTST_RTIOC_TMBENCH_STOP, &overall);
gminj = overall.result.min;
gmaxj = overall.result.max;
gavgj = overall.result.avg;
goverrun = overall.result.overruns;
}
if (benchdev >= 0)
rt_dev_close(benchdev);
if (need_histo())
dump_hist_stats();
time(&test_end);
actual_duration = test_end - test_start - WARMUP_TIME;
if (!test_duration)
test_duration = actual_duration;
printf
("---|-----------|-----------|-----------|--------|------|-------------------------\n"
"RTS|%11.3f|%11.3f|%11.3f|%8ld|%6u| %.2ld:%.2ld:%.2ld/%.2d:%.2d:%.2d\n",
(double)gminj / 1000, (double)gavgj / 1000, (double)gmaxj / 1000,
goverrun, max_relaxed, actual_duration / 3600, (actual_duration / 60) % 60,
actual_duration % 60, test_duration / 3600,
(test_duration / 60) % 60, test_duration % 60);
if (max_relaxed > 0)
printf(
"Warning! some latency maxima may have been due to involuntary mode switches.\n"
"Please contact [email protected]\n");
if (histogram_avg)
free(histogram_avg);
if (histogram_max)
free(histogram_max);
if (histogram_min)
free(histogram_min);
exit(0);
}
void cleanup_upon_sig(int sig __attribute__((unused)))
{
pthread_cancel (thid_square);
pthread_join (thid_square, NULL);
rt_dev_close (fd);
exit(0);
}
void Q8Close(device)
{
int ret;
ret = rt_dev_close(device);
if (ret < 0) {
rt_printf("ERROR : can't close device Q8\n");
exit(1);
}
else
fd=-1;
}
void cleanup_module(void)
{
/* In case you started it in this module, see comment above.
* rt_timer_stop(); */
/* Important: First close the socket! */
while (rt_dev_close(sock) == -EAGAIN) {
printk("raw-packets: Socket busy - waiting...\n");
set_current_state(TASK_UNINTERRUPTIBLE);
schedule_timeout(1*HZ); /* wait a second */
}
rt_task_delete(&rt_xmit_task);
rt_task_delete(&rt_recv_task);
}
static void cleanup(void)
{
int ret;
if (verbose)
printf("Cleaning up...\n");
if (s >= 0) {
ret = rt_dev_close(s);
s = -1;
if (ret) {
fprintf(stderr, "rt_dev_close: %s\n", strerror(-ret));
}
rt_task_delete(&rt_task_desc);
}
}
static void _cleanup(void)
{
rt_printk("RtnetTest: Module shutdown requested\n");
end = 1;
rt_task_delete(&rx_task);
rt_task_delete(&tx_task);
rt_dev_close(sock);
rt_dev_shutdown(sock, SHUT_RDWR);
rt_named_mbx_delete(mbx);
rt_printk("RtnetTest: Module shutdown completed\n");
}
void cleanup_module(void)
{
/* In case you started it in this module, see comment above.
* rt_timer_stop(); */
/* Note: The following loop and the strict ordering "close before task
* termination" is no longer required since Xenomai 2.4. */
while (rt_dev_close(sock) == -EAGAIN) {
printk("frag-ip: Socket busy - waiting...\n");
set_current_state(TASK_UNINTERRUPTIBLE);
schedule_timeout(1*HZ); /* wait a second */
}
rt_task_delete(&rt_xmit_task);
rt_task_delete(&rt_recv_task);
}
void cleanup_module(void)
{
#ifdef CONFIG_RTOS_STARTSTOP_TIMER
if (start_timer)
rtos_timer_stop();
#endif
/* Important: First close the socket! */
while (rt_dev_close(sock) == -EAGAIN) {
printk("frag-ip: Socket busy - waiting...\n");
set_current_state(TASK_UNINTERRUPTIBLE);
schedule_timeout(1*HZ); /* wait a second */
}
rtos_task_delete(&rt_xmit_task);
rtos_task_delete(&rt_recv_task);
}
int main(int argc, char *argv[])
{
char buf[1024];
ssize_t size;
int device;
int ret;
/* open the device */
device = rt_dev_open(DEVICE_NAME, 0);
if (device < 0) {
printf("ERROR : can't open device %s (%s)\n",
DEVICE_NAME, strerror(-device));
fflush(stdout);
exit(1);
}
/* first read */
size = rt_dev_read (device, (void *)buf, 1024);
printf("Read in device %s\t: %d bytes\n", DEVICE_NAME, size);
/* first write */
sprintf(buf, "HelloWorld!");
size = rt_dev_write (device, (const void *)buf, strlen(buf) + 1);
printf("Write from device %s\t: %d bytes\n", DEVICE_NAME, size);
/* second read */
size = rt_dev_read (device, (void *)buf, 1024);
printf("Read in device %s\t: %s\n", DEVICE_NAME, buf);
/* third read */
size = rt_dev_read (device, (void *)buf, 1024);
printf("Read in device %s\t: %d bytes\n", DEVICE_NAME, size);
/* close the device */
ret = rt_dev_close(device);
if (ret < 0) {
printf("ERROR : can't close device %s (%s)\n",
DEVICE_NAME, strerror(-ret));
fflush(stdout);
exit(1);
}
return 0;
}
int main (int argc, char * argv[])
{
int fd;
int i;
if (argc < 2) {
fprintf(stderr, "usage: %s rtdm_device\n", argv[0]);
exit(EXIT_FAILURE);
}
fd = rt_dev_open(argv[1], O_WRONLY);
if (fd < 0) {
fprintf(stderr, "%s: %s\n", argv[1], strerror(-fd));
exit(EXIT_FAILURE);
}
for (i = 2; i < argc; i ++)
rt_dev_write(fd, argv[i], strlen(argv[i]));
rt_dev_close(fd);
exit(EXIT_SUCCESS);
}
/**
* @brief Close a CAN device
*
* This function closes a CAN device which was opened with @a open().
*
* @return 0 on success, otherwise negative error code
*
* Environments:
*
* This service can be called from:
*
* - User-space task (non-RT)
*
* Rescheduling: possible.
*/
int CanPort::close(void)
{
int i = 5;
int ret;
do
{
ret = rt_dev_close(fd);
if (!ret) // exit on success
{
fd = -1;
return 0;
}
else if (ret == -EAGAIN) // try it again (max 5 times)
{
sleep(1); // wait 1s
}
}
while (ret != -EAGAIN && i--);
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 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;
}
static int __init klat_mod_init(void)
{
char devname[RTDM_MAX_DEVNAME_LEN + 1];
unsigned dev_nr;
int err;
err = rt_pipe_create(&klat_pipe, "klat_pipe", pipe, 4096);
if (err) {
printk("rt_pipe_create(klat_pipe): %d\n", err);
return err;
}
err = rt_task_create(&klat_srvr, "klat_srvr", 0, 0, 0);
if (err) {
printk("rt_task_create(klat_srvr): %d\n", err);
goto err_close_pipe;
}
pkt.config.mode = mode;
pkt.config.priority = priority;
pkt.config.period = period * 1000;
pkt.config.warmup_loops = 1;
pkt.config.histogram_size = 0;
pkt.config.freeze_max = freeze_max;
for (dev_nr = 0; dev_nr < DEV_NR_MAX; dev_nr++) {
snprintf(devname, sizeof(devname),
"rttest-timerbench%d",
dev_nr);
fd = rt_dev_open(devname, O_RDONLY);
if (fd < 0)
continue;
err = rt_dev_ioctl(fd, RTTST_RTIOC_TMBENCH_START, &pkt.config);
if (err == -ENOTTY) {
rt_dev_close(fd);
continue;
}
if (err < 0) {
printk("rt_dev_ioctl(RTTST_RTIOC_TMBENCH_START): %d\n",
err);
goto err_destroy_task;
}
break;
}
if (fd < 0) {
printk("rt_dev_open: could not find rttest device\n"
"(modprobe timerbench?)");
err = fd;
goto err_destroy_task;
}
err = rt_task_start(&klat_srvr, &klat_server, NULL);
if (err) {
printk("rt_task_start: %d\n", err);
goto err_close_dev;
}
return 0;
err_close_dev:
rt_dev_close(fd);
err_destroy_task:
rt_task_delete(&klat_srvr);
err_close_pipe:
rt_pipe_delete(&klat_pipe);
return err;
}
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;
}
int tims_close(int fd)
{
return rt_dev_close(fd);
}
static void klat_mod_exit(void)
{
rt_dev_close(fd);
rt_task_delete(&klat_srvr);
rt_pipe_delete(&klat_pipe);
}
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 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;
}
CANDev::~CANDev() {
if (dev > -1) {
rt_dev_close(dev);
}
}
/*!*****************************************************************************
*******************************************************************************
\note close_serial
\date Oct 2000
\remarks
closes the serial connection
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] fd : file descriptor
******************************************************************************/
void
close_serial(int fd)
{
rt_dev_close(fd);
}
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;
}
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 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;
}
