From 5aea8922ea6f623054aeb9d61da8a90ced7ee6c4 Mon Sep 17 00:00:00 2001
From: greg <greg>
Date: Thu, 5 Jun 2008 07:09:56 +0000
Subject: [PATCH] "change baudrate" implemented for lincan interface

---
 drivers/can_lincan/can_lincan.c | 98 ++++++++++++++++++++++-----------
 1 file changed, 67 insertions(+), 31 deletions(-)

diff --git a/drivers/can_lincan/can_lincan.c b/drivers/can_lincan/can_lincan.c
index 4e81849..16bdb66 100644
--- 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;
 }
 
 /***************************************************************************/
-- 
2.39.2