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