lincan/utils/readburst_rtl.c

   1 #include <rtl.h>
   2 #include <time.h>
   3 #include <signal.h>
   4 #include <pthread.h>
   5 #include <posix/unistd.h>
   6
   7 #define printf rtl_printf
   8
   9 #include "../include/can.h"
  10
  11 int fd;
  12
  13 struct canfilt_t canfilt = {
  14         .flags = 0,
  15         .queid = 0,
  16         .cob = 0,
  17         .id = 0,
  18         .mask = 0
  19 };
  20
  21 int canfilt_fl;
  22
  23 int canfilt_mask, canfilt_id, canfilt_flags;
  24
  25
  26 int can_wait_sec = 5;
  27
  28 char *can_dev_name = "/dev/can0";
  29
  30 #define PRT_PREFIX_SIZE 40
  31 char prt_prefix[PRT_PREFIX_SIZE];
  32
  33 char *prt_prefix_in = "CAN %s : ";
  34
  35 MODULE_PARM_DESC(can_dev_name,"name of CAN device [/dev/can0]");
  36 MODULE_PARM(can_dev_name,"1s");
  37 MODULE_PARM_DESC(canfilt_mask,"CAN filter mask");
  38 MODULE_PARM(canfilt_mask,"1i");
  39 MODULE_PARM_DESC(canfilt_id,"CAN filter message ID");
  40 MODULE_PARM(canfilt_id,"1i");
  41 MODULE_PARM_DESC(canfilt_flags,"CAN filter flags");
  42 MODULE_PARM(canfilt_flags,"1i");
  43 MODULE_PARM_DESC(can_wait_sec,"number of seconds to wait between messages");
  44 MODULE_PARM(can_wait_sec,"1i");
  45 MODULE_PARM_DESC(prt_prefix_in,"string prefix for output");
  46 MODULE_PARM(prt_prefix_in,"1i");
  47
  48 MODULE_SUPPORTED_DEVICE("sendburst_rtl");
  49 MODULE_AUTHOR("Pavel Pisa <pisa@cmp.felk.cvut.cz>");
  50 MODULE_DESCRIPTION("readburst_rtl: receiver of CAN messages for RT-Linux interface to LinCAN driver");
  51 MODULE_LICENSE("GPL");
  52
  53 void readburst_cleanup(void *arg)
  54 {
  55         printf("%scleanup handler called\n", prt_prefix);
  56         close((int)arg);
  57 }
  58
  59 #if 0
  60 int can_fd_wait(int fd, int wait_sec)
  61 {
  62         int ret;
  63         struct timeval timeout;
  64         fd_set set;
  65
  66         FD_ZERO (&set);
  67         FD_SET (fd, &set);
  68         timeout.tv_sec = wait_sec;
  69         timeout.tv_usec = 0;
  70         while ((ret=select(FD_SETSIZE,&set, NULL, NULL,&timeout))==-1
  71           &&errno==-EINTR);
  72         return ret;
  73 }
  74 #endif
  75
  76 int readburst_main(void *arg)
  77 {
  78         int n,ret;
  79         unsigned long i=0;
  80        #ifdef CAN_MSG_VERSION_2
  81         struct canmsg_t readmsg={0,0,5,{0,0},0,{0,}};
  82        #else /* CAN_MSG_VERSION_2 */
  83         struct canmsg_t readmsg={0,0,5,0,0,{0,}};
  84        #endif /* CAN_MSG_VERSION_2 */
  85
  86         if(canfilt_mask || canfilt_id || canfilt_flags){
  87                 canfilt_fl=1;
  88                 canfilt.mask=canfilt_mask;
  89                 canfilt.id=canfilt_id;
  90                 canfilt.flags=canfilt_flags;
  91         }
  92
  93         if ((fd=open(can_dev_name, O_RDWR)) < 0) {
  94                 printf("Error opening %s\n", can_dev_name);
  95                 return -1;
  96         }
  97         pthread_cleanup_push(readburst_cleanup,(void*)fd);
  98
  99         snprintf(prt_prefix, PRT_PREFIX_SIZE, prt_prefix_in, can_dev_name);
 100
 101         if (canfilt_fl) {
 102                 ret = ioctl(fd, CANQUE_FILTER, &canfilt);
 103                 if(ret<0) {
 104                         printf("%serror in call ioctl FILTER_QUE",prt_prefix);
 105                 }
 106         }
 107
 108
 109         while (1) {
 110                 readmsg.flags=0;
 111                 readmsg.cob=0;
 112             #if 0
 113                 ret=can_fd_wait(fd, can_wait_sec);
 114                 printf("%scan_fd_wait returned %d\n", prt_prefix, ret);
 115             #endif
 116                 ret=read(fd,&readmsg,sizeof(struct canmsg_t));
 117                 if(ret <0) {
 118                         printf("%sError reading message\n", prt_prefix);
 119                 }
 120                 else if(ret == 0) {
 121                         printf("%sNo message arrived\n", prt_prefix);
 122                 } else {
 123                         printf("%sRx msg #%lu: id=%lX dlc=%u flg=0x%02x",
 124                                 prt_prefix,i,readmsg.id,readmsg.length,readmsg.flags);
 125                         for(n=0 ; n<readmsg.length ; n++)
 126                                 printf(" %.2X",(unsigned char)readmsg.data[n]);
 127                         printf("\n");
 128                         i++;
 129                 }
 130                 pthread_testcancel();
 131
 132         }
 133         /* close(fd); is called by cleanup handler*/
 134         pthread_cleanup_pop(1);
 135         return 0;
 136 }
 137
 138 /*===========================================================*/
 139
 140
 141 void * t1_routine(void *arg)
 142 {
 143   readburst_main(NULL);
 144
 145   while (1) {
 146     pthread_wait_np ();
 147
 148   }
 149   return 0;
 150 }
 151
 152 pthread_t t1;
 153
 154 int init_module(void) {
 155
 156         return pthread_create (&t1, NULL, t1_routine, 0);
 157
 158 }
 159
 160 void cleanup_module(void) {
 161         pthread_delete_np (t1);
 162
 163         /*pthread_cancel(t1);
 164         pthread_join(t1, NULL);*/
 165 }