From c13c9806f957ebc675462737f4b328d3ab89e028 Mon Sep 17 00:00:00 2001
From: guowenxue <guowenxue@gmail.com>
Date: Mon, 10 Jul 2023 17:29:22 +0800
Subject: [PATCH] update gpsd.c
---
gpsd/booster/comport.c | 515 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 515 insertions(+), 0 deletions(-)
diff --git a/gpsd/booster/comport.c b/gpsd/booster/comport.c
new file mode 100644
index 0000000..27de29a
--- /dev/null
+++ b/gpsd/booster/comport.c
@@ -0,0 +1,515 @@
+/*
+ * Copyright (c) 2022 Guo Wenxue
+ * Author: Guo Wenxue <guowenxue@gmail.com>
+ *
+ * This library is free software; you can redistribute it and/or modify it
+ * under the terms of the GPL license.
+ */
+
+#include "comport.h"
+
+#define CONFIG_PRINT_LOGGER
+//#define CONFIG_PRINT_STDOUT
+
+#if ( defined CONFIG_PRINT_LOGGER )
+#include "logger.h"
+#define dbg_print(format,args...) log_error(format, ##args)
+
+#elif ( defined CONFIG_PRINT_STDOUT )
+#define dbg_print(format,args...) printf(format, ##args)
+
+#else
+#define dbg_print(format,args...) do{} while(0);
+#endif
+
+
+static inline void set_settings(comport_t * comport, const char *settings);
+
+/*
+ * description: Open the serial port
+ *
+ * input args: $comport: corresponding comport point
+ * $dev_name: The comport device name path, such as '/dev/ttyS3'
+ * $baudrate: The baudrate, such as 115200
+ * $settings: The databit,parity,stopbit,flowctrl settings, such as '8N1N'
+ *
+ * return value: The comport opened file description, <0 means failure
+ */
+int comport_open(comport_t *comport, const char *devname, long baudrate, const char *settings)
+{
+ int rv = -1;
+ struct termios old_cfg, new_cfg;
+ int old_flags;
+ long tmp;
+
+ if( !comport || !devname )
+ {
+ dbg_print("invalid input arugments\n");
+ return -1;
+ }
+
+ /*+-----------------------+
+ *| open the serial port |
+ *+-----------------------+*/
+
+ memset(comport, 0, sizeof(*comport));
+ strncpy(comport->devname, devname, sizeof(comport->devname));
+ comport->baudrate = baudrate;
+ comport->fd = -1;
+ comport->fragsize = CONFIG_DEF_FRAGSIZE;
+ set_settings(comport, settings);
+
+ if( !strstr(comport->devname, "tty") )
+ {
+ dbg_print("comport device \"%s\" is not tty device\n", comport->devname);
+ return -2;
+ }
+
+ comport->fd = open(comport->devname, O_RDWR | O_NOCTTY | O_NONBLOCK);
+ if( comport->fd<0 )
+ {
+ dbg_print("comport open \"%s\" failed:%s\n", comport->devname, strerror(errno));
+ return -3;
+ }
+
+ if( (-1 != (old_flags = fcntl(comport->fd, F_GETFL, 0)))
+ && (-1 != fcntl(comport->fd, F_SETFL, old_flags & ~O_NONBLOCK)) )
+ {
+ /* Flush input and output */
+ tcflush(comport->fd, TCIOFLUSH);
+ }
+ else
+ {
+ rv = -4;
+ goto CleanUp;
+ }
+
+ if (0 != tcgetattr(comport->fd, &old_cfg))
+ {
+ rv = -5;
+ goto CleanUp;
+ }
+
+
+ /*+-----------------------+
+ *| configure serial port |
+ *+-----------------------+*/
+
+ memset(&new_cfg, 0, sizeof(new_cfg));
+ new_cfg.c_cflag &= ~CSIZE;
+ new_cfg.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+ new_cfg.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);
+ new_cfg.c_oflag &= ~(OPOST);
+
+ /* Set the data bit */
+ switch (comport->databit)
+ {
+ case 0x07:
+ new_cfg.c_cflag |= CS7;
+ break;
+ case 0x06:
+ new_cfg.c_cflag |= CS6;
+ break;
+ case 0x05:
+ new_cfg.c_cflag |= CS5;
+ break;
+ default:
+ new_cfg.c_cflag |= CS8;
+ break;
+ }
+
+ /* Set the parity */
+ switch (comport->parity)
+ {
+ case 0x01: /* Odd */
+ new_cfg.c_cflag |= (PARENB | PARODD);
+ new_cfg.c_cflag |= (INPCK | ISTRIP);
+ break;
+ case 0x02: /* Even */
+ new_cfg.c_cflag |= PARENB;
+ new_cfg.c_cflag &= ~PARODD;;
+ new_cfg.c_cflag |= (INPCK | ISTRIP);
+ break;
+ case 0x03:
+ new_cfg.c_cflag &= ~PARENB;
+ new_cfg.c_cflag &= ~CSTOPB;
+ break;
+ default:
+ new_cfg.c_cflag &= ~PARENB;
+ }
+
+ /* Set Stop bit */
+ if (0x01 != comport->stopbit)
+ {
+ new_cfg.c_cflag |= CSTOPB;
+ }
+ else
+ {
+ new_cfg.c_cflag &= ~CSTOPB;
+ }
+
+ /* Set flow control */
+ switch (comport->flowctrl)
+ {
+ case 1: /* Software control */
+ case 3:
+ new_cfg.c_cflag &= ~(CRTSCTS);
+ new_cfg.c_iflag |= (IXON | IXOFF);
+ break;
+ case 2: /* Hardware control */
+ new_cfg.c_cflag |= CRTSCTS;
+ new_cfg.c_iflag &= ~(IXON | IXOFF);
+ break;
+ default: /* NONE */
+ new_cfg.c_cflag &= ~(CRTSCTS);
+ new_cfg.c_iflag &= ~(IXON | IXOFF);
+ break;
+ }
+
+ /* Set baudrate */
+ switch (comport->baudrate)
+ {
+ /* Upper is not POSIX(bits/termios-baud.h) */
+ case 4000000:
+ tmp = B4000000;
+ break;
+ case 3500000:
+ tmp = B3500000;
+ break;
+ case 3000000:
+ tmp = B3000000;
+ break;
+ case 2500000:
+ tmp = B2500000;
+ break;
+ case 2000000:
+ tmp = B2000000;
+ break;
+ case 1500000:
+ tmp = B1500000;
+ break;
+ case 1152000:
+ tmp = B1152000;
+ break;
+ case 1000000:
+ tmp = B1000000;
+ break;
+ case 921600:
+ tmp = B921600;
+ break;
+ case 576000:
+ tmp = B576000;
+ break;
+ case 500000:
+ tmp = B500000;
+ break;
+ case 460800:
+ tmp = B460800;
+ break;
+ case 230400:
+ tmp = B230400;
+ break;
+ case 115200:
+ tmp = B115200;
+ break;
+ case 57600:
+ tmp = B57600;
+ break;
+
+ /* Below is POSIX(bits/termios.h) */
+ case 38400:
+ tmp = B38400;
+ break;
+ case 19200:
+ tmp = B19200;
+ break;
+ case 9600:
+ tmp = B9600;
+ break;
+ case 4800:
+ tmp = B4800;
+ break;
+ case 2400:
+ tmp = B2400;
+ break;
+ case 1800:
+ tmp = B1800;
+ break;
+ case 1200:
+ tmp = B1200;
+ break;
+ case 600:
+ tmp = B600;
+ break;
+ case 300:
+ tmp = B300;
+ break;
+ case 200:
+ tmp = B200;
+ break;
+ case 150:
+ tmp = B150;
+ break;
+ case 134:
+ tmp = B134;
+ break;
+ case 110:
+ tmp = B110;
+ break;
+ case 75:
+ tmp = B75;
+ break;
+ case 50:
+ tmp = B50;
+ break;
+ default:
+ tmp = B115200;
+ }
+ cfsetispeed(&new_cfg, tmp);
+ cfsetispeed(&new_cfg, tmp);
+
+ /* Set the Com port timeout settings */
+ new_cfg.c_cc[VMIN] = 0;
+ new_cfg.c_cc[VTIME] = 0;
+
+ tcflush(comport->fd, TCIFLUSH);
+ if (0 != tcsetattr(comport->fd, TCSANOW, &new_cfg))
+ {
+ rv = -6; // Failed to set device com port settings
+ goto CleanUp;
+ }
+
+ rv = comport->fd;
+
+CleanUp:
+ return rv;
+}
+
+
+/*
+ * description: close comport
+ * input args: $comport: corresponding comport point
+ */
+
+void comport_close(comport_t *comport)
+{
+ if( !comport )
+ {
+ dbg_print("invalid input arugments\n");
+ return ;
+ }
+
+ if ( comport->fd >= 0 )
+ {
+ close(comport->fd);
+ }
+
+ comport->fd = -1;
+ return ;
+}
+
+/*
+ * description: write $data_bytes $data to $comport
+ * return value: 0: write ok <0: write failure
+ */
+
+int comport_send(comport_t *comport, char *data, int data_bytes)
+{
+ char *ptr;
+ int left, bytes = 0;
+ int rv = 0;
+
+ if( !comport || !data || data_bytes<=0 )
+ {
+ dbg_print("invalid input arugments\n");
+ return -1;
+ }
+
+ if( comport->fd < 0 )
+ {
+ dbg_print("Serail port not opened\n");
+ return -2;
+ }
+
+ ptr = data;
+ left = data_bytes;
+
+ while( left > 0 )
+ {
+ /* Large data, then slice them to frag and send */
+ bytes = left>comport->fragsize ? comport->fragsize : left;
+
+ rv = write(comport->fd, ptr, bytes);
+ if( rv<0 )
+ {
+ rv = -3;
+ break;
+ }
+
+ left -= rv;
+ ptr += rv;
+ }
+
+ return rv;
+}
+
+
+/*
+ * description: read data from $comport in $timeout <ms> to $buf no more than $buf_size bytes
+ * return value: the actual read data bytes, <0: read failure
+ */
+
+int comport_recv(comport_t *comport, char *buf, int buf_size, unsigned long timeout)
+{
+ fd_set rdfds, exfds;
+ struct timeval to, *to_ptr = NULL;
+ int ret, rv = 0;
+ int bytes = 0;
+
+ if ( !comport || !buf || buf_size<=0 )
+ {
+ dbg_print("invalid input arugments\n");
+ return -1;
+ }
+
+ if ( comport->fd < 0 )
+ {
+ dbg_print("Serail port not opened\n");
+ return -2;
+ }
+
+ memset(buf, 0, buf_size);
+
+ FD_ZERO(&rdfds);
+ FD_ZERO(&exfds);
+ FD_SET(comport->fd, &rdfds);
+ FD_SET(comport->fd, &exfds);
+
+ if( TIMEOUT_NONE != timeout )
+ {
+ to.tv_sec = (time_t) (timeout / 1000);
+ to.tv_usec = (long)(1000 * (timeout % 1000));
+ to_ptr = &to;
+ }
+
+ while( 1 )
+ {
+ /* check got data arrive or not */
+ ret = select(comport->fd+1, &rdfds, 0, &exfds, to_ptr);
+ if( ret<0 )
+ {
+ /* EINTR means catch interrupt signal */
+ dbg_print("comport select() failed: %s\n", strerror(errno));
+ rv = EINTR==errno ? 0 : -3;
+ break;
+ }
+ else if( 0 == ret ) /* timeout */
+ {
+ break;
+ }
+
+ /* read data from comport */
+ ret = read(comport->fd, buf+bytes, buf_size-bytes);
+ if(ret <= 0)
+ {
+ dbg_print("comport read() failed: %s\n", strerror(errno));
+ break;
+ }
+
+ bytes += ret;
+ if( bytes >= buf_size )
+ break;
+
+ /* try to read data in 1ms again, if no data arrive it will break */
+ to.tv_sec = 0;
+ to.tv_usec = 10000;
+ to_ptr = &to;
+ }
+
+ if( !rv )
+ rv = bytes;
+
+ return rv;
+}
+
+
+/**************************************************************************************
+ * Description: Set the comport databit,parity,stopbit,flowctrl into the comport structure
+ * Input Args: comport: the comport_t pointer
+ * settings: The databit/parity/stopbit/flowctrl settings as like "8N1N"
+ * Output Args: NONE
+ * Return Value: NONE
+ *************************************************************************************/
+static inline void set_settings(comport_t * comport, const char *settings)
+{
+ if( !settings || !comport )
+ {
+ dbg_print("invalid input arugments\n");
+ return ;
+ }
+
+ switch (settings[0]) /* data bit */
+ {
+ case '7':
+ comport->databit = 7;
+ break;
+ case '8':
+ default:
+ comport->databit = 8;
+ break;
+ }
+
+ switch (settings[1]) /* parity */
+ {
+ case 'O':
+ case 'o':
+ comport->parity = 1;
+ break;
+ case 'E':
+ case 'e':
+ comport->parity = 2;
+ break;
+ case 'S':
+ case 's':
+ comport->parity = 3;
+ break;
+ case 'N':
+ case 'n':
+ default:
+ comport->parity = 0;
+ break;
+ }
+
+ switch (settings[2]) /* stop bit */
+ {
+ case '0':
+ comport->stopbit = 0;
+ break;
+ case '1':
+ default:
+ comport->stopbit = 1;
+ break;
+ }
+
+ switch (settings[3]) /* flow control */
+ {
+ case 'S':
+ case 's':
+ comport->flowctrl = 1;
+ break;
+ case 'H':
+ case 'h':
+ comport->flowctrl = 2;
+ break;
+ case 'B':
+ case 'b':
+ comport->flowctrl = 3;
+ break;
+ case 'N':
+ case 'n':
+ default:
+ comport->flowctrl = 0;
+ break;
+ }
+}
+
--
Gitblit v1.9.1