src/io/slipBio.cpp

   1 #include "slipBio.h"
   2
   3 #include <unistd.h>
   4
   5 #include <iostream>
   6
   7 #include "log/logger.hpp"
   8
   9 static constexpr std::size_t buffer_size = 8192;
  10
  11 #define SLIP_ESCAPE_CHAR ( (char) 0xDB)
  12 #define SLIP_CONNECTION ( (char) 0xC0)
  13 #define SLIP_RESET ( (char) 0xCB )
  14
  15 #define SLIP_IO_DEBUG
  16
  17 char hexDigit( char c ) {
  18     if( c < 0 ) {
  19         return 'x';
  20     }
  21
  22     if( c < 10 ) {
  23         return '0' + c;
  24     }
  25
  26     if( c < 16 ) {
  27         return 'A' + c - 10;
  28     }
  29
  30     return 'x';
  31 }
  32
  33 std::string toHex( const char* buf, int len ) {
  34     std::string data = "000000";
  35
  36     for( int i = 0; i < len; i++ ) {
  37         data.append( 1, ' ' );
  38         data.append( 1, hexDigit( ( buf[i] >> 4 ) & 0xF ) );
  39         data.append( 1, hexDigit( buf[i] & 0xF ) );
  40     }
  41
  42     return data;
  43 }
  44
  45 SlipBIO::SlipBIO() : buffer( std::vector<char>( buffer_size ) ), decodeTarget( 0 ), decodePos( 0 ), rawPos( 0 ) {
  46 }
  47
  48 void SlipBIO::setTarget( std::shared_ptr<OpensslBIO> target, bool server ) {
  49     this->target = target;
  50     this->server = server;
  51 }
  52
  53 SlipBIO::SlipBIO( std::shared_ptr<OpensslBIO> target ) : target( target ), buffer( std::vector<char>( buffer_size ) ), decodeTarget( 0 ), decodePos( 0 ), rawPos( 0 ) {
  54 }
  55
  56 SlipBIO::~SlipBIO() {}
  57
  58 int SlipBIO::write( const char* buf, int num ) {
  59 #ifdef SLIP_IO_DEBUG
  60     logger::notef( "Out: %s", toHex( buf, num ) );
  61 #endif
  62
  63     int badOnes = 0;
  64
  65     for( int i = 0; i < num; i++ ) {
  66         if( ( buf[i] == SLIP_CONNECTION ) || ( buf[i] == SLIP_ESCAPE_CHAR ) ) {
  67             badOnes++;
  68         }
  69     }
  70
  71     int totalLen = num + badOnes; // 2
  72     char* targetPtr = ( char* ) malloc( totalLen );
  73
  74     if( !targetPtr ) {
  75         return -1;
  76     }
  77
  78     std::shared_ptr<char> t = std::shared_ptr<char>( targetPtr, free );
  79     int j = 0;
  80
  81     for( int i = 0; i < num; i++ ) {
  82         if( buf[i] == SLIP_CONNECTION ) {
  83             targetPtr[j++] = SLIP_ESCAPE_CHAR;
  84             targetPtr[j++] = ( char )0xDC;
  85         } else if( buf[i] == SLIP_ESCAPE_CHAR ) {
  86             targetPtr[j++] = SLIP_ESCAPE_CHAR;
  87             targetPtr[j++] = ( char )0xDD;
  88         } else {
  89             targetPtr[j++] = buf[i];
  90         }
  91     }
  92
  93     int sent = 0;
  94
  95     while( sent < j ) {
  96
  97         errno = 0;
  98         int dlen = target->write( targetPtr + sent, std::min( 1024, j - sent ) );
  99         std::ostringstream debug;
 100         debug << "Wrote " << dlen << " bytes ";
 101         debug << toHex( targetPtr + sent, dlen );
 102         logger::note( debug.str() );
 103         target->ctrl( BIO_CTRL_FLUSH, 0, NULL );
 104
 105         if( dlen < 0 ) {
 106             throw "Error, target write failed";
 107         } else if( dlen == 0 ) {
 108             // sleep
 109             logger::note( "waiting for write ability" );
 110             usleep( 50000 );
 111         }
 112
 113         if( errno != 0 ) {
 114             perror( "Error" );
 115         }
 116
 117         sent += dlen;
 118     }
 119
 120     return num;
 121 }
 122
 123 int SlipBIO::read( char* buf, int size ) {
 124     logger::note( "starting read" );
 125     // while we have no data to decode or unmasking does not yield a full package
 126     while( decodeTarget == 0 ) {
 127         if( waitForReset ) {
 128             logger::note( "denying read because of reset-need!" );
 129             return -1;
 130         }
 131         if(decodePos < rawPos) {
 132             int res = unmask();
 133             if( res == 1 ) {
 134                 continue; // probably Packet :-)
 135             } else if(res == -1) {
 136                 logger::note( "sending reset because of malfomed packet" );
 137                return -1;
 138             }
 139         }
 140         if( decodeTarget != 0 ){
 141             // we have data now, emit it!
 142             break;
 143         }
 144         // we have no data, read more
 145         if( buffer.size() - rawPos < 64 ) {
 146             // not enough space... package is too big
 147             decodeTarget = 0;
 148             waitForConnection = true;
 149             waitForReset = true;
 150             resetCounter = -1;
 151             return -1;
 152         }
 153
 154         logger::note( "beginning read" );
 155         std::ostringstream converter;
 156         converter << "rawPos is now: " << rawPos << ", buffer.size():" << buffer.size();
 157         logger::note( converter.str() );
 158         int len = target->read( buffer.data() + rawPos, buffer.size() - rawPos );
 159         logger::note( toHex(buffer.data() + rawPos, len ) );
 160
 161         if( len > 0 ) {
 162             rawPos += len;
 163         } else {
 164             logger::note("Reporting EOS from slip");
 165             return -1;
 166             //decodeTarget = 0;
 167             //failed = true;
 168         }
 169
 170     }
 171     if( waitForReset ) return -1;
 172     logger::note( "emitting data!" );
 173
 174     int len = std::min( decodeTarget, ( unsigned int ) size );
 175     // a package finished, return it
 176     std::copy( buffer.data(), buffer.data() + len, buf );
 177     // move the buffer contents back
 178     std::copy( buffer.data() + len, buffer.data() + decodeTarget, buffer.data() );
 179     decodeTarget -= len;
 180     std::ostringstream convert;
 181     convert << "decodeTarget: " << decodeTarget << ", rawPos: " << rawPos << ", decodePos: " << decodePos;
 182     convert << ", requested were: " << size;
 183     logger::note( convert.str() );
 184
 185     if(decodeTarget == 0 && rawPos <= decodePos + 1){
 186         // compact the remaining at most 1 byte of raw data
 187         buffer[0] = buffer[decodePos];
 188         rawPos -= decodePos;
 189         decodePos = 0;
 190     }
 191
 192 #ifdef SLIP_IO_DEBUG
 193     logger::notef( "in: %s", toHex( buf, len ) );
 194 #endif
 195
 196     return len;
 197 }
 198
 199 long SlipBIO::ctrl( int cmod, long arg1, void* arg2 ) {
 200     ( void ) cmod;
 201     ( void ) arg1;
 202     ( void ) arg2;
 203
 204     if( cmod == BIO_CTRL_RESET ) {
 205         decodePos = 0;
 206         if( server ) {
 207             waitForReset = false;
 208             waitForConnection = true;
 209             resetCounter = -1;
 210         } else {
 211             static char ctr = 8;
 212             char resetSequence[] = {SLIP_CONNECTION, 1,2,3,4,5,6,7, ctr};
 213             target->write( resetSequence, 9 );
 214             logger::note( "wrote 9-byte reset seq" );
 215             header = {1, 2, 3, 4, 5, 6, 7, ctr};
 216             resetCounter = -1;
 217             waitForConnection = true;
 218             logger::note( "Resetting SLIP layer" );
 219         }
 220         return 0;
 221     }else if(cmod == BIO_CTRL_FLUSH ){
 222         logger::note( "flush requested ");
 223     }
 224
 225     return target->ctrl( cmod, arg1, arg2 );
 226 }
 227
 228 const char* SlipBIO::getName() {
 229     return "SlipBIO";
 230 }
 231
 232 // 1 success, data avail, 0 need moar data (see that decodeTarget is still 0),
 233 // -1: fail... connection needs resetting
 234 int SlipBIO::unmask() {
 235     {
 236         std::ostringstream conv;
 237         conv << "unmasking starting, decodeTarget: " << decodeTarget << " decodePos: " << decodePos << " rawPos: " << rawPos << "bytes stored";
 238         logger::note( conv.str() );
 239     }
 240     logger::note( "unmasking" );
 241     if( waitForConnection ){
 242         logger::note( "scanning for connection" );
 243         decodeTarget = 0;
 244         if( server ) {
 245             logger::note( "on server site, waiting for CONNECTION-byte");
 246             while(decodePos < rawPos) {
 247                 if(buffer[decodePos] == SLIP_CONNECTION) {
 248                     resetCounter = 0;
 249                     logger::note( "got connection byte" );
 250                 } else if(resetCounter >= 0) {
 251                     header[resetCounter] = buffer[decodePos];
 252                     resetCounter++;
 253                 }
 254                 decodePos++;
 255                 if( resetCounter >= ((int) header.size()) ){
 256                     waitForConnection = false;
 257                     char data[] = { SLIP_CONNECTION };
 258                     target->write( data, 1);
 259                     logger::notef( "SLIP, initing connection with ping-seq %s:", toHex(header.data(), header.size()) );
 260                     target->write( header.data(), header.size() );
 261                     break;
 262                 }
 263             }
 264             if( decodePos >= rawPos ){
 265                 decodePos = 0;
 266                 rawPos = 0;
 267                 return 0; // no package
 268             }
 269
 270         } else {
 271             while(decodePos < rawPos) {
 272                 if(buffer[decodePos] == SLIP_CONNECTION) {
 273                     logger::note( "got connbyte" );
 274                     resetCounter = 0;
 275                 } else if(resetCounter >= 0) {
 276                     logger::note( "got head-byte" );
 277                     if(buffer[decodePos] == header[resetCounter]) {
 278                         logger::note( "thats correct!!" );
 279                         resetCounter++;
 280                     } else {
 281                         resetCounter = -1;
 282                     }
 283                 }
 284                 decodePos++;
 285                 if( resetCounter >= ((int) header.size()) ){
 286                     waitForConnection = false;
 287                     logger::note("connection found! :-)!");
 288                     break;
 289                 }
 290             }
 291             if( decodePos >= rawPos ){
 292                 rawPos = 0;
 293                 decodePos = 0;
 294                 return 0; // no package
 295             }
 296         }
 297     }
 298     unsigned int j = decodeTarget;
 299
 300     for( unsigned int i = decodePos; i < rawPos; i++ ) {
 301         if(waitForConnection && buffer[i] != SLIP_CONNECTION ) {
 302             continue;
 303         }
 304         if( buffer[i] == SLIP_ESCAPE_CHAR ) {
 305             i++;
 306
 307             if( i >= rawPos ) {
 308                 decodeTarget = j;
 309                 buffer[decodeTarget] = buffer[i - 1];
 310                 decodePos = decodeTarget;
 311                 rawPos = decodePos + 1;
 312                 return 0;// no packet
 313             } else if( buffer[i] == ( char )0xdc ) {
 314                 buffer[j++] = SLIP_CONNECTION;
 315             } else if( buffer[i] == ( char )0xdd ) {
 316                 buffer[j++] = SLIP_ESCAPE_CHAR;
 317             } else if( buffer[i] == SLIP_ESCAPE_CHAR
 318                        || buffer[i] == SLIP_CONNECTION ) {
 319                 i--;
 320                 continue;
 321             } else {
 322                 waitForReset = true;
 323                 resetCounter = -1;
 324                 waitForConnection = true;
 325                 decodeTarget = 0;
 326                 decodePos = i + 1;
 327                 // failed package
 328                 // error
 329                 return -1; // we don't have a pkg, set all appropriately to wait for a pkg start for next pkg.
 330             }
 331         } else if( buffer[i] == SLIP_CONNECTION ) {
 332             decodePos = i;
 333             decodeTarget = j;
 334
 335             // copy rest to bufferfer i to len
 336             if( !waitForConnection ) {
 337                 waitForReset = true;
 338                 resetCounter = -1;
 339                 waitForConnection = true;
 340                 decodeTarget = 0;
 341                 decodePos = i;
 342                 logger::note( "error connection failed" );
 343                 return -1;
 344             }
 345             logger::note( "got package border; slip re-validated SHOULD NEVER HAPPEN!!" );
 346             decodeTarget = 0;
 347             waitForConnection = false;
 348         } else {
 349             buffer[j++] = buffer[i];
 350         }
 351     }
 352
 353     std::ostringstream conv;
 354     conv << "unmasking paused, 0 remaining, " << j << "bytes stored";
 355     logger::note( conv.str() );
 356     decodePos = j;
 357     rawPos = j;
 358     decodeTarget = j;
 359     return decodeTarget > 0;
 360 }