liblnp.cpp

Go to the documentation of this file.

//------------------------------------------------------------------------------
// $HeadURL: file:///s:/svn/lego_programs/lib/liblnp/liblnp.cpp $
// revision  $LastChangedRevision: 2 $ at $LastChangedDate: 2006-01-24 18:20:16 +0100 (Tue, 24 Jan 2006) $
//------------------------------------------------------------------------------

//                  LIBLNP
//                  ======
//
//a library that can be linked to your application making it capable of
//sending and receiving messages according the LNP protocol
//
//This is the a port of the linux liblnp source code to windows. The
//interface file is exactly the same. The only difference is that there
//is no support for tcp/ip. Therefore it is not necessary to have a separate 
//lnpd daemon running. All you have to do is :
//  - make a C++ project (C projects do not compile because C++ code
//    is called to communicate with the com port)
//  - add the  liblnp.cpp and liblnp.h file to your visual C++ project
//  - include the liblnp.h header file into the file where you want to
//    give lnp commands.
//
//For usage see the LNP documentation. ( a good start is the Documentation
//of Mike Ash at : http://legos.sourceforge.net/HOWTO/x405.html and the general 
//documentation from Martin Cornelius on the winlnp website
//http://www.geocities.com/winlnp/doclnp.html )
//
//There is an alternative for this implementation which is the winlnp project
//at http://www.geocities.com/winlnp/. But I prefer this implementation because
//it maintains the same interface as on linux.

//
//SUCCESS!!
//
// =======================================================================
//#include "stdafx.h"

#include <cstdio>
#include <ctime>
#include <atlbase.h>

#include "liblnp.h"


// =======================================================================

// DEBUGGING PARAMETERS
#define DEBUGLNP 0
#define LNPCHECKSOMSTOP 0
#define LNPBLOCK 0

// =======================================================================

// lnp parameters
#define DEFAULT_LNP_ADDR 0x80
#define DEFAULT_LNP_MASK 0xf0
#define MAX_LNP_PACKET (256+3)

// Infraredtower parameters 
#define TOWER_KEEP_ALIVE_CHAR   0xFF
#define TOWER_KEEP_ALIVE_DELAY   2000 //ms

// comport parameters
//#define BAUDRATE   2400
#define BAUDRATE   4800
#define STOPBITS   0  // 1 stopbit
#define PARITY     0  // no parity
#define BYTESIZE   8  // read per 8 bits synchroon  ( bits arive at fixed time intervals )
// read the separate bytes asynchroon (bytes are at random time intervals)        

// Com timings 
#define READTIMEOUT 2000 // package reading timeout in milliseconds
// packages of 250 bytes take 1500 milliseconds
#define AFTERWRITESLEEPTIME 500 // time to sleep after write :
// to give incoming messages also sometime to
// be handled (when having many writes after each other)
// REMARK :  the package which is written to the tower will be send by the tower, but\
//           ALSO will be send back to the pc. ( needed for correct broadcast!!)
//           Therefore sending many packages will generate the same packages on the input
//           which may delve the packages of others!!!


// =======================================================================
// LNP variables

static unsigned char lnp_host_address;
static unsigned char lnp_host_mask;
static unsigned char lnp_port_mask;

static int connected=0;
static int lnp_stop=0;

static lnp_tx_result tx_result;

static unsigned char lnp_buffer[MAX_LNP_PACKET];

static lnp_integrity_handler_t integrity_handler;
static lnp_addressing_handler_t addressing_handler[256];

// states for the integrity layer state machine
typedef enum lnp_integrity_state_t
{
    LNPwaitHeader,
    LNPwaitLength,
    LNPwaitData,
    LNPwaitCRC
} lnp_integrity_state_t;

// the integrity layer state 
static lnp_integrity_state_t lnp_integrity_state=LNPwaitHeader;


// =======================================================================
// Com variables

static CComAutoCriticalSection crit_sect;   // protected area for locking
static HANDLE comHandle=NULL;                    // communication port file handle
static HANDLE h_KeepAliveComThread;
static HANDLE h_ComReaderThread;
static char *ComPort="com1";              // com1 or com2 ; default com1
static char *USBPort="\\\\.\\legotower1";

