src/io/slipBio.cpp

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