Reorganized drivers (move them to drivers subfolder)

4 files changed, 708 insertions, 0 deletions

diff --git a/drivers/serial/pcserialport.c b/drivers/serial/pcserialport.c
new file mode 100644
index 0000000..8cbe281
--- /dev/null
+++ b/drivers/serial/pcserialport.c
@@ -0,0 +1,337 @@
+/*
+ * pcserialport.h
+ *
+ * Header for PC serial port access library (win/linux)
+ *
+ *  Created on: 28.12.2016
+ *      Author: Tero
+ *
+ *  Inspired by RS232 library by Teunis van Beelen
+ */
+
+
+
+#include "pcserialport.h"
+#include "simplemotion_private.h" //needed for timeout variable
+
+#if defined(__unix__) || defined(__APPLE__)
+
+#include <termios.h>
+#include <limits.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <string.h>
+
+#if defined(__linux__)
+//needed for setting low latency
+#include <linux/serial.h>
+#endif
+
+#if defined(__APPLE__)
+#include <CoreFoundation/CoreFoundation.h>
+#include <IOKit/IOKitLib.h>
+#include <IOKit/serial/IOSerialKeys.h>
+#include <IOKit/serial/ioss.h>
+#include <IOKit/IOBSD.h>
+#endif
+
+smint32 serialPortOpen(const char * port_device_name, smint32 baudrate_bps)
+{
+    int port_handle;
+    int err;
+    int baudrateEnumValue;
+    struct termios new_port_settings;
+    int customBaudRate = 0;
+
+    port_handle = open(port_device_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+    if(port_handle==-1)
+    {
+        smDebug(-1, Low, "Serial port error: port open failed");
+        return -1;
+    }
+
+
+    // open() follows POSIX semantics: multiple open() calls to the same file will succeed
+    // unless the TIOCEXCL ioctl is issued (except for root)
+    if (ioctl(port_handle, TIOCEXCL) == -1) {
+        smDebug(-1, Low, "Serial port error: error setting TIOCEXCL");
+        close(port_handle);
+        return -1;
+    }
+
+    // as the port is now open, clear O_NONBLOCK flag for subsequent I/O calls
+    if (fcntl(port_handle, F_SETFL, 0) == -1) {
+        smDebug(-1, Low, "Serial port error: error clearing O_NONBLOCK");
+        close(port_handle);
+        return -1;
+    }
+
+    switch(baudrate_bps)
+    {
+#if defined(B9600)
+        case    9600 : baudrateEnumValue = B9600; break;
+#endif
+#if defined(B19200)
+        case   19200 : baudrateEnumValue = B19200; break;
+#endif
+#if defined(B38400)
+        case   38400 : baudrateEnumValue = B38400; break;
+#endif
+#if defined(B57600)
+        case   57600 : baudrateEnumValue = B57600; break;
+#endif
+#if defined(B115200)
+        case  115200 : baudrateEnumValue = B115200; break;
+#endif
+#if defined(B230400)
+        case  230400 : baudrateEnumValue = B230400; break;
+#endif
+#if defined(B460800)
+        case  460800 : baudrateEnumValue = B460800; break;
+#endif
+#if defined(B500000)
+        case  500000 : baudrateEnumValue = B500000; break;
+#endif
+#if defined(B576000)
+        case  576000 : baudrateEnumValue = B576000; break;
+#endif
+#if defined(B921600)
+        case  921600 : baudrateEnumValue = B921600; break;
+#endif
+#if defined(B1000000)
+        case 1000000 : baudrateEnumValue = B1000000; break;
+#endif
+#if defined(B1152000)
+        case 1115200 : baudrateEnumValue = B1152000; break;
+#endif
+#if defined(B1500000)
+        case 1500000 : baudrateEnumValue = B1500000; break;
+#endif
+#if defined(B2000000)
+        case 2000000 : baudrateEnumValue = B2000000; break;
+#endif
+#if defined(B2500000)
+        case 2500000 : baudrateEnumValue = B2500000; break;
+#endif
+#if defined(B3000000)
+        case 3000000 : baudrateEnumValue = B3000000; break;
+#endif
+#if defined(B3500000)
+        case 3500000 : baudrateEnumValue = B3500000; break;
+#endif
+#if defined(B4000000)
+        case 4000000 : baudrateEnumValue = B4000000; break;
+#endif
+        default:
+            customBaudRate = 1;
+            baudrateEnumValue=B9600;//must set something initially, changed later
+        break;
+    }
+
+    memset(&new_port_settings, 0, sizeof(new_port_settings)); //reset struct
+    cfmakeraw(&new_port_settings);//reset struct
+    new_port_settings.c_cflag = CS8 | CLOCAL | CREAD;
+    new_port_settings.c_iflag = IGNPAR;
+    new_port_settings.c_oflag = 0;
+    new_port_settings.c_lflag = 0;
+    new_port_settings.c_cc[VMIN] = 0;      /* non blocking mode */
+    new_port_settings.c_cc[VTIME] = readTimeoutMs/100;     /* timeout 100 ms steps */
+
+#if defined(_BSD_SOURCE)
+    cfsetspeed(&new_port_settings, baudrateEnumValue);
+#else
+    cfsetispeed(&new_port_settings, baudrateEnumValue);
+    cfsetospeed(&new_port_settings, baudrateEnumValue);
+#endif
+
+    // Activate settings
+    err = tcsetattr(port_handle, TCSANOW, &new_port_settings);
+    if(err==-1)
+    {
+        close(port_handle);
+        smDebug(-1, Low, "Serial port error: failed to set port parameters");
+        return -1;
+    }
+
+    if(customBaudRate)
+    {
+        #if defined(IOSSIOSPEED)
+        speed_t bps = baudrate_bps;
+        if (ioctl(port_handle, IOSSIOSPEED, &bps) == -1)
+        {
+            smDebug(-1, Low, "Serial port error: unsupported baudrate\n");
+            close(port_handle);
+            return -1;
+        }
+        #else
+        smDebug(-1, Low, "Serial port error: unsupported baudrate\n");
+        close(port_handle);
+        return -1;
+        #endif
+    }
+
+    // set receive latency to 1 ms
+    #if defined(IOSSDATALAT)
+    unsigned long microsecs = 1000UL;
+    if (ioctl(port_handle, IOSSDATALAT, &microsecs) == -1) {
+        smDebug(-1, Low, "Serial port error: error setting read latency");
+        close(port_handle);
+        return -1;
+    }
+    #endif
+    
+    #if defined(TIOCGSERIAL) && defined(ASYNC_LOW_LATENCY)
+    struct serial_struct serial; 
+    if(ioctl(port_handle, TIOCGSERIAL, &serial)!=-1)
+    {
+        serial.flags |= ASYNC_LOW_LATENCY;
+        if(ioctl(port_handle, TIOCSSERIAL, &serial) == -1 )
+        {
+            smDebug(-1, Low, "Serial port warning: unable to set low latency mode, maybe try running with root permissions.");
+        }
+    }
+    else
+        smDebug(-1, Low, "Serial port warning: unable to read TIOCGSERIAL for low latency mode, maybe try running with root permissions.");
+    #endif
+
+    //flush any stray bytes from device receive buffer that may reside in it
+    //note: according to following page, delay before this may be necessary http://stackoverflow.com/questions/13013387/clearing-the-serial-ports-buffer
+    usleep(100000);
+    tcflush(port_handle,TCIOFLUSH);
+
+    return port_handle;
+}
+
+
+smint32 serialPortRead(smint32 serialport_handle, smuint8 *buf, smint32 size)
+{
+    smint32 n;
+    if(size>4096)  size = 4096;
+    n = read(serialport_handle, buf, size);
+    return n;
+}
+
+
+smint32 serialPortWrite(smint32 serialport_handle, unsigned char byte)
+{
+    smint32 n;
+    n = write(serialport_handle, &byte, 1);
+    if(n<0)
+        return 1;
+    return 0;
+}
+
+
+smint32 serialPortWriteBuffer(smint32 serialport_handle, unsigned char *buf, smint32 size)
+{
+        return(write(serialport_handle, buf, size));
+}
+
+
+void serialPortClose(smint32 serialport_handle)
+{
+        close(serialport_handle);
+}
+
+#else   //windows: for API, see https://msdn.microsoft.com/en-us/library/ff802693.aspx
+
+#include <windows.h>
+#include <string.h>
+
+smint32 serialPortOpen(const char *port_device_name, smint32 baudrate_bps)
+{
+    char port_def_string[64], port_name[32];
+    HANDLE port_handle;
+
+    sprintf(port_def_string,"baud=%d data=8 parity=N stop=1", (int)baudrate_bps);
+    sprintf(port_name,"\\\\.\\%s",port_device_name);
+
+    port_handle = CreateFileA(port_name,GENERIC_READ|GENERIC_WRITE,0,NULL,OPEN_EXISTING,0,NULL);
+
+    if(port_handle==INVALID_HANDLE_VALUE)
+    {
+        smDebug( -1, Low, "Serial port error: Unable to create serial port handle");
+        return -1;
+    }
+
+    //fill DCB settings struct
+    DCB dcb;
+    FillMemory(&dcb, sizeof(dcb), 0);
+    dcb.DCBlength = sizeof(dcb);
+
+    if(!BuildCommDCBA(port_def_string, &dcb))
+    {
+        smDebug( -1, Low, "Serial port error: Unable to build DCB settings\n");
+        CloseHandle(port_handle);
+        return -1;
+    }
+
+    if(!SetCommState(port_handle, &dcb))
+    {
+        smDebug( -1, Low, "Serial port error: Unable to set port settings\n");
+        CloseHandle(port_handle);
+        return -1;
+    }
+
+    //set timeout
+    COMMTIMEOUTS port_timeouts;
+    port_timeouts.ReadTotalTimeoutConstant    = readTimeoutMs;
+    port_timeouts.ReadIntervalTimeout         = 0;
+    port_timeouts.ReadTotalTimeoutMultiplier  = 0;
+    port_timeouts.WriteTotalTimeoutMultiplier = 50;
+    port_timeouts.WriteTotalTimeoutConstant   = 50;
+
+    if(!SetCommTimeouts(port_handle, &port_timeouts))
+    {
+        smDebug( -1, Low, "Serial port error: Failed to set port timeout settings\n");
+        CloseHandle(port_handle);
+        return(-1);
+    }
+
+    //flush any stray bytes from device receive buffer that may reside in it
+    PurgeComm((HANDLE)port_handle,PURGE_RXABORT|PURGE_RXCLEAR|PURGE_TXABORT|PURGE_TXCLEAR);
+
+    return( (smint32)port_handle);
+}
+
+
+smint32 serialPortRead(smint32 serialport_handle, unsigned char *buf, smint32 size)
+{
+    smint32 n;
+    if(size>4096)
+        size = 4096;
+    ReadFile((HANDLE)serialport_handle, buf, size, (LPDWORD)((void *)&n), NULL);
+    return n;
+}
+
+
+smint32 serialPortWriteByte(smint32 serialport_handle, unsigned char byte)
+{
+    smint32 n;
+    WriteFile((HANDLE)serialport_handle, &byte, 1, (LPDWORD)((void *)&n), NULL);
+    if(n<0)
+        return 1;
+    return 0;
+}
+
+
+smint32 serialPortWriteBuffer(smint32 serialport_handle, unsigned char *buf, smint32 size)
+{
+    smint32 n;
+    if(WriteFile((HANDLE)serialport_handle, buf, size, (LPDWORD)((void *)&n), NULL))
+        return n;
+    return -1;
+}
+
+
+void serialPortClose(smint32 serialport_number)
+{
+    CloseHandle((HANDLE)serialport_number);
+}
+
+
+#endif//windows
diff --git a/drivers/serial/pcserialport.h b/drivers/serial/pcserialport.h
new file mode 100644
index 0000000..832786d
--- /dev/null
+++ b/drivers/serial/pcserialport.h
@@ -0,0 +1,40 @@
+/*
+ * pcserialport.h
+ *
+ * Header for PC serial port access library (win/linux)
+ *
+ *  Created on: 28.12.2016
+ *      Author: Tero
+ *
+ *  Inspired by RS232 library by Teunis van Beelen
+ */
+
+
+/*  Todo:
+  -Restore port settings at CloseComport
+  */
+
+#ifndef PCSERAILPORT_H
+#define PCSERAILPORT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "simplemotion.h"
+
+//return port handle or -1 if fails
+smint32 serialPortOpen(const char *port_device_name, smint32 baudrate_bps);
+smint32 serialPortRead(smint32 serialport_handle, unsigned char *buf, smint32 size);
+smint32 serialPortWriteByte(smint32 serialport_handle, unsigned char byte);
+smint32 serialPortWriteBuffer(smint32 serialport_handle, unsigned char *buf, smint32 size);
+void serialPortClose(smint32 serialport_number);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+
diff --git a/drivers/tcpip/tcpclient.c b/drivers/tcpip/tcpclient.c
new file mode 100644
index 0000000..458b77a
--- /dev/null
+++ b/drivers/tcpip/tcpclient.c
@@ -0,0 +1,303 @@
+#include "simplemotion_private.h"
+#include "tcpclient.h"
+#include <stdio.h>
+#include <ctype.h>
+
+#if defined(_WIN32)
+#if defined(CM_NONE)
+#undef CM_NONE
+#endif
+#include <winsock2.h>
+#include <ws2tcpip.h>
+#define close closesocket
+#define read(SOCKET, BUF, LEN) recv((SOCKET), (BUF), (LEN), 0)
+#define write(SOCKET, BUF, LEN) send((SOCKET), (BUF), (LEN), 0)
+#ifdef errno
+#undef errno
+#endif
+#define errno (WSAGetLastError())
+#ifdef EINPROGRESS
+#undef EINPROGRESS
+#endif
+#define EINPROGRESS WSAEWOULDBLOCK
+#else
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <sys/time.h>
+#include <netdb.h>
+#include <netinet/tcp.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#endif
+
+#if defined(_WIN32)
+static int initwsa()
+{
+    WORD req;
+    WSADATA data;
+    req = MAKEWORD(2, 2);
+    int err = WSAStartup(req, &data);
+    if (err != 0)
+    {
+        printf("WSAStartup failed\n");
+        return 0;
+    }
+    return 1;
+}
+#endif
+
+int OpenTCPPort(const char * ip_addr, int port)
+{
+    int sockfd;
+    struct sockaddr_in server;
+    struct timeval tv;
+    fd_set myset;
+    int res, valopt;
+    socklen_t lon;
+    unsigned long arg;
+
+#if defined(_WIN32)
+    initwsa();
+#endif
+
+    //Create socket
+    sockfd = socket(AF_INET , SOCK_STREAM , IPPROTO_TCP);
+    if (sockfd == -1)
+    {
+        return -1;
+    }
+
+    // Set OFF NAGLE algorithm to reduce latency with small packets
+    /*
+    int one = 1;
+    setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (void *)&one, sizeof(one));
+    */
+
+    server.sin_addr.s_addr = inet_addr(ip_addr);
+    server.sin_family = AF_INET;
+    server.sin_port = htons(port);
+
+    // Set non-blocking when trying to establish the connection
+#if !defined(_WIN32)
+    arg = fcntl(sockfd, F_GETFL, NULL);
+    arg |= O_NONBLOCK;
+    fcntl(sockfd, F_SETFL, arg);
+#else
+    arg = 1;
+    ioctlsocket(sockfd, FIONBIO, &arg);
+#endif
+
+    res = connect(sockfd, (struct sockaddr *)&server, sizeof(server));
+
+    if (res < 0)
+    {
+        if (errno == EINPROGRESS)
+        {
+            tv.tv_sec = 5;
+            tv.tv_usec = 0;
+            FD_ZERO(&myset);
+            FD_SET((unsigned int)sockfd, &myset);
+            if (select(sockfd+1, NULL, &myset, NULL, &tv) > 0)
+            {
+                lon = sizeof(int);
+                getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void*)(&valopt), &lon);
+                if (valopt)
+                {
+                    return -1;
+                }
+            }
+            else
+            {
+               return -1;
+            }
+        }
+        else
+        {
+            return -1;
+        }
+    }
+
+    // Set to blocking mode again
+#if !defined(_WIN32)
+    arg = fcntl(sockfd, F_GETFL, NULL);
+    arg &= (~O_NONBLOCK);
+    fcntl(sockfd, F_SETFL, arg);
+#else
+    arg = 0;
+    ioctlsocket(sockfd, FIONBIO, &arg);
+#endif
+
+    return sockfd;
+}
+
+// Read bytes from socket
+int PollTCPPort(int sockfd, unsigned char *buf, int size)
+{
+    int n;
+    fd_set input;
+    FD_ZERO(&input);
+    FD_SET((unsigned int)sockfd, &input);
+    struct timeval timeout;
+    timeout.tv_sec = 0;
+    timeout.tv_usec = readTimeoutMs * 1000;
+
+    n = select(sockfd + 1, &input, NULL, NULL, &timeout);
+
+    // Error or timeout
+    if (n < 1)
+    {
+        return(-1);
+    }
+    if(!FD_ISSET(sockfd, &input))
+    {
+        return(-1);
+    }
+
+    n = read(sockfd, (char*)buf, size);
+    return(n);
+}
+
+int SendTCPByte(int sockfd, unsigned char byte)
+{
+    int n;
+    n = write(sockfd, (char*)&byte, 1);
+    if(n<0)
+        return(1);
+    return(0);
+}
+
+
+int SendTCPBuf(int sockfd, unsigned char *buf, int size)
+{
+    int sent = write(sockfd, (char*)buf, size);
+    if (sent != size)
+    {
+        return sent;
+    }
+    return sent;
+}
+
+
+void CloseTCPport(int sockfd)
+{
+    close(sockfd);
+#if defined(_WIN32)
+    WSACleanup();
+#endif
+}
+
+
+//accepted TCP/IP address format is nnn.nnn.nnn.nnn:pppp where n is IP address numbers and p is port number
+int validateIpAddress(const char *s, const char **pip_end,
+                             const char **pport_start)
+{
+    int octets = 0;
+    int ch = 0, prev = 0;
+    int len = 0;
+    const char *ip_end = NULL;
+    const char *port_start = NULL;
+
+    while (*s)
+    {
+        ch = *s;
+
+        if (isdigit(ch))
+        {
+            ++len;
+            // Octet len must be 1-3 digits
+            if (len > 3)
+            {
+                return -1;
+            }
+        }
+        else if (ch == '.' && isdigit(prev))
+        {
+            ++octets;
+            len = 0;
+            // No more than 4 octets please
+            if (octets > 4)
+            {
+                return -1;
+            }
+        }
+        else if (ch == ':' && isdigit(prev))
+        {
+            ++octets;
+            // We want exactly 4 octets at this point
+            if (octets != 4)
+            {
+                return -1;
+            }
+            ip_end = s;
+            ++s;
+            port_start = s;
+            while (isdigit((ch = *s)))
+                ++s;
+            // After port we want the end of the string
+            if (ch != '\0')
+                return -1;
+            // This will skip over the ++s below
+            continue;
+        }
+        else
+        {
+            return -1;
+        }
+
+        prev = ch;
+        ++s;
+    }
+
+    // We reached the end of the string and did not encounter the port
+    if (*s == '\0' && ip_end == NULL)
+    {
+        ++octets;
+        ip_end = s;
+    }
+
+    // Check that there are exactly 4 octets
+    if (octets != 4)
+        return -1;
+
+    if (pip_end)
+        *pip_end = ip_end;
+
+    if (pport_start)
+        *pport_start = port_start;
+
+    return 0;
+}
+
+int parseIpAddress(const char *s, char *ip, size_t ipsize, short *port)
+{
+    const char *ip_end, *port_start;
+
+    //ip_end and port_start are pointers to memory area of s, not offsets or indexes to s
+    if (validateIpAddress(s, &ip_end, &port_start) == -1)
+        return -1;
+
+    // If ip=NULL, we just report that the parsing was ok
+    if (!ip)
+        return 0;
+
+    if (ipsize < (size_t)(ip_end - s + 1))
+        return -1;
+
+    memcpy(ip, s, ip_end - s);
+    ip[ip_end - s] = '\0';
+
+    if (port_start)
+    {
+        *port = 0;
+        while (*port_start)
+        {
+            *port = *port * 10 + (*port_start - '0');
+            ++port_start;
+        }
+    }
+
+    return 0;
+}
diff --git a/drivers/tcpip/tcpclient.h b/drivers/tcpip/tcpclient.h
new file mode 100644
index 0000000..5ed075a
--- /dev/null
+++ b/drivers/tcpip/tcpclient.h
@@ -0,0 +1,28 @@
+#ifndef tcpclient_INCLUDED
+#define tcpclient_INCLUDED
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//return port handle or -1 if fails
+int OpenTCPPort(const char * ip_addr, int port);
+int PollTCPPort(int, unsigned char *, int);
+int SendTCPByte(int, unsigned char);
+int SendTCPBuf(int, unsigned char *, int);
+void CloseTCPport(int);
+
+
+//accepted TCP/IP address format is nnn.nnn.nnn.nnn:pppp where n is IP address numbers and p is port number
+int validateIpAddress(const char *s, const char **pip_end,
+                             const char **pport_start);
+int parseIpAddress(const char *s, char *ip, size_t ipsize, short *port);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif
+
+