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 }
  41
  42 void SlipBIO::setTarget( std::shared_ptr<OpensslBIO> target ) {
  43     this->target = target;
  44 }
  45
  46 SlipBIO::SlipBIO( std::shared_ptr<OpensslBIO> target ) {
  47     this->target = target;
  48     this->buffer = std::vector<char>( 4096 );
  49     this->decodeTarget = 0;
  50     this->decodePos = 0;
  51     this->rawPos = 0;
  52
  53     this->failed = false;
  54 }
  55
  56 SlipBIO::~SlipBIO() {}
  57
  58 int SlipBIO::write( const char* buf, int num ) {
  59     int badOnes = 0;
  60
  61     for( int i = 0; i < num; i++ ) {
  62         if( ( buf[i] == ( char )0xc0 ) || ( buf[i] == ( char )0xDB ) ) {
  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     //targetPtr[j++] = (char)0xC0;
  78
  79     for( int i = 0; i < num; i++ ) {
  80         if( buf[i] == ( char )0xc0 ) {
  81             targetPtr[j++] = ( char )0xDB;
  82             targetPtr[j++] = ( char )0xDC;
  83         } else if( buf[i] == ( char )0xDB ) {
  84             targetPtr[j++] = ( char )0xDB;
  85             targetPtr[j++] = ( char )0xDD;
  86         } else {
  87             targetPtr[j++] = buf[i];
  88         }
  89     }
  90
  91     targetPtr[j++] = ( char )0xC0;
  92     int sent = 0;
  93
  94     while( sent < j ) {
  95
  96         errno = 0;
  97         int dlen = target->write( targetPtr + sent, std::min( 1024, j - sent ) );
  98
  99         if( dlen < 0 ) {
 100             throw "Error, target write failed";
 101         } else if( dlen == 0 ) {
 102             // sleep
 103             usleep( 50000 );
 104         }
 105
 106         if( errno != 0 ) {
 107             perror( "Error" );
 108         }
 109
 110         sent += dlen;
 111     }
 112
 113     return num;
 114 }
 115
 116 int SlipBIO::read( char* buf, int size ) {
 117     // while we have no data to decode or unmasking does not yield a full package
 118     while( !packageLeft && ( decodePos >= rawPos || !unmask() ) ) {
 119
 120         // we have no data, read more
 121         if( buffer.size() - rawPos < 64 ) {
 122             // not enough space... package is too big
 123             decodeTarget = 0;
 124             failed = true;
 125         }
 126
 127         int len = target->read( buffer.data() + rawPos, buffer.capacity() - rawPos );
 128
 129         if( len > 0 ) {
 130             rawPos += len;
 131         } else {
 132             return -1;
 133             //decodeTarget = 0;
 134             //failed = true;
 135         }
 136
 137     }
 138
 139     packageLeft = true;
 140     int len = std::min( decodeTarget, ( unsigned int ) size );
 141     // a package finished, return it
 142     std::copy( buffer.data(), buffer.data() + len, buf );
 143     // move the buffer contents back
 144     std::copy( buffer.data() + len, buffer.data() + decodeTarget, buffer.data() );
 145     decodeTarget -= len;
 146
 147     if( decodeTarget == 0 ) {
 148         packageLeft = false;
 149     }
 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     std::cout << "SLIP crtl: " << cmod << std::endl;
 159     return target->ctrl( cmod, arg1, arg2 );
 160 }
 161
 162 const char* SlipBIO::getName() {
 163     return "SlipBIO";
 164 }
 165
 166 bool SlipBIO::unmask() {
 167     unsigned int j = decodeTarget;
 168
 169     for( unsigned int i = decodePos; i < rawPos; i++ ) {
 170         if( buffer[i] == ( char ) 0xDB ) {
 171             i++;
 172
 173             if( i >= rawPos ) {
 174                 decodeTarget = j;
 175                 buffer[decodeTarget] = buffer[i - 1];
 176                 decodePos = decodeTarget;
 177                 rawPos = decodePos + 1;
 178                 return 0;// no packet
 179             } else if( buffer[i] == ( char )0xdc ) {
 180                 buffer[j++] = ( char ) 0xc0;
 181             } else if( buffer[i] == ( char )0xdd ) {
 182                 buffer[j++] = ( char ) 0xdb;
 183             } else {
 184                 decodeTarget = 0;
 185                 failed = true;
 186                 // failed package
 187                 // error
 188             }
 189         } else if( buffer[i] == ( char ) 0xc0 ) {
 190             decodePos = i + 1;
 191             decodeTarget = j;
 192
 193             // copy rest to bufferfer i to len
 194             if( !failed ) {
 195                 return 1;
 196             }
 197
 198             decodeTarget = 0;
 199             failed = false;
 200         } else {
 201             buffer[j++] = buffer[i];
 202         }
 203     }
 204
 205     decodePos = j;
 206     rawPos = j;
 207     decodeTarget = j;
 208
 209     return 0;
 210 }