"change baudrate" implemented for lincan interface

diff --git a/drivers/can_lincan/can_lincan.c b/drivers/can_lincan/can_lincan.c
index 4e81849060f4680dbcd1abc5c0283b665105db75..16bdb665fc9669fb8c287a9a8ec173d727eb5064 100644 (file)
--- a/drivers/can_lincan/can_lincan.c
+++ b/drivers/can_lincan/can_lincan.c
@@ -37,7 +37,6 @@ UNS8 canReceive_driver(CAN_HANDLE fd0, Message *m)
   struct canmsg_t canmsg;
 
   canmsg.flags = 0; /* Ensure standard receive, not required for LinCAN>=0.3.1 */
-
   do{
     res = read(fd0,&canmsg,sizeof(canmsg_t));
     if((res<0)&&(errno == -EAGAIN)) res = 0;
@@ -90,10 +89,35 @@ UNS8 canSend_driver(CAN_HANDLE fd0, Message *m)
 }
 
 /***************************************************************************/
-UNS8 canChangeBaudRate_driver( CAN_HANDLE fd, char* baud)
+int TranslateBaudeRate(char* optarg){
+       if(!strcmp( optarg, "1M")) return 1000000;
+       if(!strcmp( optarg, "500K")) return 500000;
+       if(!strcmp( optarg, "250K")) return 250000;
+       if(!strcmp( optarg, "125K")) return 125000;
+       if(!strcmp( optarg, "100K")) return 100000;
+       if(!strcmp( optarg, "50K")) return 50000;
+       if(!strcmp( optarg, "20K")) return 20000;
+       if(!strcmp( optarg, "none")) return 0;
+       return 0x0000;
+}
+
+/***************************************************************************/
+UNS8 canChangeBaudRate_driver( CAN_HANDLE fd0, char* baud)
 {
-       printf("canChangeBaudRate not yet supported by this driver\n");
-       return 0;
+       struct can_baudparams_t params;
+
+       params.baudrate = TranslateBaudeRate(baud);
+       if(params.baudrate == 0)
+               return 0;
+       params.flags = -1;      // use driver defaults
+       params.sjw = -1;        // use driver defaults
+       params.sample_pt = -1;  // use driver defaults
+       if(ioctl((int)fd0, CONF_BAUDPARAMS, &params) < 0)
+       {
+               fprintf(stderr, "canOpen_driver (lincan): IOCTL set speed failed\n");
+               return 0;
+       }
+       return 1;
 }
 
 /***************************************************************************/
@@ -101,33 +125,45 @@ static const char lnx_can_dev_prefix[] = "/dev/can";
 
 CAN_HANDLE canOpen_driver(s_BOARD *board)
 {
-  int name_len = strlen(board->busname);
-  int prefix_len = strlen(lnx_can_dev_prefix);
-  char dev_name[prefix_len+name_len+1];
-  int o_flags = 0;
-  CAN_HANDLE fd0;
-
-  fd0=malloc(sizeof(*fd0));
-  if(fd0==NULL)
-    return NULL;
-
-  /*o_flags = O_NONBLOCK;*/
-
-  memcpy(dev_name,lnx_can_dev_prefix,prefix_len);
-  memcpy(dev_name+prefix_len,board->busname,name_len);
-  dev_name[prefix_len+name_len] = 0;
-
-  fd0 = open(dev_name, O_RDWR|o_flags);
-  if(fd0 < 0){
-    fprintf(stderr,"!!! Board %s is unknown. See can_lincan.c\n", board->busname);
-    goto error_ret;
-  }
-
-  return fd0;
-
- error_ret:
-  free(fd0);
-  return NULL;
+       int name_len = strlen(board->busname);
+       int prefix_len = strlen(lnx_can_dev_prefix);
+       char dev_name[prefix_len+name_len+1];
+       struct can_baudparams_t params;
+       int o_flags = 0;
+       int fd;
+
+       /*o_flags = O_NONBLOCK;*/
+
+       memcpy(dev_name,lnx_can_dev_prefix,prefix_len);
+       memcpy(dev_name+prefix_len,board->busname,name_len);
+       dev_name[prefix_len+name_len] = 0;
+       printf("dev_name %s\n", dev_name);
+
+       fd = open(dev_name, O_RDWR|o_flags);
+       if(fd < 0)
+       {
+               fprintf(stderr,"!!! Board %s is unknown. See can_lincan.c\n", board->busname);
+               goto error_ret;
+       }
+       printf("fd = %d\n", fd);
+
+       // set baudrate
+       params.baudrate = TranslateBaudeRate(board->baudrate);
+       if(params.baudrate == 0)
+               goto error_ret;
+       params.flags = -1;      // use driver defaults
+       params.sjw = -1;        // use driver defaults
+       params.sample_pt = -1;  // use driver defaults
+       if(ioctl(fd, CONF_BAUDPARAMS, &params) < 0)
+       {
+               fprintf(stderr, "canOpen_driver (lincan): IOCTL set speed failed\n");
+               goto error_ret;
+       }
+
+       return (CAN_HANDLE)fd;
+
+error_ret:
+       return NULL;
 }
 
 /***************************************************************************/