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