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