src/slipBio.cpp

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