Implemented code for Slamtec rplidar C1

7 files changed, 1818 insertions, 0 deletions

diff --git a/LIDAR/sdk/src/arch/linux/arch_linux.h b/LIDAR/sdk/src/arch/linux/arch_linux.h
new file mode 100644
index 0000000..ce31343
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/arch_linux.h
@@ -0,0 +1,64 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+// libc dep
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <math.h>
+#include <time.h>
+#include <stdarg.h>
+
+// libc++ dep
+#include <iostream>
+#include <string>
+
+// linux specific
+#include <unistd.h>
+#include <errno.h>
+#include <pthread.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <sys/select.h>
+#include <time.h>
+
+#include "timer.h"
+
diff --git a/LIDAR/sdk/src/arch/linux/net_serial.cpp b/LIDAR/sdk/src/arch/linux/net_serial.cpp
new file mode 100644
index 0000000..2bec556
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/net_serial.cpp
@@ -0,0 +1,475 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <assert.h>
+// linux specific
+
+#include <errno.h>
+#include <fcntl.h>
+
+#include <time.h>
+#include "hal/types.h"
+#include "arch/linux/net_serial.h"
+#include <sys/select.h>
+
+#include <algorithm>
+//__GNUC__
+#if defined(__GNUC__)
+// for Linux extension
+#include <asm/ioctls.h>
+#include <asm/termbits.h>
+#include <sys/ioctl.h>
+extern "C" int tcflush(int fildes, int queue_selector);
+#else
+// for other standard UNIX
+#include <termios.h>
+#include <sys/ioctl.h>
+
+#endif
+
+
+namespace rp{ namespace arch{ namespace net{
+
+raw_serial::raw_serial()
+    : rp::hal::serial_rxtx()
+    , _baudrate(0)
+    , _flags(0)
+    , serial_fd(-1)
+{
+    _init();
+}
+
+raw_serial::~raw_serial()
+{
+    close();
+
+}
+
+bool raw_serial::open()
+{
+    return open(_portName, _baudrate, _flags);
+}
+
+bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
+{   
+    strncpy(_portName, portname, sizeof(_portName));
+    _baudrate = baudrate;
+    _flags    = flags;
+    return true;
+}
+
+bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
+{
+    if (isOpened()) close();
+    
+    serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
+
+    if (serial_fd == -1) return false;
+
+    
+
+#if !defined(__GNUC__)
+    // for standard UNIX
+    struct termios options, oldopt;
+    tcgetattr(serial_fd, &oldopt);
+    bzero(&options,sizeof(struct termios));
+
+    // enable rx and tx
+    options.c_cflag |= (CLOCAL | CREAD);
+
+    _u32 termbaud = getTermBaudBitmap(baudrate);
+
+    if (termbaud == (_u32)-1) {
+        close();
+        return false;
+    }
+    cfsetispeed(&options, termbaud);
+    cfsetospeed(&options, termbaud);
+
+    options.c_cflag &= ~PARENB; //no checkbit
+    options.c_cflag &= ~CSTOPB; //1bit stop bit
+    options.c_cflag &= ~CRTSCTS; //no flow control
+
+    options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8; /* Select 8 data bits */
+
+#ifdef CNEW_RTSCTS
+    options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
+#endif
+
+    options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
+
+    // raw input mode   
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    // raw output mode   
+    options.c_oflag &= ~OPOST;
+
+
+
+    if (tcsetattr(serial_fd, TCSANOW, &options))
+    {
+        close();
+        return false;
+    }
+
+#else
+
+    // using Linux extension ...
+    struct termios2 tio;
+
+    ioctl(serial_fd, TCGETS2, &tio);
+    bzero(&tio, sizeof(struct termios2));
+
+    tio.c_cflag = BOTHER;
+    tio.c_cflag |= (CLOCAL | CREAD | CS8); //8 bit no hardware handshake
+
+    tio.c_cflag &= ~CSTOPB;   //1 stop bit
+    tio.c_cflag &= ~CRTSCTS;  //No CTS
+    tio.c_cflag &= ~PARENB;   //No Parity
+
+#ifdef CNEW_RTSCTS
+    tio.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
+#endif
+
+    tio.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
+
+
+    tio.c_cc[VMIN] = 0;         //min chars to read
+    tio.c_cc[VTIME] = 0;        //time in 1/10th sec wait
+
+    tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    // raw output mode   
+    tio.c_oflag &= ~OPOST;
+
+    tio.c_ispeed = baudrate;
+    tio.c_ospeed = baudrate;
+
+
+    ioctl(serial_fd, TCSETS2, &tio);
+
+#endif
+
+
+    tcflush(serial_fd, TCIFLUSH);
+
+    if (fcntl(serial_fd, F_SETFL, FNDELAY))
+    {
+        close();
+        return false;
+    }
+
+
+    _is_serial_opened = true;
+    _operation_aborted = false;
+
+    //Clear the DTR bit to let the motor spin
+    clearDTR();
+    do {
+        // create self pipeline for wait cancellation
+        if (pipe(_selfpipe) == -1) break;
+
+        int flags = fcntl(_selfpipe[0], F_GETFL);
+        if (flags == -1)
+            break;
+
+        flags |= O_NONBLOCK;                /* Make read end nonblocking */
+        if (fcntl(_selfpipe[0], F_SETFL, flags) == -1)
+            break;
+
+        flags = fcntl(_selfpipe[1], F_GETFL);
+        if (flags == -1)
+            break;
+
+        flags |= O_NONBLOCK;                /* Make write end nonblocking */
+        if (fcntl(_selfpipe[1], F_SETFL, flags) == -1)
+            break;
+
+    } while (0);
+    
+    return true;
+}
+
+void raw_serial::close()
+{
+    if (serial_fd != -1)
+        ::close(serial_fd);
+    serial_fd = -1;
+    
+    if (_selfpipe[0] != -1)
+        ::close(_selfpipe[0]);
+
+    if (_selfpipe[1] != -1)
+        ::close(_selfpipe[1]);
+
+    _selfpipe[0] = _selfpipe[1] = -1;
+
+    _operation_aborted = false;
+    _is_serial_opened = false;
+}
+
+int raw_serial::senddata(const unsigned char * data, size_t size)
+{
+// FIXME: non-block io should be used
+    if (!isOpened()) return 0;
+
+    if (data == NULL || size ==0) return 0;
+    
+    size_t tx_len = 0;
+    required_tx_cnt = 0;
+    do {
+        int ans = ::write(serial_fd, data + tx_len, size-tx_len);
+        
+        if (ans == -1) return tx_len;
+        
+        tx_len += ans;
+        required_tx_cnt = tx_len;
+    }while (tx_len<size);
+    
+    
+    return tx_len;
+}
+
+
+int raw_serial::recvdata(unsigned char * data, size_t size)
+{
+    if (!isOpened()) return 0;
+    
+    int ans = ::read(serial_fd, data, size);
+    
+    if (ans == -1) ans=0;
+    required_rx_cnt = ans;
+    return ans;
+}
+
+
+void raw_serial::flush( _u32 flags)
+{
+    tcflush(serial_fd,TCIFLUSH); 
+}
+
+int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
+{
+    if (returned_size) *returned_size = required_tx_cnt;
+    return 0;
+}
+
+int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
+{
+    if (!isOpened() ) return -1;
+   
+    if (returned_size) *returned_size = required_rx_cnt;
+    return 0;
+}
+
+int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
+{
+    size_t length = 0;
+    if (returned_size==NULL) returned_size=(size_t *)&length;
+    *returned_size = 0;
+
+    int max_fd;
+    fd_set input_set;
+    struct timeval timeout_val;
+
+    /* Initialize the input set */
+    FD_ZERO(&input_set);
+    FD_SET(serial_fd, &input_set);
+
+    if (_selfpipe[0] != -1)
+        FD_SET(_selfpipe[0], &input_set);
+
+    max_fd =  std::max<int>(serial_fd, _selfpipe[0]) + 1;
+
+    /* Initialize the timeout structure */
+    timeout_val.tv_sec = timeout / 1000;
+    timeout_val.tv_usec = (timeout % 1000) * 1000;
+
+    if ( isOpened() )
+    {
+        int nread;
+
+        if ( ioctl(serial_fd, FIONREAD, &nread) == -1) return ANS_DEV_ERR;
+
+        *returned_size = nread;
+
+        if (*returned_size >= data_count)
+        {
+            return 0;
+        }
+    }
+
+    while ( isOpened() )
+    {
+        /* Do the select */
+        int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
+
+        if (n < 0)
+        {
+            // select error
+            *returned_size =  0;
+            return ANS_DEV_ERR;
+        }
+        else if (n == 0)
+        {
+            // time out
+            *returned_size =0;
+            return ANS_TIMEOUT;
+        }
+        else
+        {
+            if (FD_ISSET(_selfpipe[0], &input_set)) {   
+                // require aborting the current operation
+                int ch;
+                for (;;) {                    
+                    if (::read(_selfpipe[0], &ch, 1) == -1) {
+                        break;
+                    }
+                    
+                }
+
+                // treat as  timeout
+                *returned_size = 0;
+                return ANS_TIMEOUT;
+            }
+
+            // data avaliable
+            assert (FD_ISSET(serial_fd, &input_set));
+
+
+            if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
+            if (*returned_size >= data_count)
+            {
+                return 0;
+            }
+        }
+        
+    }
+
+    *returned_size=0;
+    return ANS_DEV_ERR;
+}
+
+size_t raw_serial::rxqueue_count()
+{
+    if  ( !isOpened() ) return 0;
+    size_t remaining;
+    
+    if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
+    return remaining;
+}
+
+void raw_serial::setDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIS, &dtr_bit);
+}
+
+void raw_serial::clearDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIC, &dtr_bit);
+}
+
+void raw_serial::_init()
+{
+    serial_fd = -1;  
+    _portName[0] = 0;
+    required_tx_cnt = required_rx_cnt = 0;
+    _operation_aborted = false;
+    _selfpipe[0] = _selfpipe[1] = -1;
+}
+
+void raw_serial::cancelOperation()
+{
+    _operation_aborted = true;
+    if (_selfpipe[1] == -1) return;
+
+    (int)::write(_selfpipe[1], "x", 1);
+}
+
+_u32 raw_serial::getTermBaudBitmap(_u32 baud)
+{
+#define BAUD_CONV( _baud_) case _baud_:  return B##_baud_ 
+switch (baud) {
+        BAUD_CONV(1200);
+        BAUD_CONV(1800);
+        BAUD_CONV(2400);
+        BAUD_CONV(4800);
+        BAUD_CONV(9600);
+        BAUD_CONV(19200);
+        BAUD_CONV(38400);
+        BAUD_CONV(57600);
+        BAUD_CONV(115200);
+        BAUD_CONV(230400);
+        BAUD_CONV(460800);
+        BAUD_CONV(500000);
+        BAUD_CONV(576000);
+        BAUD_CONV(921600);
+        BAUD_CONV(1000000);
+        BAUD_CONV(1152000);
+        BAUD_CONV(1500000);
+        BAUD_CONV(2000000);
+        BAUD_CONV(2500000);
+        BAUD_CONV(3000000);
+        BAUD_CONV(3500000);
+        BAUD_CONV(4000000);
+    }
+    return -1;
+}
+
+}}} //end rp::arch::net
+
+//begin rp::hal
+namespace rp{ namespace hal{
+
+serial_rxtx * serial_rxtx::CreateRxTx()
+{
+    return new rp::arch::net::raw_serial();
+}
+
+void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
+{
+    delete rxtx;
+}
+
+}} //end rp::hal
diff --git a/LIDAR/sdk/src/arch/linux/net_serial.h b/LIDAR/sdk/src/arch/linux/net_serial.h
new file mode 100644
index 0000000..627369a
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/net_serial.h
@@ -0,0 +1,90 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "hal/abs_rxtx.h"
+
+namespace rp{ namespace arch{ namespace net{
+
+class raw_serial : public rp::hal::serial_rxtx
+{
+public:
+    enum{
+        SERIAL_RX_BUFFER_SIZE = 512,
+        SERIAL_TX_BUFFER_SIZE = 128,
+    };
+
+    raw_serial();
+    virtual ~raw_serial();
+    virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    virtual bool open();
+    virtual void close();
+    virtual void flush( _u32 flags);
+    
+    virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual int senddata(const unsigned char * data, size_t size);
+    virtual int recvdata(unsigned char * data, size_t size);
+
+    virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL);
+    virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual size_t rxqueue_count();
+
+    virtual void setDTR();
+    virtual void clearDTR();
+
+    _u32 getTermBaudBitmap(_u32 baud);
+
+    virtual void cancelOperation();
+
+protected:
+    bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    void _init();
+
+    char _portName[200];
+    uint32_t _baudrate;
+    uint32_t _flags;
+
+    int serial_fd;
+
+    size_t required_tx_cnt;
+    size_t required_rx_cnt;
+
+    int    _selfpipe[2];
+    bool   _operation_aborted;
+};
+
+}}}
diff --git a/LIDAR/sdk/src/arch/linux/net_socket.cpp b/LIDAR/sdk/src/arch/linux/net_socket.cpp
new file mode 100644
index 0000000..99eb2dc
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/net_socket.cpp
@@ -0,0 +1,893 @@
+/*
+ *  RoboPeak Project
+ *  HAL Layer - Socket Interface
+ *  Copyright 2009 - 2013 RoboPeak Project
+ *
+ *  POXIS Implementation
+ */
+
+
+#include "sdkcommon.h"
+#include "../../hal/socket.h"
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+
+
+namespace rp{ namespace net {
+
+
+static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) 
+{
+    switch (type) {
+        case SocketAddress::ADDRESS_TYPE_INET:
+            return AF_INET;
+        case SocketAddress::ADDRESS_TYPE_INET6:
+            return AF_INET6;
+        case SocketAddress::ADDRESS_TYPE_UNSPEC:
+            return AF_UNSPEC;
+
+        default:
+            assert(!"should not reach here");
+            return AF_UNSPEC;
+    }
+}
+
+
+SocketAddress::SocketAddress() 
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+}
+
+SocketAddress::SocketAddress(const SocketAddress & src)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+}
+
+
+
+SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    // default to ipv4 in case the following operation fails
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+
+    setAddressFromString(addrString, type);
+    setPort(port);
+}
+
+SocketAddress::SocketAddress(void * platform_data)
+ : _platform_data(platform_data)
+{}
+
+SocketAddress & SocketAddress::operator = (const SocketAddress &src)
+{
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+    return *this;
+}
+
+
+SocketAddress::~SocketAddress()
+{
+    delete reinterpret_cast<sockaddr_storage *>(_platform_data);
+}
+
+SocketAddress::address_type_t SocketAddress::getAddressType() const
+{
+    switch(reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family) {
+        case AF_INET:
+            return ADDRESS_TYPE_INET;
+        case AF_INET6:
+            return ADDRESS_TYPE_INET6;
+        default:
+            assert(!"should not reach here");
+            return ADDRESS_TYPE_INET;
+    }
+}
+
+int SocketAddress::getPort() const
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_port);
+        case ADDRESS_TYPE_INET6:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
+        default:
+            return 0;
+    }
+}
+
+u_result SocketAddress::setPort(int port)
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
+            break;
+        case ADDRESS_TYPE_INET6:
+            reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_port = htons((short)port);
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+u_result SocketAddress::setAddressFromString(const char * address_string,  SocketAddress::address_type_t type)
+{
+    int ans = 0;
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+            ans = inet_pton(AF_INET, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
+        break;
+
+
+        case ADDRESS_TYPE_INET6:
+            
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
+            ans = inet_pton(AF_INET6, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in6  *>(_platform_data)->sin6_addr);
+        break;
+
+        default:
+            return RESULT_INVALID_DATA;
+
+    }
+    setPort(prevPort);
+
+    return ans<=0?RESULT_INVALID_DATA:RESULT_OK;
+}
+
+
+u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const
+{
+    int net_family = reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family;
+    const char *ans = NULL;
+    switch (net_family) {
+        case AF_INET:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
+                            buffer, buffersize);
+        break;
+
+        case AF_INET6:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr,
+                            buffer, buffersize);
+
+        break;
+    }
+    return ans==NULL?RESULT_OPERATION_FAIL:RESULT_OK;
+}
+
+
+
+size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS, SocketAddress::address_type_t type)
+{
+    struct addrinfo hints;
+    struct addrinfo *result;
+    int ans;
+
+    memset(&hints, 0, sizeof(struct addrinfo));
+    hints.ai_family = _halAddrTypeToOSType(type);
+    hints.ai_flags = AI_PASSIVE;
+
+    if (!performDNS) {
+        hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST;
+    
+    }
+
+    ans = getaddrinfo(hostname, sevicename, &hints, &result);
+
+    addresspool.clear();
+
+    if (ans != 0) {
+        // hostname loopup failed
+        return 0;
+    }
+
+    
+    for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) {
+        if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) {
+            sockaddr_storage * storagebuffer = new sockaddr_storage;
+            assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen);
+            memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen);
+            addresspool.push_back(SocketAddress(storagebuffer));
+        }
+    }
+
+    
+    freeaddrinfo(result);
+
+    return addresspool.size();
+}
+
+
+u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const
+{
+     switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            if (bufferSize < sizeof(in_addr::s_addr)) return RESULT_INSUFFICIENT_MEMORY;
+
+            memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
+
+            
+            break;
+        case ADDRESS_TYPE_INET6:
+            if (bufferSize < sizeof(in6_addr::s6_addr)) return RESULT_INSUFFICIENT_MEMORY;
+            memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr));
+
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+
+void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type)
+{
+
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_loopback;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+}
+
+void SocketAddress::setBroadcastAddressIPv4()
+{
+
+    int prevPort = getPort();
+    sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+    addrv4->sin_family = AF_INET;
+    addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST);
+    setPort(prevPort);
+
+}
+
+void SocketAddress::setAnyAddress(SocketAddress::address_type_t type)
+{
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_ANY);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_any;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+
+
+}
+
+
+}}
+
+
+
+///--------------------------------
+
+
+namespace rp { namespace arch { namespace net{ 
+
+using namespace rp::net;
+
+class _single_thread StreamSocketImpl : public StreamSocket
+{
+public:
+
+    StreamSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) );
+
+        enableNoDelay(true);
+        this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~StreamSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>( const_cast<void *>(localaddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+    virtual u_result connect(const SocketAddress & pairAddress)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(pairAddress.getPlatformData());
+        int ans = ::connect(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (!ans) return RESULT_OK;
+
+
+        switch (errno) {
+            case EAFNOSUPPORT:
+                return RESULT_OPERATION_NOT_SUPPORT;
+#if 0
+            case EINPROGRESS:
+                return RESULT_OK; //treat async connection as good status
+#endif
+            case ETIMEDOUT:
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+      
+    virtual u_result listen(int backlog)
+    {
+        int ans = ::listen( _socket_fd,   backlog);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual StreamSocket * accept(SocketAddress * pairAddress) 
+    {
+        size_t addrsize;
+        addrsize = sizeof(sockaddr_storage);
+        int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast<struct sockaddr *>(const_cast<void *>(pairAddress->getPlatformData())):NULL
+            , (socklen_t*)&addrsize);
+
+        if (pair_socket>=0) {
+            return new StreamSocketImpl(pair_socket);
+        } else {
+            return NULL;
+        }
+    }
+
+    virtual u_result waitforIncomingConnection(_u32 timeout)
+    {
+        return waitforData(timeout);
+    }
+
+    virtual u_result send(const void * buffer, size_t len) 
+    {
+        size_t ans = ::send( _socket_fd, buffer, len, MSG_NOSIGNAL);
+        if (ans == len) {
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        }
+        
+    }
+
+
+    virtual u_result recv(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, 0);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+            
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+    }
+
+#if 0
+    virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+
+    virtual u_result getPeerAddress(SocketAddress & peerAddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>(peerAddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <=  sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+
+    }
+
+    virtual u_result shutdown(socket_direction_mask mask)
+    {
+        int shutdw_opt ;
+
+        switch (mask) {
+            case SOCKET_DIR_RD:
+                shutdw_opt = SHUT_RD;
+                break;
+            case SOCKET_DIR_WR:
+                shutdw_opt = SHUT_WR;
+                break;
+            case SOCKET_DIR_BOTH:
+            default:
+                shutdw_opt = SHUT_RDWR;
+        }
+
+        int ans = ::shutdown(_socket_fd, shutdw_opt);
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableKeepAlive(bool enable)
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableNoDelay(bool enable ) 
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+protected:
+    int  _socket_fd;
+
+
+};
+
+
+class _single_thread DGramSocketImpl : public DGramSocket
+{
+public:
+
+    DGramSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) );
+        setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~DGramSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>((localaddr.getPlatformData()))); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len)
+    {
+        const struct sockaddr * addr = target ? reinterpret_cast<const struct sockaddr *>(target->getPlatformData()) : NULL;
+        int dest_addr_size = (target ? sizeof(sockaddr_storage) : 0);
+        int ans = ::sendto(_socket_fd, (const char *)buffer, (int)len, 0, addr, dest_addr_size);
+        if (ans != -1) {
+            assert(ans == len);
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+
+                case EMSGSIZE:
+                    return RESULT_INVALID_DATA;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        
+        }
+
+    }
+
+    virtual u_result setPairAddress(const SocketAddress* pairAddress)
+    {
+        sockaddr_storage unspecAddr;
+        unspecAddr.ss_family = AF_UNSPEC;
+
+        const struct sockaddr* addr = pairAddress ? reinterpret_cast<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&unspecAddr);
+        int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage));
+        return ans ? RESULT_OPERATION_FAIL : RESULT_OK;
+
+    }
+    
+    virtual u_result clearRxCache()
+    {
+        timeval tv;
+        tv.tv_sec = 0;
+        tv.tv_usec = 0;
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        int res = -1;
+        char recv_data[2];
+        memset(recv_data, 0, sizeof(recv_data));
+        while (true) {
+            res = select(FD_SETSIZE, &rdset, nullptr, nullptr, &tv);
+            if (res == 0) break;
+            recv(_socket_fd, recv_data, 1, 0);
+        }
+        return RESULT_OK;
+    }
+
+    virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+
+#if 0
+    virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size);
+
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+    
+protected:
+    int  _socket_fd;
+
+};
+
+
+}}}
+
+
+namespace rp { namespace net{ 
+
+
+static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family)
+{
+    switch (family) {
+        case SocketBase::SOCKET_FAMILY_INET:
+            return AF_INET;
+        case SocketBase::SOCKET_FAMILY_INET6:
+            return AF_INET6;
+        case SocketBase::SOCKET_FAMILY_RAW:
+            return AF_PACKET;
+        default:
+            assert(!"should not reach here");
+            return AF_INET; // force treating as IPv4 in release mode
+    }
+
+}
+
+StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    if (family == SOCKET_FAMILY_RAW) return NULL;
+
+
+    int socket_family = _socketHalFamilyToOSFamily(family);
+    int socket_fd = ::socket(socket_family, SOCK_STREAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    StreamSocket * newborn = static_cast<StreamSocket *>(new rp::arch::net::StreamSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    int socket_family = _socketHalFamilyToOSFamily(family);
+
+
+    int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    DGramSocket * newborn = static_cast<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+}}
+
diff --git a/LIDAR/sdk/src/arch/linux/thread.hpp b/LIDAR/sdk/src/arch/linux/thread.hpp
new file mode 100644
index 0000000..6a9107b
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/thread.hpp
@@ -0,0 +1,185 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+
+#include <sched.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+#include <sys/time.h>
+#include <sys/resource.h>
+
+namespace rp{ namespace hal{
+
+Thread Thread::create(thread_proc_t proc, void * data)
+{
+    Thread newborn(proc, data);
+    
+    // tricky code, we assume pthread_t is not a structure but a word size value
+    assert( sizeof(newborn._handle) >= sizeof(pthread_t));
+
+    pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data);
+
+    return newborn;
+}
+
+u_result Thread::terminate()
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL;
+}
+
+u_result Thread::SetSelfPriority( priority_val_t p)
+{
+
+    pid_t selfTid = syscall(SYS_gettid);
+
+        // check whether current schedule policy supports priority levels
+    int current_policy = SCHED_OTHER;
+    struct sched_param current_param;
+    int nice = 0;
+    int ans;
+
+    if (sched_getparam(selfTid, &current_param))
+    {
+        // cannot retreieve values
+        return RESULT_OPERATION_FAIL;
+    }   
+
+    int pthread_priority_min;
+
+#if 1
+    pthread_priority_min = sched_get_priority_min(SCHED_RR);
+#else
+    pthread_priority_min = 1;
+#endif
+	int pthread_priority = 0 ;
+
+	switch(p)
+	{
+	case PRIORITY_REALTIME:
+		//pthread_priority = pthread_priority_max;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min + 1;
+        nice = 0;
+		break;
+	case PRIORITY_HIGH:
+		//pthread_priority = (pthread_priority_max + pthread_priority_min)/2;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min;
+        nice = 0;
+		break;
+	case PRIORITY_NORMAL:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 0;
+        break;
+	case PRIORITY_LOW:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 10;
+        break;
+	case PRIORITY_IDLE:
+		pthread_priority = 0;
+        current_policy = SCHED_IDLE;
+        nice = 0;
+		break;
+	}
+    // change the inhertiable behavior
+    current_policy |= SCHED_RESET_ON_FORK;
+
+    current_param.__sched_priority = pthread_priority;
+
+  
+
+    
+    // do not use pthread version as it will make the priority be inherited by a thread child
+	if ( (ans = sched_setscheduler(selfTid, current_policy , &current_param)) )
+	{
+        if (ans == EPERM)
+        {
+            //DBG_PRINT("warning, current process hasn't the right permission to set threads priority\n");
+        }
+		return RESULT_OPERATION_FAIL;
+	}
+
+
+    if ((current_policy == SCHED_OTHER) || (current_policy == SCHED_BATCH))
+    {
+        if (setpriority(PRIO_PROCESS, selfTid, nice)) {
+            return RESULT_OPERATION_FAIL;
+        }
+    }
+    
+
+	return  RESULT_OK;
+}
+
+Thread::priority_val_t Thread::getPriority()
+{
+	if (!this->_handle) return PRIORITY_NORMAL;
+
+    int current_policy;
+    struct sched_param current_param;
+    if (pthread_getschedparam( (pthread_t) this->_handle, &current_policy, &current_param))
+    {
+        // cannot retreieve values
+        return PRIORITY_NORMAL;
+    }   
+
+    int pthread_priority_max = sched_get_priority_max(SCHED_RR);
+    int pthread_priority_min = sched_get_priority_min(SCHED_RR);
+
+    if (current_param.__sched_priority ==(pthread_priority_max ))
+	{
+		return PRIORITY_REALTIME;
+	}
+	if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2)
+	{
+		return PRIORITY_HIGH;
+	}
+	return PRIORITY_NORMAL;
+}
+
+u_result Thread::join(unsigned long timeout)
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    pthread_join((pthread_t)(this->_handle), NULL);
+    this->_handle = 0;
+    return RESULT_OK;
+}
+
+}}
diff --git a/LIDAR/sdk/src/arch/linux/timer.cpp b/LIDAR/sdk/src/arch/linux/timer.cpp
new file mode 100644
index 0000000..a727e46
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/timer.cpp
@@ -0,0 +1,52 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+
+namespace rp{ namespace arch{
+_u64 rp_getus()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000000LL + t.tv_nsec/1000;
+}
+_u64 rp_getms()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000L + t.tv_nsec/1000000L;
+}
+}}
diff --git a/LIDAR/sdk/src/arch/linux/timer.h b/LIDAR/sdk/src/arch/linux/timer.h
new file mode 100644
index 0000000..1641001
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/timer.h
@@ -0,0 +1,59 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "hal/types.h"
+
+#include <unistd.h>
+static inline void delay(_word_size_t ms){
+    while (ms>=1000){
+        usleep(1000*1000);
+        ms-=1000;
+    };
+    if (ms!=0)
+        usleep(ms*1000);
+}
+
+// TODO: the highest timer interface should be clock_gettime
+namespace rp{ namespace arch{
+
+_u64 rp_getus();
+_u64 rp_getms();
+
+}}
+
+#define getms() rp::arch::rp_getms()
+#define getus() rp::arch::rp_getus()
+