/opt/ros/diamondback/stacks/ethzasl_aseba/asebaros/dashel/svn/dashel/dashel-win32.cpp

Go to the documentation of this file.

/*
        Dashel
        A cross-platform DAta Stream Helper Encatargetulation Library
        Copyright (C) 2007 -- 2009:
                
                Stephane Magnenat <stephane at magnenat dot net>
                        (http://stephane.magnenat.net)
                Mobots group - Laboratory of Robotics Systems, EPFL, Lausanne
                        (http://mobots.epfl.ch)
                
                Sebastian Gerlach
                Kenzan Technologies
                        (http://www.kenzantech.com)
        
        All rights reserved.
        
        Redistribution and use in source and binary forms, with or without
        modification, are permitted provided that the following conditions are met:
                * Redistributions of source code must retain the above copyright
                  notice, this list of conditions and the following disclaimer.
                * 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.
                * Neither the names of "Mobots", "Laboratory of Robotics Systems", "EPFL",
                  "Kenzan Technologies" nor the names of the contributors may be used to
                  endorse or promote products derived from this software without specific
                  prior written permission.
        
        THIS SOFTWARE IS PROVIDED BY COPYRIGHT HOLDERS ``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 COPYRIGHT HOLDERS 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 "dashel.h"

#include <cassert>
#include <cstdlib>
#include <malloc.h>
#include <map>
#include <vector>
#include <algorithm>
#include <iostream>
#include <sstream>

#ifdef _MSC_VER
        #pragma comment(lib, "ws2_32.lib")
        #pragma comment(lib, "wbemuuid.lib")
        #pragma comment(lib, "comsuppw.lib")
#endif // _MSC_VER

#define _WIN32_WINNT 0x0501
#include <winsock2.h>
#include <windows.h>
#include <setupapi.h>


#ifdef _MSC_VER
        #include <comdef.h>
        #include <Wbemidl.h>
//#else
//      #include "Wbemidl.h"
#endif // _MSC_VER

#pragma warning(disable:4996)

#include "dashel-private.h"

namespace Dashel
{
        template<typename Derived, typename Base>
        inline Derived polymorphic_downcast(Base base)
        {
                Derived derived = dynamic_cast<Derived>(base);
                assert(derived);
                return derived;
        }

        void Stream::fail(DashelException::Source s, int se, const char* reason)
        {
                char sysMessage[1024] = {0};
                failedFlag = true;

                if (se)
                        FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, se, 0, sysMessage, 1024, NULL);

                failReason = reason;
                failReason += " ";
                failReason += sysMessage;
                
                throw DashelException(s, se, failReason.c_str(), this);
        }

        // Serial port enumerator

        std::map<int, std::pair<std::string, std::string> > SerialPortEnumerator::getPorts()
        {
                std::map<int, std::pair<std::string, std::string> > ports;

                #ifndef _MSC_VER
                
                // Oldschool technique - returns too many ports...
                DWORD n, p;
                EnumPorts(NULL, 1, NULL, 0, &n, &p);
                PORT_INFO_1 *d = (PORT_INFO_1*)alloca(n);
                if(!d)
                        throw DashelException(DashelException::EnumerationError, GetLastError(), "Could not allocate buffer for port devices.");
                if(!EnumPorts(NULL, 1, (LPBYTE)d, n, &n, &p))
                        throw DashelException(DashelException::EnumerationError, GetLastError(), "Cannot get serial port devices.");

                for(n = 0; n < p; ++n)
                {
                        if(!strncmp(d[n].pName, "COM", 3))
                        {
                                int v = atoi(&d[n].pName[3]);
                                if(v > 0 && v < 256)
                                {
                                        ports.insert(std::pair<int, std::pair<std::string, std::string> >(v, std::pair<std::string, std::string> (d[n].pName, d[n].pName)));
                                }
                        }
                }
                
                #else // _MSC_VER 
                
                // Newschool technique - OK behaviour now...
                // WMI should be able to return everything, but everything we are looking for is probably well
                // lost in the namespaces.
                HRESULT hr;
            hr = CoInitializeEx(0, COINIT_MULTITHREADED); 
                if(hr == RPC_E_CHANGED_MODE)
                {
                        hr = CoInitializeEx(0, COINIT_APARTMENTTHREADED); 
                        if(FAILED(hr))
                                throw DashelException(DashelException::EnumerationError, GetLastError(), "Cannot get serial port devices (could not start COM with apartment thread model either).");
                }
            else if(FAILED(hr))
                        throw DashelException(DashelException::EnumerationError, GetLastError(), "Cannot get serial port devices (could not start COM).");
            hr = CoInitializeSecurity(NULL, -1, NULL, NULL, RPC_C_AUTHN_LEVEL_DEFAULT, RPC_C_IMP_LEVEL_IMPERSONATE, NULL, EOAC_NONE, NULL);
            if(FAILED(hr))
                {
                CoUninitialize();
                        throw DashelException(DashelException::EnumerationError, GetLastError(), "Cannot get serial port devices (could not setup COM security).");
                }

                IWbemLocator *pLoc = NULL;
                hr = CoCreateInstance(CLSID_WbemLocator, 0, CLSCTX_INPROC_SERVER, IID_IWbemLocator, (LPVOID *) &pLoc);
            if(FAILED(hr))
                {
                CoUninitialize();
                        throw DashelException(DashelException::EnumerationError, GetLastError(), "Cannot get serial port devices (could not create WBEM locator).");
                } 

                IWbemServices *pSvc = NULL;
                hr = pLoc->ConnectServer(_bstr_t(L"ROOT\\CIMV2"), NULL, NULL, 0, NULL, 0, 0, &pSvc);
            if(FAILED(hr))
                {
                        pLoc->Release();
                CoUninitialize();
                        throw DashelException(DashelException::EnumerationError, GetLastError(), "Cannot get serial port devices (could not connect to root of CIMV2).");
                } 
    
                hr = CoSetProxyBlanket(pSvc, RPC_C_AUTHN_WINNT, RPC_C_AUTHZ_NONE, NULL, RPC_C_AUTHN_LEVEL_CALL, RPC_C_IMP_LEVEL_IMPERSONATE, NULL, EOAC_NONE);
            if(FAILED(hr))
                {
                        pSvc->Release();
                        pLoc->Release();
                CoUninitialize();
                        throw DashelException(DashelException::EnumerationError, GetLastError(), "Cannot get serial port devices (could not set proxy blanket).");
                } 

                IEnumWbemClassObject* pEnumerator = NULL;
                hr = pSvc->ExecQuery(bstr_t("WQL"), bstr_t("SELECT * FROM Win32_PnPEntity WHERE ClassGuid=\"{4D36E978-E325-11CE-BFC1-08002BE10318}\""), WBEM_FLAG_FORWARD_ONLY | WBEM_FLAG_RETURN_IMMEDIATELY, NULL, &pEnumerator);
            if(FAILED(hr))
                {
                        pSvc->Release();
                        pLoc->Release();
                CoUninitialize();
                        throw DashelException(DashelException::EnumerationError, GetLastError(), "Cannot get serial port devices (could not execute WBEM query).");
                } 

                IWbemClassObject *pclsObj;
                ULONG uReturn = 0;
                while (pEnumerator)
                {
                        HRESULT hr = pEnumerator->Next(WBEM_INFINITE, 1, &pclsObj, &uReturn);
                        if(0 == uReturn)
                                break;

                        VARIANT vtProp;
                        VariantInit(&vtProp);
                        char dn[1024], dcn[1024], *co;

                        // Get the value of the Name property
                        hr = pclsObj->Get(L"Caption", 0, &vtProp, 0, 0);
                        if(!FAILED(hr))
                        {
                                WideCharToMultiByte(CP_UTF8, 0, vtProp.bstrVal, -1, dn, 1024, NULL, NULL);
                                VariantClear(&vtProp);

                                //hr = pclsObj->Get(L"Name", 0, &vtProp, 0, 0);
                                //WideCharToMultiByte(CP_UTF8, 0, vtProp.bstrVal, -1, dcn, 1024, NULL, NULL);
                                //VariantClear(&vtProp);

                                if((co = strstr(dn, "(COM")))
                                {
                                        strcpy(dcn, co+1);
                                        strtok(dcn, ")");

                                        int v = atoi(&dcn[3]);

                                        if(v > 0 && v < 256)
                                        {
                                                std::string name = std::string("\\\\.\\").append(dcn);
                                                ports.insert(std::pair<int, std::pair<std::string, std::string> >(v, std::pair<std::string, std::string> (name, dn)));
                                        }
                                }
                        }
                        pclsObj->Release();
                        pclsObj = NULL;
                }

                pSvc->Release();
                pLoc->Release();
                pEnumerator->Release();
                CoUninitialize();
                
                #endif // _MSC_VER
                
                return ports;
        };


        void startWinSock()
        {
                bool started = false;
                if(!started)
                {
                        WORD ver = 0x0101;
                        WSADATA d;
                        memset(&d, 0, sizeof(d));

                        int rv = WSAStartup(ver, &d);
                        if(rv)
                                throw DashelException(DashelException::Unknown, rv, "Could not start WinSock service.");
                        started = true;
                }
        }

        class WaitableStream: virtual public Stream
        {
        public:

                std::map<EvType,HANDLE> hEvents; 

                bool readDone;
                
        protected:

                HANDLE createEvent(EvType t) 
                {
                        HANDLE he = CreateEvent(NULL, FALSE, FALSE, NULL);
                        hEvents[t] = he;
                        return he;
                }


                void addEvent(EvType t, HANDLE he) 
                {
                        hEvents[t] = he;
                }
                
        public:
                WaitableStream(const std::string& protocolName) : Stream(protocolName) { }


                virtual ~WaitableStream()
                {
                        for(std::map<EvType, HANDLE>::iterator it = hEvents.begin(); it != hEvents.end(); ++it)
                                CloseHandle(it->second);
                }
                

                virtual void notifyEvent(Hub *srv, EvType& t) { }
        };


        class SocketServerStream : public WaitableStream
        {
        protected:
                SOCKET sock;

                HANDLE hev;

                const bool resolveIncomingNames;

        public:

                SocketServerStream(const std::string& params, const bool resolveIncomingNames) : Stream("tcpin"), WaitableStream("tcpin"),
                        resolveIncomingNames(resolveIncomingNames)
                { 
                        target.add("tcpin:port=5000;address=0.0.0.0");
                        target.add(params.c_str());

                        startWinSock();

                        IPV4Address bindAddress(target.get("address"), target.get<int>("port"));
                        
                        // Create socket.
                        sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
                        if (sock == SOCKET_ERROR)
                                throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot create socket.");
                        
                        // Reuse address.
                        int flag = 1;
                        if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (const char *)&flag, sizeof (flag)) < 0)
                                throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot set address reuse flag on socket, probably the port is already in use.");
                        
                        // Bind socket.
                        sockaddr_in addr;
                        addr.sin_family = AF_INET;
                        addr.sin_port = htons(bindAddress.port);
                        addr.sin_addr.s_addr = htonl(bindAddress.address);
                        if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) != 0)
                                throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot bind socket to port, probably the port is already in use.");
                        
                        // Listen on socket, backlog is sort of arbitrary.
                        if(listen(sock, 16) == SOCKET_ERROR)
                                throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot listen on socket.");

                        // Create and register event.
                        hev = createEvent(EvConnect);
                        WSAEventSelect(sock, hev, FD_ACCEPT);
                }

                ~SocketServerStream()
                {
                        if(sock)
                                closesocket(sock);
                }


                virtual void notifyEvent(Hub *srv, EvType& t) 
                { 
                        if(t == EvConnect)
                        {
                                // Accept incoming connection.
                                struct sockaddr_in targetAddr;
                                int l = sizeof (targetAddr);
                                SOCKET trg = accept (sock, (struct sockaddr *)&targetAddr, &l);
                                if (trg == SOCKET_ERROR)
                                {
                                        fail(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot accept incoming connection on socket.");
                                }
                                
                                // create stream
                                std::string ls = IPV4Address(ntohl(targetAddr.sin_addr.s_addr), ntohs(targetAddr.sin_port)).format(resolveIncomingNames);
                                
                                std::ostringstream buf;
                                buf << ";connectionPort=";
                                buf << atoi(getTargetParameter("port").c_str());
                                buf << ";sock=";
                                buf << (int)trg;
                                ls.append(buf.str());
                                srv->connect(ls);
                        }
                }

                virtual void write(const void *data, const size_t size) { }
                virtual void flush() { }
                virtual void read(void *data, size_t size) { }
        };


        class StdinStream : public WaitableStream
        {
        protected:
                HANDLE hf;

                HANDLE hev;

        public:

                StdinStream(const std::string& params) : Stream("stdin"), WaitableStream("stdin")
                { 
                        target.add(params.c_str());

                        if((hf = GetStdHandle(STD_INPUT_HANDLE)) == INVALID_HANDLE_VALUE)
                                throw DashelException(DashelException::ConnectionFailed, GetLastError(), "Cannot open standard input.");

                        DWORD cm;
                        GetConsoleMode(hf, &cm);
                        cm &= ~(ENABLE_ECHO_INPUT | ENABLE_LINE_INPUT);
                        if(!SetConsoleMode(hf, cm))
                                throw DashelException(DashelException::ConnectionFailed, GetLastError(), "Cannot change standard input mode to immediate.");

                        // Create events.
                        addEvent(EvPotentialData, hf);
                        hev = createEvent(EvData);
                }

                ~StdinStream()
                {
                        CloseHandle(hf);
                }


                virtual void notifyEvent(Hub *srv, EvType& t) 
                { 
                        DWORD n = 0;
                        if(GetNumberOfConsoleInputEvents(hf, &n))
                        {
                                if(n > 0)
                                {
                                        INPUT_RECORD ir;
                                        PeekConsoleInput(hf, &ir, 1, &n);
                                        if(ir.EventType != KEY_EVENT)
                                                ReadConsoleInput(hf, &ir, 1, &n);
                                        else
                                        {
                                                t = EvData;
                                        }
                                }
                        }
                }

                virtual void write(const void *data, const size_t size)
                { 
                        throw DashelException(DashelException::InvalidOperation, GetLastError(), "Cannot write to standard input.", this);
                }
                
                virtual void flush() 
                { 
                        throw DashelException(DashelException::InvalidOperation, GetLastError(), "Cannot flush standard input.", this);
                }
                
                virtual void read(void *data, size_t size)
                {
                        char *ptr = (char *)data;
                        DWORD left = (DWORD)size;

                        // Quick check to make sure nobody is giving us funny 64-bit stuff.
                        assert(left == size);

                        readDone = true;

                        while (left)
                        {
                                DWORD len = 0;
                                BOOL r;

                                // Blocking write.
                                if((r = ReadFile(hf, ptr, left, &len, NULL)) == 0)
                                {
                                        fail(DashelException::IOError, GetLastError(), "Read error from standard input.");
                                }
                                else
                                {
                                        ptr += len;
                                        left -= len;
                                }
                        }
                }

        };

        class StdoutStream : public WaitableStream
        {
        protected:
                HANDLE hf;

        public:

                StdoutStream(const std::string& params) : Stream("stdout"), WaitableStream("stdout")
                { 
                        target.add(params.c_str());

                        if((hf = GetStdHandle(STD_OUTPUT_HANDLE)) == INVALID_HANDLE_VALUE)
                                throw DashelException(DashelException::ConnectionFailed, GetLastError(), "Cannot open standard output.");
                }

                ~StdoutStream()
                {
                        CloseHandle(hf);
                }

                virtual void write(const void *data, const size_t size)
                {
                        const char *ptr = (const char *)data;
                        DWORD left = (DWORD)size;

                        // Quick check to make sure nobody is giving us funny 64-bit stuff.
                        assert(left == size);

                        while (left)
                        {
                                DWORD len = 0;
                                BOOL r;

                                // Blocking write.
                                if((r = WriteFile(hf, ptr, left, &len, NULL)) == 0)
                                {
                                        fail(DashelException::IOError, GetLastError(), "Write error to standard output.");
                                }
                                else
                                {
                                        ptr += len;
                                        left -= len;
                                }
                        }
                }
                
                virtual void flush()
                {
                        FlushFileBuffers(hf);
                }
                
                virtual void read(void *data, size_t size) 
                { 
                        fail(DashelException::InvalidOperation, GetLastError(), "Cannot read from standard output.");
                }

        };

        class FileStream : public WaitableStream
        {
        protected:
                HANDLE hf;

                OVERLAPPED ovl;

                DWORD writeOffset;


                bool readyToRead;

                char readByte;

                bool readByteAvailable;

                HANDLE hEOF;

        protected:

                FileStream(const std::string& protocolName, bool dummy) : Stream(protocolName), WaitableStream(protocolName) { }

                void startStream(EvType et = EvData)
                {
                        readByteAvailable = false;
                        memset(&ovl, 0, sizeof(ovl));
                        ovl.hEvent = createEvent(et);
                        BOOL r = ReadFile(hf, &readByte, 1, NULL, &ovl);
                        if(!r)
                        {
                                DWORD err = GetLastError();
                                if(err != ERROR_IO_PENDING)
                                        throw DashelException(DashelException::IOError, GetLastError(), "Cannot read from file stream.");
                        }
                        else
                                readByteAvailable = true;
                }

        public:

                FileStream(const std::string& params) : Stream("file"), WaitableStream("file")
                { 
                        target.add("file:name;mode=read");
                        target.add(params.c_str());
                        std::string name = target.get("name");
                        std::string mode = target.get("mode");

                        hf = NULL;
                        if (mode == "read")
                        {
                                hf = CreateFile(name.c_str(), GENERIC_READ, FILE_SHARE_READ, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL);
                                startStream();
                        }
                        else if (mode == "write")
                        {
                                writeOffset = 0;
                                hf = CreateFile(name.c_str(), GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, CREATE_ALWAYS, FILE_FLAG_OVERLAPPED, NULL);
                        }
                        else if (mode == "readwrite") 
                        {
                                writeOffset = 0;
                                hf = CreateFile(name.c_str(), GENERIC_WRITE | GENERIC_READ, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_ALWAYS, FILE_FLAG_OVERLAPPED, NULL);
                                startStream();
                        }
                        if(hf == INVALID_HANDLE_VALUE)
                                throw DashelException(DashelException::ConnectionFailed, GetLastError(), "Cannot open file.");

                        hEOF = createEvent(EvClosed);
                }

                ~FileStream()
                {
                        CloseHandle(hf);
                }
                
                virtual void write(const void *data, const size_t size)
                {
                        const char *ptr = (const char *)data;
                        DWORD left = (DWORD)size;

                        // Quick check to make sure nobody is giving us funny 64-bit stuff.
                        assert(left == size);

                        while (left)
                        {
                                DWORD len = 0;
                                OVERLAPPED o;
                                memset(&o, 0, sizeof(o));

                                o.Offset = writeOffset;

                                // Blocking write.
                                BOOL r = WriteFile(hf, ptr, left, &len, &o);
                                if(!r)
                                {
                                        DWORD err;
                                        switch((err = GetLastError()))
                                        {
                                        case ERROR_IO_PENDING:
                                                GetOverlappedResult(hf, &o, &len, TRUE);
                                                ptr += len;
                                                left -= len;
                                                break;

                                        default:
                                                fail(DashelException::IOError, GetLastError(), "Cannot write to file.");
                                                break;
                                        }
                                }
                                else
                                {
                                        ptr += len;
                                        left -= len;
                                }
                        
                                writeOffset += len;     
                        }
                }
                
                virtual void flush()
                {
                        FlushFileBuffers(hf);
                }
                
                virtual void read(void *data, size_t size)
                {
                        char *ptr = (char *)data;
                        DWORD left = (DWORD)size;

                        // Quick check to make sure nobody is giving us funny 64-bit stuff.
                        assert(left == size);
                        
                        if (size == 0)
                                return;

                        readDone = true;

                        if(!readByteAvailable)
                                WaitForSingleObject(ovl.hEvent, INFINITE);

                        DWORD dataUsed;
                        if(!GetOverlappedResult(hf, &ovl, &dataUsed, TRUE))
                                fail(DashelException::IOError, GetLastError(), "File read I/O error.");

                        if(dataUsed)
                        {
                                *ptr++ = readByte;
                                left--;
                        }
                        readByteAvailable = false;

                        while (left)
                        {
                                DWORD len = 0;
                                OVERLAPPED o;
                                memset(&o, 0, sizeof(o));
                                o.Offset = ovl.Offset + size - left;
                                o.hEvent = ovl.hEvent ;

                                // Non-blocking read.
                                BOOL r = ReadFile(hf, ptr, left, &len, &o);
                                if(!r)
                                {
                                        DWORD err;
                                        switch((err = GetLastError()))
                                        {
                                        case ERROR_HANDLE_EOF:
                                                fail(DashelException::ConnectionLost, GetLastError(), "Reached end of file.");
                                                break;

                                        case ERROR_IO_PENDING:
                                                WaitForSingleObject(ovl.hEvent, INFINITE);
                                                if(!GetOverlappedResult(hf, &o, &len, TRUE))
                                                        fail(DashelException::IOError, GetLastError(), "File read I/O error.");
                                                if(len == 0)
                                                        return;

                                                ptr += len;
                                                left -= len;
                                                break;

                                        default:
                                                fail(DashelException::IOError, GetLastError(), "File read I/O error.");
                                                break;
                                        }
                                }
                                else
                                {
                                        WaitForSingleObject(ovl.hEvent, INFINITE);
                                        ptr += len;
                                        left -= len;
                                }
                        }

                        // Reset our blocking read for whatever is up next.
                        ovl.Offset += (DWORD)size;
                        BOOL r = ReadFile(hf, &readByte, 1, &dataUsed, &ovl);
                        if(!r)
                        {
                                DWORD err = GetLastError();
                                if(err == ERROR_HANDLE_EOF)
                                {
                                        SetEvent(hEOF);
                                }
                                else if(err != ERROR_IO_PENDING)
                                {
                                        fail(DashelException::IOError, GetLastError(), "Cannot read from file stream.");
                                }
                        }
                        else
                                readByteAvailable = true;

                }


                virtual void notifyEvent(Hub *srv, EvType& t) 
                { 
                        if(t == EvPotentialData)
                        {
                                DWORD dataUsed;
                                GetOverlappedResult(hf, &ovl, &dataUsed, TRUE);
                                if(dataUsed == 0)
                                        ReadFile(hf, &readByte, 1, NULL, &ovl);
                                else
                                {
                                        readByteAvailable = true;
                                        t = EvData;
                                }
                        }
                }
        };

        class SerialStream : public FileStream
        {
        private:

                bool buildDCB(HANDLE sp, int speed, int bits, const std::string& parity, const std::string& stopbits, const std::string& fc)
                {
                        DCB dcb;

                        memset(&dcb, 0, sizeof(dcb));
                        dcb.DCBlength = sizeof(dcb);

                        if(!GetCommState(sp, &dcb))
                                throw DashelException(DashelException::ConnectionFailed, GetLastError(), "Cannot read current serial port state.", this);

                        // Fill in the DCB
                        memset(&dcb,0,sizeof(dcb));
                        dcb.DCBlength = sizeof(dcb);
                        if(fc == "hard")
                        {
                                dcb.fOutxCtsFlow = TRUE;
                                dcb.fRtsControl = RTS_CONTROL_HANDSHAKE;
                        }
                        else
                        {
                                dcb.fOutxCtsFlow = FALSE;
                                dcb.fRtsControl = RTS_CONTROL_DISABLE;
                        }

                        dcb.fOutxDsrFlow = FALSE;
                        dcb.fDtrControl = DTR_CONTROL_DISABLE;
                        dcb.fDsrSensitivity = FALSE;
                        dcb.fBinary = TRUE;
                        dcb.fParity = TRUE;
                        dcb.BaudRate = speed;
                        dcb.ByteSize = bits;
                        if(parity == "even")
                                dcb.Parity = EVENPARITY;
                        else if(parity == "odd")
                                dcb.Parity = ODDPARITY;
                        else if(parity == "space")
                                dcb.Parity = SPACEPARITY;
                        else if(parity == "mark")
                                dcb.Parity = MARKPARITY;
                        else
                                dcb.Parity = NOPARITY;

                        if(stopbits == "1.5")
                                dcb.StopBits = ONE5STOPBITS;
                        else if(stopbits == "2")
                                dcb.StopBits = TWOSTOPBITS;
                        else 
                                dcb.StopBits = ONESTOPBIT;

                        // Set the com port state.
                        if(!SetCommState(sp, &dcb))
                                throw DashelException(DashelException::ConnectionFailed, GetLastError(), "Cannot set new serial port state.", this);

                        // Set timeouts as well for good measure. Since we will effectively be woken whenever
                        // this happens, keep it long.
                        COMMTIMEOUTS cto;
                        memset(&cto, 0, sizeof(cto));
                        cto.ReadIntervalTimeout = 100000;
                        cto.ReadTotalTimeoutConstant = 100000;
                        cto.ReadTotalTimeoutMultiplier = 100000;
                        cto.WriteTotalTimeoutConstant = 100000;
                        cto.WriteTotalTimeoutMultiplier = 100000;
                        if(!SetCommTimeouts(sp, &cto))
                                throw DashelException(DashelException::ConnectionFailed, GetLastError(), "Cannot set new serial port timeouts.", this);

                        return true;
                }

        public:

                SerialStream(const std::string& params) : Stream("ser"), FileStream("ser", true)
                { 
                        target.add("ser:port=1;baud=115200;stop=1;parity=none;fc=none;bits=8");
                        target.add(params.c_str());

                        std::string name;
                        if (target.isSet("device"))
                        {
                                target.addParam("device", NULL, true);
                                target.erase("port");
                                
                                name = target.get("device");
                        }
                        else
                        {
                                target.erase("device");
                                
                                name = std::string("\\\\.\\COM").append(target.get("port"));
                        }

                        hf = CreateFile(name.c_str(), GENERIC_WRITE | GENERIC_READ, 0, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL);
                        if(hf == INVALID_HANDLE_VALUE)
                                throw DashelException(DashelException::ConnectionFailed, GetLastError(), "Cannot open serial port.");

                        buildDCB(hf, target.get<int>("baud"), target.get<int>("bits"), target.get("parity"), target.get("stop"), target.get("fc"));

                        startStream(EvPotentialData);
                }
        };

        class SocketStream : public WaitableStream
        {
                SOCKET sock;

                HANDLE hev;

                HANDLE hev2;

                HANDLE hev3;


                bool readyToRead;

                char readByte;

                bool readByteAvailable;

        public:

                SocketStream(const std::string& params) : Stream("tcp"), WaitableStream("tcp")
                { 
                        target.add("tcp:host;port;connectionPort=-1;sock=0");
                        target.add(params.c_str());

                        sock = target.get<SOCKET>("sock");
                        if(!sock)
                        {
                                startWinSock();

                                // create socket
                                sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
                                if (sock == SOCKET_ERROR)
                                        throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot create socket.");
                        
                                IPV4Address remoteAddress(target.get("host"), target.get<int>("port"));
                                // connect
                                sockaddr_in addr;
                                addr.sin_family = AF_INET;
                                addr.sin_port = htons(remoteAddress.port);
                                addr.sin_addr.s_addr = htonl(remoteAddress.address);
                                if (connect(sock, (struct sockaddr *)&addr, sizeof(addr)) != 0)
                                        throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot connect to remote host.");
                                
                                // overwrite target name with a canonical one
                                target.add(remoteAddress.format().c_str());
                                target.erase("connectionPort");
                        }
                        else
                        {
                                // remove file descriptor information from target name
                                target.erase("sock");
                        }

                        hev2 = createEvent(EvData);
                        hev3 = createEvent(EvClosed);
                        hev = createEvent(EvPotentialData);

                        int rv = WSAEventSelect(sock, hev, FD_READ | FD_CLOSE);
                        if (rv == SOCKET_ERROR)
                                throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot select socket events.");

                        readyToRead = false;
                        readByteAvailable = false;
                }

                ~SocketStream()
                {
                        shutdown(sock, SD_BOTH);
                        closesocket(sock);
                }


                virtual void notifyEvent(Hub *srv, EvType& t) 
                { 
                        if(t == EvPotentialData)
                        {
                                if(readByteAvailable)
                                        return;

                                int rv = recv(sock, &readByte, 1, 0);
                                if(rv <= 0)
                                {
                                        t = EvClosed;
                                }
                                else
                                {
                                        readByteAvailable = true;
                                        readyToRead = true;
                                        t = EvData;
                                }
                        }
                }

                virtual void write(const void *data, const size_t size)
                {
                        char *ptr = (char *)data;
                        size_t left = size;
                        
                        while (left)
                        {
                                int len = send(sock, ptr, (int)left, 0);
                                
                                if (len == SOCKET_ERROR)
                                {
                                        fail(DashelException::ConnectionFailed, GetLastError(), "Connection lost on write.");
                                }
                                else
                                {
                                        ptr += len;
                                        left -= len;
                                }
                        }
                }
                
                virtual void flush() { }
                
                virtual void read(void *data, size_t size)
                {
                        char *ptr = (char *)data;
                        size_t left = size;
                        
                        if (size == 0)
                                return;

                        readDone = true;

                        //std::cerr << "ready to read " << readyToRead << std::endl;
                        if(!readyToRead)
                        {
                                // Block until something happens.
                                WaitForSingleObject(hev, INFINITE);
                        }
                        readyToRead = false;

                        if(readByteAvailable)
                        {
                                *ptr++ = readByte;
                                readByteAvailable = false;
                                left--;
                                if(left)
                                        WaitForSingleObject(hev, INFINITE);
                        }
                        
                        while (left)
                        {
                                //std::cerr << "ready to recv " << std::endl;
                                int len = recv(sock, ptr, (int)left, 0);
                                //std::cerr << "recv done " << std::endl;
                                
                                if (len == SOCKET_ERROR)
                                {
                                        //std::cerr << "socket error" << std::endl;
                                        fail(DashelException::ConnectionFailed, GetLastError(), "Connection lost on read.");
                                }
                                else if(len == 0)
                                {
                                        // We have been disconnected.
                                }
                                else
                                {
                                        ptr += len;
                                        left -= len;
                                }
                                if(left)
                                {
                                        // Wait for more data.
                                        WaitForSingleObject(hev, INFINITE);
                                }
                        }

/*                      int rv = WSAEventSelect(sock, hev, FD_READ | FD_CLOSE);
                        if (rv == SOCKET_ERROR)
                                throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot select socket events.");
*/
                }
        };

        class UDPSocketStream: public MemoryPacketStream, public WaitableStream
        {
        private:
                SOCKET sock;

                HANDLE hev;

        public:
                UDPSocketStream(const std::string& targetName) :
                        Stream("udp"),
                        MemoryPacketStream("udp"),
                        WaitableStream("udp")
                {
                        target.add("udp:port=5000;address=0.0.0.0;sock=0");
                        target.add(targetName.c_str());
                        
                        sock = target.get<SOCKET>("sock");
                        if(!sock)
                        {
                                startWinSock();
                                
                                // create socket
                                sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
                                if (sock == SOCKET_ERROR)
                                        throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot create socket.");
                                
                                IPV4Address bindAddress(target.get("address"), target.get<int>("port"));
                                
                                // bind
                                sockaddr_in addr;
                                addr.sin_family = AF_INET;
                                addr.sin_port = htons(bindAddress.port);
                                addr.sin_addr.s_addr = htonl(bindAddress.address);
                                if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) != 0)
                                        throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot bind socket to port, probably the port is already in use.");
                        }
                        else
                        {
                                // remove file descriptor information from target name
                                target.erase("sock");
                        }
                        
                        // enable broadcast
                        int broadcastPermission = 1;
                        setsockopt(sock, SOL_SOCKET, SO_BROADCAST, (const char*)&broadcastPermission, sizeof(broadcastPermission));
                        
                        // Create and register event.
                        hev = createEvent(EvData);
                        
                        int rv = WSAEventSelect(sock, hev, FD_READ);
                        if (rv == SOCKET_ERROR)
                                throw DashelException(DashelException::ConnectionFailed, WSAGetLastError(), "Cannot select socket events.");
                }

                virtual ~UDPSocketStream()
                {
                        closesocket(sock);
                }

                virtual void send(const IPV4Address& dest)
                {
                        sockaddr_in addr;
                        addr.sin_family = AF_INET;
                        addr.sin_port = htons(dest.port);;
                        addr.sin_addr.s_addr = htonl(dest.address);
                        
                        if (sendto(sock, (const char*)sendBuffer.get(), sendBuffer.size(), 0, (struct sockaddr *)&addr, sizeof(addr)) != sendBuffer.size())
                                fail(DashelException::IOError, WSAGetLastError(), "UDP Socket write I/O error.");
                        
                        sendBuffer.clear();
                }
                
                virtual void receive(IPV4Address& source)
                {
                        unsigned char buf[4006];
                        sockaddr_in addr;
                        int addrLen = sizeof(addr);
                        readDone = true;

                        int recvCount = recvfrom(sock, (char*)buf, 4096, 0, (struct sockaddr *)&addr, &addrLen);
                        if (recvCount <= 0)
                                fail(DashelException::ConnectionLost, WSAGetLastError(), "UDP Socket read I/O error.");
                        
                        receptionBuffer.resize(recvCount);
                        std::copy(buf, buf+recvCount, receptionBuffer.begin());
                        
                        source = IPV4Address(ntohl(addr.sin_addr.s_addr), ntohs(addr.sin_port));
                }
        };

        Hub::Hub(const bool resolveIncomingNames):
                resolveIncomingNames(resolveIncomingNames)
        {
                hTerminate = CreateEvent(NULL, TRUE, FALSE, NULL);
        }
        
        Hub::~Hub()
        {
                for (StreamsSet::iterator it = streams.begin(); it != streams.end(); ++it)
                        delete *it;
        }
        
        Stream* Hub::connect(const std::string &target)
        {
                std::string proto, params;
                size_t c = target.find_first_of(':');
                if(c == std::string::npos)
                        throw DashelException(DashelException::InvalidTarget, 0, "No protocol specified in target.");
                proto = target.substr(0, c);
                params = target.substr(c+1);

                WaitableStream *s = NULL;
                if(proto == "file")
                        s = new FileStream(target);
                if(proto == "stdin")
                        s = new StdinStream(target);
                if(proto == "stdout")
                        s = new StdoutStream(target);
                if(proto == "ser")
                        s = new SerialStream(target);
                if(proto == "tcpin")
                        s = new SocketServerStream(target, resolveIncomingNames);
                if(proto == "tcp")
                        s = new SocketStream(target);
                if(proto == "udp")
                        s = new UDPSocketStream(target);
                
                if(!s)
                {
                        std::string r = "Invalid protocol in target: ";
                        r = r.append(proto);
                        throw DashelException(DashelException::InvalidTarget, 0, r.c_str());
                }
                
                streams.insert(s);
                if (proto != "tcpin")
                {
                        dataStreams.insert(s);
                        connectionCreated(s);
                }
                return s;
        }
        
        void Hub::run()
        {
                while(step(-1));
        }
        
        bool Hub::step(const int timeout)
        {
                HANDLE hEvs[64] = { hTerminate };
                WaitableStream *strs[64] = { NULL };
                EvType ets[64] = { EvClosed };
                
                // Wait on all our events.
                DWORD ms = timeout >= 0 ? timeout : INFINITE;

                // Loop in order to consume all events.
                do
                {
                        DWORD hc = 1;

                        // Collect all events from all our streams.
                        for(std::set<Stream*>::iterator it = streams.begin(); it != streams.end(); ++it)
                        {
                                WaitableStream* stream = polymorphic_downcast<WaitableStream*>(*it);
                                for(std::map<EvType,HANDLE>::iterator ei = stream->hEvents.begin(); ei != stream->hEvents.end(); ++ei)
                                {
                                        strs[hc] = stream;
                                        ets[hc] = ei->first;
                                        hEvs[hc] = ei->second;
                                        hc++;
                                }
                        }

                        DWORD r = WaitForMultipleObjects(hc, hEvs, FALSE, ms);
                        
                        // Check for error or timeout.
                        if (r == WAIT_FAILED)
                                throw DashelException(DashelException::SyncError, 0, "Wait failed.");
                        if (r == WAIT_TIMEOUT)
                                return true;

                        // Look for what we got.
                        r -= WAIT_OBJECT_0;
                        if(r == 0)
                        {
                                // Quit
                                return false;
                        }
                        else 
                        {
                                // Notify the stream that its event arrived.
                                strs[r]->notifyEvent(this, ets[r]);

                                // Notify user that something happended.
                                if(ets[r] == EvData)
                                {
                                        try
                                        {
                                                strs[r]->readDone = false;
                                                incomingData(strs[r]);
                                        }
                                        catch (DashelException e) { }
                                        if(!strs[r]->readDone)
                                                throw DashelException(DashelException::PreviousIncomingDataNotRead, 0, "Previous incoming data not read.", strs[r]);
                                        if(strs[r]->failed())
                                        {
                                                connectionClosed(strs[r], true);
                                                closeStream(strs[r]);
                                        }
                                }
                                // Notify user that something happended.
                                if(ets[r] == EvClosed)
                                {
                                        try
                                        {
                                                connectionClosed(strs[r], false);
                                        }
                                        catch (DashelException e) { }
                                        closeStream(strs[r]);
                                }
                        }

                        // No more timeouts on following rounds.
                        ms = 0;
                }
                while(true);
        }
        
        void Hub::lock()
        {
        }
        
        void Hub::unlock()
        {
        }

        void Hub::stop()
        {
                SetEvent(hTerminate);
        }


}

---

asebaros
Author(s): $author
autogenerated on Mon Sep 5 08:41:53 2011