Add TCP/IP support to busdevice.cfeature/tcpip

4 files changed, 376 insertions, 7 deletions

diff --git a/SimpleMotionV2.pri b/SimpleMotionV2.pri
index b5619bc..79bfc56 100644
--- a/SimpleMotionV2.pri
+++ b/SimpleMotionV2.pri
@@ -6,9 +6,9 @@ DEPENDPATH += $$PWD
 DEFINES += SIMPLEMOTIONV2_LIBRARY

 

 SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c $$PWD/pcserialport.c \

-    $$PWD/bufferedmotion.c

+    $$PWD/bufferedmotion.c $$PWD/tcpclient.c

 

 HEADERS += $$PWD/simplemotion_private.h\

     $$PWD/pcserialport.h $$PWD/busdevice.h  $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h \

-    $$PWD/bufferedmotion.h

+    $$PWD/bufferedmotion.h $$PWD/tcpclient.h

 

diff --git a/busdevice.c b/busdevice.c
index c4f89a1..6a89396 100644
--- a/busdevice.c
+++ b/busdevice.c
@@ -1,11 +1,17 @@
 #include "busdevice.h"
 
 #include "pcserialport.h"
+#include "tcpclient.h"
+
 #include <string.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <ctype.h>
 
 #define BD_NONE 0
 #define BD_RS 1
 #define BD_FTDI 2
+#define BD_TCP 3
 
 //how much bytes available in transmit buffer
 #define TANSMIT_BUFFER_LENGTH 128
@@ -46,6 +52,116 @@ void smBDinit()
 	bdInitialized=smtrue;
 }
 
+static 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;
+}
+
+static int parseIpAddress(const char *s, char *ip, size_t ipsize, short *port)
+{
+    const char *ip_end, *port_start;
+
+    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 < 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;
+}
+
 //ie "COM1" "VSD2USB"
 //return -1 if fails, otherwise handle number
 smbusdevicehandle smBDOpen( const char *devicename )
@@ -75,10 +191,24 @@ smbusdevicehandle smBDOpen( const char *devicename )
 		BusDevice[handle].bdType=BD_RS;
         BusDevice[handle].txBufferUsed=0;
 	}
-	else//no other bus types supproted yet
-	{
-		return -1;
-	}
+    else if (validateIpAddress(devicename, NULL, NULL) == 0)
+    {
+        char ip[128];
+        short port = 4001;
+        if (parseIpAddress(devicename, ip, sizeof(ip), &port) < 0)
+            return -1;
+        BusDevice[handle].comPort=OpenTCPPort( ip, port );
+        if( BusDevice[handle].comPort == -1 )
+        {
+            return -1; //failed to open
+        }
+        BusDevice[handle].bdType=BD_TCP;
+        BusDevice[handle].txBufferUsed=0;
+    }
+    else//no other bus types supproted yet
+    {
+        return -1;
+    }
 
 	//success
     BusDevice[handle].cumulativeSmStatus=0;
@@ -105,6 +235,12 @@ smbool smBDClose( const smbusdevicehandle handle )
 		BusDevice[handle].opened=smfalse;
 		return smtrue;
 	}
+    else if( BusDevice[handle].bdType==BD_TCP )
+    {
+        CloseTCPport( BusDevice[handle].comPort );
+        BusDevice[handle].opened=smfalse;
+        return smtrue;
+    }
 
 	return smfalse;
 }
@@ -119,7 +255,7 @@ smbool smBDWrite(const smbusdevicehandle handle, const smuint8 byte )
 	//check if handle valid & open
 	if( smIsBDHandleOpen(handle)==smfalse ) return smfalse;
 
-	if( BusDevice[handle].bdType==BD_RS )
+	if( BusDevice[handle].bdType==BD_RS || BusDevice[handle].bdType==BD_TCP )
 	{
         if(BusDevice[handle].txBufferUsed<TANSMIT_BUFFER_LENGTH)
         {
@@ -153,6 +289,20 @@ smbool smBDTransmit(const smbusdevicehandle handle)
             return smfalse;
         }
     }
+    else if( BusDevice[handle].bdType==BD_TCP )
+    {
+        if(SendTCPBuf(BusDevice[handle].comPort,BusDevice[handle].txBuffer, BusDevice[handle].txBufferUsed)==BusDevice[handle].txBufferUsed)
+        {
+            BusDevice[handle].txBufferUsed=0;
+            return smtrue;
+        }
+        else
+        {
+            BusDevice[handle].txBufferUsed=0;
+            return smfalse;
+        }
+    }
+
     return smfalse;
 }
 
@@ -170,6 +320,14 @@ smbool smBDRead( const smbusdevicehandle handle, smuint8 *byte )
 		if( n!=1 ) return smfalse;
 		else return smtrue;
 	}
+    else if( BusDevice[handle].bdType==BD_TCP )
+    {
+        int n;
+        n=PollTCPPort(BusDevice[handle].comPort, byte, 1);
+        if( n!=1 ) return smfalse;
+        else return smtrue;
+    }
+
 
 	return smfalse;
 }
diff --git a/tcpclient.c b/tcpclient.c
new file mode 100644
index 0000000..31fb2f2
--- /dev/null
+++ b/tcpclient.c
@@ -0,0 +1,189 @@
+#include "simplemotion_private.h"
+#include "tcpclient.h"
+#include <stdio.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;
+    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(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(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, buf, size);
+    return(n);
+}
+
+int SendTCPByte(int sockfd, unsigned char byte)
+{
+    int n;
+    n = write(sockfd, &byte, 1);
+    if(n<0)
+        return(1);
+    return(0);
+}
+
+
+int SendTCPBuf(int sockfd, unsigned char *buf, int size)
+{
+    int sent = write(sockfd, buf, size);
+    if (sent != size)
+    {
+        return sent;
+    }
+    return sent;
+}
+
+
+void CloseTCPport(int sockfd)
+{
+    close(sockfd);
+#if defined(_WIN32)
+    WSACleanup();
+#endif
+}
diff --git a/tcpclient.h b/tcpclient.h
new file mode 100644
index 0000000..ea02e42
--- /dev/null
+++ b/tcpclient.h
@@ -0,0 +1,22 @@
+#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);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif
+
+