static BOOL UseUSB = true;

// =======================================================================


static time_t stoptime=-1;  // to see how long you are already reading a 
                            // package -> prevent too long reading !!

// =======================================================================


// compute checksum of lnp packet

static unsigned char 
lnp_checksum( const unsigned char *data, unsigned length )
{
    unsigned char a = 0xff;
    unsigned char b = 0xff;

    while (length > 0) 
    {
        a = a + *data;
        b = b + a;
        data++;
        length--;
    }

    return a + (b << 8);
}



static void 
block_rcv(void)
{
    crit_sect.Lock();
}


static void 
unblock_rcv(void)
{
    crit_sect.Unlock();
}



void 
lnp_shutdown(void)
{
    block_rcv();
    connected = 0;
    lnp_stop=1;
    lnp_integrity_state=LNPwaitHeader;
    unblock_rcv();
    DWORD rvalue=0;
    GetExitCodeThread(h_ComReaderThread,&rvalue);
    while ( rvalue == STILL_ACTIVE ) 
    {
        GetExitCodeThread(h_ComReaderThread,&rvalue);
    };
    CloseHandle(h_ComReaderThread);
    if( ! UseUSB )
    {
        GetExitCodeThread(h_KeepAliveComThread,&rvalue);
        while ( rvalue == STILL_ACTIVE ) 
        {
            GetExitCodeThread(h_KeepAliveComThread,&rvalue);
        }; 
        CloseHandle(h_KeepAliveComThread);
    }
    lnp_stop=0;
    CloseHandle( comHandle );
    comHandle=NULL;
}


// returns true if communication tower is online, 
// this is to check if you have to run lnp_init
bool lnp_test(void)
{
   return ( comHandle != NULL );
}


static lnp_tx_result 
lnp_logical_write( unsigned char *data, int length )
{
    BOOL fWriteStat;
    DWORD nWritten = 0;

    if( ! connected ) 
    {
        return( TX_ERROR );
    }

    // block rcv signal
#if LNPBLOCK
    printf("write BLOCK\n");
#endif
    block_rcv();
#if LNPBLOCK
    printf("write BLOCKED\n");
#endif
    // send data
    fWriteStat = WriteFile( comHandle, 
                            data, 
                            (unsigned long)length,
                            &nWritten, 
                            NULL ); 
#if LNPBLOCK
    printf("write UNBLOCK\n");
#endif
    unblock_rcv();

    if( !fWriteStat ) 
    {
        printf("write error\n");
        return( TX_ERROR );
        // lnp_shutdown();
    }

    if( nWritten != (unsigned long) length ) 
    {
        printf("write error\n");
        return( TX_FAILURE );
        // lnp_shutdown();
    }

    // give incoming messages time to be read
    Sleep( AFTERWRITESLEEPTIME );

    return( TX_SUCCESS );
}   


lnp_tx_result 
lnp_integrity_write( const unsigned char *data,unsigned char length ) 
{
    lnp_buffer[0]=0xf0;
    lnp_buffer[1]=length;
    memcpy( lnp_buffer+2, data, length );
    lnp_buffer[length+2]=(unsigned char) lnp_checksum( lnp_buffer, length+2 );
#if DEBUGLNP
    printf("send broadcat package of length %d from this host 0x%02x ",
        length,lnp_host_address);
#endif
    return( lnp_logical_write( lnp_buffer, length+3 ) );
}


lnp_tx_result 
lnp_addressing_write( const unsigned char *data, 
                      unsigned char length,
                      unsigned char dest,
                      unsigned char srcport ) 
{
    lnp_buffer[0]=0xf1;
    lnp_buffer[1]=length+2;
    lnp_buffer[2]=dest;
    lnp_buffer[3]=lnp_host_address | (srcport & lnp_port_mask);
    memcpy( lnp_buffer+4, data, length );
    lnp_buffer[length+4]=(unsigned char) lnp_checksum( lnp_buffer, length+4 );
#if DEBUGLNP
    printf("send package of length %d to dest 0x%02x from this host 0x%02x from port 0x%x\n",
        length,dest,lnp_host_address,(srcport & lnp_port_mask));
#endif 
    return( lnp_logical_write( lnp_buffer, length+5 ) );
}


