src/io/slipBio.cpp

   1 #include "slipBio.h"
   2
   3 #include <unistd.h>
   4
   5 #include <iostream>
   6
   7 #include "log/logger.hpp"
   8
   9 #define BUFFER_SIZE 8192
  10
  11 #define SLIP_ESCAPE_CHAR ( (char) 0xDB)
  12 #define SLIP_PACKET ( (char) 0xC0)
  13
  14 char hexDigit( char c ) {
  15     if( c < 0 ) {
  16         return 'x';
  17     }
  18
  19     if( c < 10 ) {
  20         return '0' + c;
  21     }
  22
  23     if( c < 16 ) {
  24         return 'A' + c - 10;
  25     }
  26
  27     return 'x';
  28 }
  29
  30 std::string toHex( const char* buf, int len ) {
  31     std::string data = "000000";
  32
  33     for( int i = 0; i < len; i++ ) {
  34         data.append( 1, ' ' );
  35         data.append( 1, hexDigit( ( buf[i] >> 4 ) & 0xF ) );
  36         data.append( 1, hexDigit( buf[i] & 0xF ) );
  37     }
  38
  39     return data;
  40 }
  41
  42 SlipBIO::SlipBIO() : buffer( std::vector<char>( BUFFER_SIZE ) ), decodeTarget( 0 ), decodePos( 0 ), rawPos( 0 ), failed( false ) {
  43 }
  44
  45 void SlipBIO::setTarget( std::shared_ptr<OpensslBIO> target ) {
  46     this->target = target;
  47 }
  48
  49 SlipBIO::SlipBIO( std::shared_ptr<OpensslBIO> target ) : target( target ), buffer( std::vector<char>( BUFFER_SIZE ) ), decodeTarget( 0 ), decodePos( 0 ), rawPos( 0 ), failed( false ) {
  50 }
  51
  52 SlipBIO::~SlipBIO() {}
  53
  54 int SlipBIO::write( const char* buf, int num ) {
  55 #ifdef SLIP_IO_DEBUG
  56     logger::debug( "Out: " << toHex( buf, num ) );
  57 #endif
  58
  59     int badOnes = 0;
  60
  61     for( int i = 0; i < num; i++ ) {
  62         if( ( buf[i] == SLIP_PACKET ) || ( buf[i] == SLIP_ESCAPE_CHAR ) ) {
  63             badOnes++;
  64         }
  65     }
  66
  67     int totalLen = num + badOnes + 1; // 2
  68     char* targetPtr = ( char* ) malloc( totalLen );
  69
  70     if( !targetPtr ) {
  71         return -1;
  72     }
  73
  74     std::shared_ptr<char> t = std::shared_ptr<char>( targetPtr, free );
  75     int j = 0;
  76
  77     for( int i = 0; i < num; i++ ) {
  78         if( buf[i] == SLIP_PACKET ) {
  79             targetPtr[j++] = SLIP_ESCAPE_CHAR;
  80             targetPtr[j++] = ( char )0xDC;
  81         } else if( buf[i] == SLIP_ESCAPE_CHAR ) {
  82             targetPtr[j++] = SLIP_ESCAPE_CHAR;
  83             targetPtr[j++] = ( char )0xDD;
  84         } else {
  85             targetPtr[j++] = buf[i];
  86         }
  87     }
  88
  89     targetPtr[j++] = SLIP_PACKET;
  90     int sent = 0;
  91
  92     while( sent < j ) {
  93
  94         errno = 0;
  95         int dlen = target->write( targetPtr + sent, std::min( 1024, j - sent ) );
  96
  97         if( dlen < 0 ) {
  98             throw "Error, target write failed";
  99         } else if( dlen == 0 ) {
 100             // sleep
 101             usleep( 50000 );
 102         }
 103
 104         if( errno != 0 ) {
 105             perror( "Error" );
 106         }
 107
 108         sent += dlen;
 109     }
 110
 111     return num;
 112 }
 113
 114 int SlipBIO::read( char* buf, int size ) {
 115     // while we have no data to decode or unmasking does not yield a full package
 116     while( !packageLeft && ( decodePos >= rawPos || !unmask() ) ) {
 117
 118         // we have no data, read more
 119         if( buffer.size() - rawPos < 64 ) {
 120             // not enough space... package is too big
 121             decodeTarget = 0;
 122             failed = true;
 123         }
 124
 125         int len = target->read( buffer.data() + rawPos, buffer.capacity() - rawPos );
 126
 127         if( len > 0 ) {
 128             rawPos += len;
 129         } else {
 130             return -1;
 131             //decodeTarget = 0;
 132             //failed = true;
 133         }
 134
 135     }
 136
 137     packageLeft = true;
 138     int len = std::min( decodeTarget, ( unsigned int ) size );
 139     // a package finished, return it
 140     std::copy( buffer.data(), buffer.data() + len, buf );
 141     // move the buffer contents back
 142     std::copy( buffer.data() + len, buffer.data() + decodeTarget, buffer.data() );
 143     decodeTarget -= len;
 144
 145     if( decodeTarget == 0 ) {
 146         packageLeft = false;
 147     }
 148
 149 #ifdef SLIP_IO_DEBUG
 150     logger::debug( "in: " << toHex( buf, len ) );
 151 #endif
 152
 153     return len;
 154 }
 155
 156 long SlipBIO::ctrl( int cmod, long arg1, void* arg2 ) {
 157     ( void ) cmod;
 158     ( void ) arg1;
 159     ( void ) arg2;
 160
 161     if( cmod == BIO_CTRL_RESET ) {
 162         char resetSequence[] = {SLIP_ESCAPE_CHAR, 0, SLIP_PACKET};
 163         target->write( resetSequence, 3 );
 164         decodePos = 0;
 165         decodeTarget = 0;
 166         rawPos = 0;
 167         logger::note( "Resetting SLIP layer" );
 168         return 0;
 169     }
 170
 171     return target->ctrl( cmod, arg1, arg2 );
 172 }
 173
 174 const char* SlipBIO::getName() {
 175     return "SlipBIO";
 176 }
 177
 178 bool SlipBIO::unmask() {
 179     unsigned int j = decodeTarget;
 180
 181     for( unsigned int i = decodePos; i < rawPos; i++ ) {
 182         if( buffer[i] == SLIP_ESCAPE_CHAR ) {
 183             i++;
 184
 185             if( i >= rawPos ) {
 186                 decodeTarget = j;
 187                 buffer[decodeTarget] = buffer[i - 1];
 188                 decodePos = decodeTarget;
 189                 rawPos = decodePos + 1;
 190                 return 0;// no packet
 191             } else if( buffer[i] == ( char )0xdc ) {
 192                 buffer[j++] = SLIP_PACKET;
 193             } else if( buffer[i] == ( char )0xdd ) {
 194                 buffer[j++] = SLIP_ESCAPE_CHAR;
 195             } else if( buffer[i] == SLIP_PACKET ) {
 196                 failed = true;
 197                 i--;
 198                 continue;
 199             } else {
 200                 decodeTarget = 0;
 201                 failed = true;
 202                 // failed package
 203                 // error
 204             }
 205         } else if( buffer[i] == SLIP_PACKET ) {
 206             decodePos = i + 1;
 207             decodeTarget = j;
 208
 209             // copy rest to bufferfer i to len
 210             if( !failed ) {
 211                 return 1;
 212             }
 213
 214             decodeTarget = 0;
 215             failed = false;
 216         } else {
 217             buffer[j++] = buffer[i];
 218         }
 219     }
 220
 221     decodePos = j;
 222     rawPos = j;
 223     decodeTarget = j;
 224     return 0;
 225 }