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