void 
lnp_integrity_set_handler( lnp_integrity_handler_t handler )
{
#if LNPBLOCK
    printf( "integrity handler BLOCK\n" );
#endif
    block_rcv();
#if LNPBLOCK
    printf( "integrity handler BLOCKED\n" );
#endif
    integrity_handler = handler;
#if LNPBLOCK
    printf( "integrity handler UNBLOCK\n" );
#endif
    unblock_rcv();
}


void 
lnp_addressing_set_handler( unsigned char port, 
                            lnp_addressing_handler_t handler )
{
#if LNPBLOCK
    printf( "addressing handler BLOCK\n" );
#endif
    block_rcv();
#if LNPBLOCK
    printf( "addressing handler BLOCKED\n" );
#endif
    addressing_handler[port] = handler;
#if LNPBLOCK
    printf( "addressing handler UNBLOCK\n" );
#endif
    unblock_rcv();
}



void 
lnp_receive_packet( const unsigned char *data ) 
{
    unsigned char header=*(data++);
    unsigned char length=*(data++);


    // only handle non-degenerate packets in boot protocol 0xf0
    //
    switch(header) 
    {
    case 0xf0:         // raw integrity layer packet, no addressing.
#if DEBUGLNP
        printf("send to integrity handler\n");
#endif 
        if( integrity_handler )           
        {
            integrity_handler(data,length);
        }
        break;
    case 0xf1:         // addressing layer.
        
#if DEBUGLNP
        printf("send to addressing handler for port %x if hostmatch %02x=%02x&%02x \n",
            (*data & lnp_port_mask),lnp_host_address ,*data , lnp_host_mask);
#endif
        if( length > 2 ) 
        {
            unsigned char dest=*(data++);
            if( lnp_host_address == (dest & lnp_host_mask)) 
            {
                unsigned char port=dest & lnp_port_mask;

                if(addressing_handler[port]) 
                {
                    unsigned char src=*(data++);
                    addressing_handler[port](data,length-2,src);
                }
            }
        }
    } // switch(header)
}



// return whether a packet is currently being received
static int 
lnp_integrity_active( void )
{
    return( lnp_integrity_state != LNPwaitHeader );
}



// reset the integrity layer on error or timeout.

static void 
lnp_integrity_reset( void )
{
    lnp_integrity_state=LNPwaitHeader;
}


// receive a byte, decoding LNP packets with a state machine.

void 
lnp_integrity_byte(unsigned char b)
{
    static unsigned char buffer[MAX_LNP_PACKET];
    static int bytesRead,endOfData;

    if( lnp_integrity_state == LNPwaitHeader )
    {
        bytesRead=0;
    }

    buffer[bytesRead++]=b;

    switch( lnp_integrity_state ) 
    {
    case LNPwaitHeader:
    // valid headers are 0xf0 .. 0xf7
    // if((b & (unsigned char) 0xf8) == (unsigned char) 0xf0)
    // more restricted : the only valid headers are 0xf0 and 0xf1
#if DEBUGLNP
        printf("header 0x%02x ",b);
#endif
    // b may only be 0xf0 or 0xf1 ( integrity resp adressing packet)
        if( (b & (unsigned char) 0xfe) == (unsigned char) 0xf0 )
        {
            lnp_integrity_state= LNPwaitLength ;

            // set stoptime for which the total packet must be read,
            // exceeding this time restarts reading on a new package
            stoptime = clock() + CLOCKS_PER_SEC * READTIMEOUT / 1000;
#if DEBUGLNP
            printf(" SETTING stoptime %ld at currentime %ld", stoptime,clock());
#endif
            // let com port be blocked -> do not unblock : do nothing
        } else {
            // unblock comport 
#if LNPBLOCK
            printf("read UNBLOCK\n");   
#endif
            unblock_rcv();
        }
        break;
    case LNPwaitLength:
        endOfData=b+2;
        lnp_integrity_state=LNPwaitData;
#if DEBUGLNP
        printf("\n  length 0x%02x \n",b);
        printf(" currentime %ld  stoptime %ld ",clock(),stoptime);
#endif
        break;

    case LNPwaitData:
        if( bytesRead == endOfData )
        {
            lnp_integrity_state=LNPwaitCRC;
        }
#if DEBUGLNP
        printf("data 0x%02x ",b);
        printf(" currentime %ld  stoptime %ld ",clock(),stoptime);
#endif
        break;

    case LNPwaitCRC:   
        if( b == (unsigned char)lnp_checksum(buffer,endOfData) )
        {
#if DEBUGLNP
            printf( "\n\nCORRECT checksum\n" );
            printf( "checksum %02x %d \n", b, b );
            printf( "stoptime-currenttime %d \n", (stoptime-clock()) );
            printf( "\nreceive packet\n");
#endif
#if LNPCHECKSOMSTOP
            getchar( );
#endif   
#if LNPBLOCK
            printf( "read UNBLOCK\n" );   
#endif
            unblock_rcv( );
            lnp_receive_packet( buffer );
        } else {
#if DEBUGLNP
            printf( "\n\nFALSE checksum\n" );
            printf( "checksum %02x %d != %02x %d \n", b, b, lnp_checksum( buffer, endOfData ), lnp_checksum( buffer, endOfData ) );
#endif   
#if LNPCHECKSOMSTOP
            getchar( );
#endif
#if LNPBLOCK
            printf( "read UNBLOCK\n" );   
#endif
            unblock_rcv( );
        }
        lnp_integrity_reset( );
    }
}


