src/slipBio.cpp

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