"change baudrate" implemented for lincan interface
authorgreg
Thu, 05 Jun 2008 09:09:56 +0200
changeset 477 ec7654f71964
parent 476 08cdcb021beb
child 478 4306e851d95c
"change baudrate" implemented for lincan interface
drivers/can_lincan/can_lincan.c
--- a/drivers/can_lincan/can_lincan.c	Thu Jun 05 08:04:30 2008 +0200
+++ b/drivers/can_lincan/can_lincan.c	Thu Jun 05 09:09:56 2008 +0200
@@ -37,7 +37,6 @@
   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 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 @@
 
 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;
+	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;
 
-  fd0=malloc(sizeof(*fd0));
-  if(fd0==NULL)
-    return NULL;
+	/*o_flags = O_NONBLOCK;*/
 
-  /*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);
 
-  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;
+	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);
 
-  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;
-  }
+	// 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 fd0;
+	return (CAN_HANDLE)fd;
 
- error_ret:
-  free(fd0);
-  return NULL;
+error_ret:
+	return NULL;
 }
 
 /***************************************************************************/