// Thread to constantly read out the COM port
static DWORD WINAPI 
ComReaderThread(LPVOID pParam)
{
    //unsigned char buffer[MAX_LNP_PACKET];
    unsigned char recvd_byte;
    int totalsleep=0;
    DWORD dwRead=0;
    BOOL fReadStat;
    while( ! lnp_stop ) // read per byte
    {
        //Sleep(5);
        if( lnp_integrity_state==LNPwaitHeader ) 
        {
#if LNPBLOCK
            printf("read header BLOCK\n");
#endif
            block_rcv();
#if LNPBLOCK
            printf("read header BLOCKED\n");
#endif
        }
        
        fReadStat = ReadFile( comHandle, 
                              &recvd_byte, 
                              1, 
                              &dwRead, 
                              NULL );
        
        // max sure parameters are reset at beginning new package
        if( lnp_integrity_state == LNPwaitHeader ) 
        {
            stoptime=-1;
            totalsleep=0;
        }

        if ( fReadStat && (dwRead == 1) ) 
        {
            lnp_integrity_byte( recvd_byte );
            totalsleep=0;
        } 

        if ( ! fReadStat ) 
        {
#if LNPBLOCK
            printf("read header UNBLOCK\n");
#endif
            unblock_rcv();


            //printf(" read error");
            // read error -> package probably corrupt, restart
            if( lnp_integrity_state != LNPwaitHeader ) 
            {
                lnp_integrity_state=LNPwaitHeader;
                totalsleep=0;
                unblock_rcv( );
            }
            Sleep( 50 );
            continue;
        }

        if( dwRead == 0 ) 
        {
            //Sleep(5000000);
            //  printf(" currentime %ld ",clock());
            if( lnp_integrity_state==LNPwaitHeader ) 
            {
#if LNPBLOCK
                printf( "read header UNBLOCK\n" );
#endif
                unblock_rcv( );

                // no packet yet arrived, -> sleep a while
                // -> active polling for packet every 50 milliseconds
                //    in this sleep time the program can do its thing
                Sleep( 50 );
#if DEBUGLNP
                printf( "sleep 50 , %d ", totalsleep );
#endif
                continue;
            } else {
                // we are in the process of reading a packet, what went
                // wrong ?
                // -> reading a packet to fast, that is we were finished 
                //    reading for the next byte arrived ( it it is possible?)
                //   (reading a byte timed out after 20 milliseconds 
                //    see CommTimeOuts.ReadTotalTimeoutConstant in lnp_init())
                // -> therefore we give the next byte extra time


                // when nothing is read this thread is waiting 5 milliseconds 
                // for the next byte to arrive  : this can be done 
                //  only for maximal 6 times = 5x6=30 milliseconds
                //  if passed :  reset to start reading a new package
                if( totalsleep > 30 ) 
                {
                    lnp_integrity_state=LNPwaitHeader;
                    totalsleep=0;
#if DEBUGLNP
                    printf( "\n\nREADING SLEEPED TOO LONG\n\n" );
#endif
                    unblock_rcv( );
                    Sleep( 50 );
                }

                // reading a package blocks the com port, the com port
                // may not be blocked longer than timeout milliseconds
                time_t currenttime;
                currenttime=clock( ); 

                //if((stoptime != -1 ) && (currenttime > stoptime))  {
                if(currenttime > stoptime)  
                {
                    // receiving message failed within timeout milliseconds
                    lnp_integrity_state= LNPwaitHeader;
                    // unblock com poort
#if DEBUGLNP
                    printf( "\n\nPACKET TAKES TOO LONG : currentime %d  stoptime %d \n\n", currenttime, stoptime );
#endif
#if LNPBLOCK
                    printf( "read timeout UNBLOCK\n" );
#endif
                    unblock_rcv( );      
                    Sleep( 50 );
                }

                // if all checks PASS : 
                // SLEEP for 5 milliseconds to wait for the next byte
#if DEBUGLNP
                printf( "sleep 5 , %d ", totalsleep );
#endif
                Sleep( 5 );            
                totalsleep=totalsleep+5;
                continue;
            }
        }     



        //   return(1); never stop the thread early (could be blocked!!!
    }

    ExitThread( 0 );
    // _endthread();
    return( 1 );
}




