Set the baud rate correctly.

1 files changed, 22 insertions, 17 deletions

diff --git a/serial.c b/serial.c
index 648c52b2..d21edfd7 100644
--- a/serial.c
+++ b/serial.c
@@ -15,6 +15,27 @@
 #  endif /* CNEW_RTSCTS */
 #endif /* !CRTSCTS */
 
+void gpsd_set_speed(struct termios *ttyctl, int device_speed)
+{
+    if (device_speed < 200)
+      device_speed *= 1000;
+    if (device_speed < 2400)
+      device_speed =  B1200;
+    else if (device_speed < 4800)
+      device_speed =  B2400;
+    else if (device_speed < 9600)
+      device_speed =  B4800;
+    else if (device_speed < 19200)
+      device_speed =  B9600;
+    else if (device_speed < 38400)
+      device_speed =  B19200;
+    else
+      device_speed =  B38400;
+
+    cfsetispeed(ttyctl, (speed_t)device_speed);
+    cfsetospeed(ttyctl, (speed_t)device_speed);
+}
+
 int gpsd_open(int device_speed, int stopbits, struct gps_session_t *session)
 {
     int ttyfd;
@@ -29,24 +50,8 @@ int gpsd_open(int device_speed, int stopbits, struct gps_session_t *session)
 	if (tcgetattr(ttyfd,&session->ttyset_old) != 0)
 	  return -1;
 
-	if (device_speed < 200)
-	  device_speed *= 1000;
-	if (device_speed < 2400)
-	  device_speed =  B1200;
-	else if (device_speed < 4800)
-	  device_speed =  B2400;
-	else if (device_speed < 9600)
-	  device_speed =  B4800;
-	else if (device_speed < 19200)
-	  device_speed =  B9600;
-	else if (device_speed < 38400)
-	  device_speed =  B19200;
-	else
-	  device_speed =  B38400;
-
 	memcpy(&session->ttyset, &session->ttyset_old, sizeof(session->ttyset));
-	cfsetispeed(&session->ttyset, (speed_t)device_speed);
-	cfsetospeed(&session->ttyset, (speed_t)device_speed);
+	gpsd_set_speed(&session->ttyset, device_speed);
 	session->ttyset.c_cflag &= ~(PARENB | CRTSCTS);
 	session->ttyset.c_cflag |= (CSIZE & (stopbits==2 ? CS7 : CS8)) | CREAD | CLOCAL;
 	session->ttyset.c_iflag = session->ttyset.c_oflag = session->ttyset.c_lflag = (tcflag_t) 0;