src/io/slipBio.cpp

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