// Thread to keep COM active listening to infrared signals
static DWORD WINAPI 
KeepAliveComThread(LPVOID pParam)
{
    unsigned char cKeepAliveByte = TOWER_KEEP_ALIVE_CHAR;

    while ( ! lnp_stop )
    {      
#if DEBUGLNP
        printf( "\n\n   TRY to start wakeup tower   \n\n" );
#endif
        lnp_logical_write( &cKeepAliveByte, 1 );
#if DEBUGLNP
        printf( "\n\n   wakeup tower DONE  \n\n" );
#endif
        Sleep( (unsigned long)TOWER_KEEP_ALIVE_DELAY );
    }
    //_endthread();
    ExitThread( 0 );   
    return( 1 );
}


static int 
lnp_init_com()
{
    //   HANDLE comHandle;    global defined   //  communication port file handle
    COMMTIMEOUTS CommTimeOuts;   // timeout settings for communication port
    DCB dcb;                   // device control block to setup communication port       
    BOOL fRetVal;  // some return values from system calls

    // open COMM device
    if( ( comHandle = CreateFile( ComPort, 
                                  GENERIC_READ | GENERIC_WRITE,
                                  0,                    // exclusive access
                                  NULL,                 // no security attrs
                                  OPEN_EXISTING,
                                  FILE_ATTRIBUTE_NORMAL, // |
                                  NULL ) ) == INVALID_HANDLE_VALUE )
    {
        printf( "Cannot open communication port" );
        comHandle=NULL;
        return( INIT_ERROR );
    }

    // setup device : size of input en output buffer
    SetupComm( comHandle, 4096, 4096 );

    // purge any information in the buffer : reset buffers for fresh start
    PurgeComm( comHandle, 
               PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR );

    //  setting the comm port to receive separate bytes one for one : 
    //   - read a byte within timeout of 20 ms
    //   - write a package (set of bytes) within timeout of 2000 ms

    // set up timeouts for serial port
    CommTimeOuts.ReadIntervalTimeout = 0;  // only read per byte   
    CommTimeOuts.ReadTotalTimeoutMultiplier = 0;  //  only read per byte
    CommTimeOuts.ReadTotalTimeoutConstant = 20;  //   reading a byte may take maximal 20 ms
    CommTimeOuts.WriteTotalTimeoutMultiplier = 0; //  only read per byte
    CommTimeOuts.WriteTotalTimeoutConstant = 2000;//  writing a packet (bytestring) may take maximal 2000 ms 
    SetCommTimeouts(comHandle, &CommTimeOuts ) ;

    // communication parameters
    dcb.DCBlength = sizeof( DCB ) ;
    // assign dcb pointer to device control block of comport
    GetCommState( comHandle, &dcb ); 

    // set the parameters for dcb
    dcb.BaudRate = BAUDRATE;
    dcb.ByteSize = BYTESIZE;
    dcb.Parity   = PARITY;
    dcb.StopBits = STOPBITS;

    fRetVal = SetCommState( comHandle, &dcb );
    
         // when activating the next line will probably disable
    // echoing on a tower on a com port  (however nobody is 
    // implementing a wait handler for the events this line
    // will disable? -> probably nothing to do with echo
    //
         //  however I'm pretty sure this is not going to
    // disable echoing because echoing is done by the tower's
    // hardware and is not done by the local serial driver
         // on the pc (turning echoing of is saying to serial
         // driver to turn of echoing)
         // ( tower on usb port
    //  never does this echoing -> usb is already good)
 //SetCommMask(comHandle,0) // no more events, terminate pending waits 
    
         EscapeCommFunction( comHandle, SETDTR );

    if ( ! fRetVal )
    {
        CloseHandle( comHandle );
        comHandle=NULL;
        printf( "SetCommState()\n" );
        return( INIT_ERROR );
    }

    return( INIT_OK );
}

static int 
lnp_init_usb()
{

    // open COMM device
    if( ( comHandle = CreateFile( USBPort, 
                                  GENERIC_ALL,
                                  0,                    // exclusive access
                                  NULL,                 // no security attrs
                                  OPEN_EXISTING,
                                  FILE_ATTRIBUTE_NORMAL, // |
                                  NULL ) ) == INVALID_HANDLE_VALUE )
    {
        printf( "Cannot open communication port" );
        comHandle=NULL;
        return( INIT_ERROR );
    }

    return( INIT_OK );
}




static int 
lnp_init_threads( )
{
    DWORD dwThreadID;

    // Only use KeepAlive on Serial Tower
    if( ! UseUSB )
    {
        h_KeepAliveComThread = CreateThread( NULL,
                                             0,
                                             KeepAliveComThread,
                                             0,
                                             NULL,
                                             &dwThreadID );

        if( !h_KeepAliveComThread )
        {
            return( INIT_ERROR );
        }
        
        SetThreadPriority( h_KeepAliveComThread, THREAD_PRIORITY_HIGHEST );
        Sleep( 1000 );
    }

    h_ComReaderThread = CreateThread( NULL,
                                      0,
                                      ComReaderThread,
                                      0,
                                      NULL,
                                      &dwThreadID );   
    
    if (!h_ComReaderThread) 
    {
        return( INIT_ERROR );
    }

    SetThreadPriority( h_ComReaderThread, THREAD_PRIORITY_ABOVE_NORMAL );
    
    return( INIT_OK );
}



lnp_init_result 
lnp_init ( char *tcp_hostname, 
           unsigned short tcp_port,
           unsigned char lnp_address, 
           unsigned char lnp_mask, 
           int flags )
{   
    int returnvalue;
    if( lnp_stop ) 
    { 
        return( INIT_ERROR );
    }
     

        // check if initialization is already done

    if ( lnp_test() )

        {

                return( INIT_OK );

        };


    lnp_host_address = lnp_address ? lnp_address : DEFAULT_LNP_ADDR;
    lnp_host_mask = lnp_mask ? lnp_mask : DEFAULT_LNP_MASK;
    lnp_port_mask = lnp_host_mask ^ 0xFF;
    if( ( lnp_host_address & lnp_host_mask ) != lnp_host_address )
    {
        return( INIT_BAD_PARAM );
    }

    lnp_integrity_state=LNPwaitHeader;
    connected = 1;

    // determine USB or COM
    char *rcxtty = getenv( "RCXTTY" );

    UseUSB = ( strcmpi( rcxtty, "COM" ) != 0 );
    
        
    if( UseUSB )
    {
        returnvalue = lnp_init_usb( );
    } else {
        returnvalue = lnp_init_com( );
    }


    // if port not connected
    if( returnvalue != INIT_OK )
    {
        lnp_shutdown( );
        return( INIT_ERROR );
    }

    returnvalue = lnp_init_threads( );

    // if threads not started

    if( returnvalue != INIT_OK )
    {
        lnp_shutdown();
        return( INIT_ERROR );
    }

    return( INIT_OK );
}

---