fly/flightgear
1
0
Fork 0
Code Activity

generic protocol: don't rely on GNU extension

Browse source
This commit is contained in:
Torsten Dreyer 2013-10-22 14:17:35 +02:00
parent 47ecc0ce29
commit 6b75b23309
1 changed files with 39 additions and 40 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

77
src/Network/generic.cxx
View file

@ -61,47 +61,46 @@ public:
virtual int wrap( size_t n, uint8_t * buf ); virtual int wrap( size_t n, uint8_t * buf );
virtual int unwrap( size_t n, uint8_t * buf ); virtual int unwrap( size_t n, uint8_t * buf );
private: private:
static const uint8_t FEND = 0xC0; static const uint8_t FEND;
static const uint8_t FESC = 0xDB; static const uint8_t FESC;
static const uint8_t TFEND = 0xDC; static const uint8_t TFEND;
static const uint8_t TFESC = 0xDD; static const uint8_t TFESC;
}; };
const uint8_t FGKissWrapper::FEND = 0xC0;
const uint8_t FGKissWrapper::FESC = 0xDB;
const uint8_t FGKissWrapper::TFEND = 0xDC;
const uint8_t FGKissWrapper::TFESC = 0xDD;
int FGKissWrapper::wrap( size_t n, uint8_t * buf ) int FGKissWrapper::wrap( size_t n, uint8_t * buf )
{ {
uint8_t dest[2*n+3]; // worst case, FEND+command+all bytes escaped+FEND std::vector<uint8_t> dest;
uint8_t *sp = buf, *dp = dest; uint8_t *sp = buf;
*dp++ = FEND; dest.push_back(FEND);
*dp++ = 0; // command/channel always zero dest.push_back(0); // command/channel always zero
for( size_t i = 0; i < n; i++ ) { for( size_t i = 0; i < n; i++ ) {
uint8_t c = *sp++; uint8_t c = *sp++;
switch( c ) { switch( c ) {
case FESC: case FESC:
*dp++ = FESC; dest.push_back(FESC);
*dp++ = TFESC; dest.push_back(TFESC);
break; break;
case FEND: case FEND:
*dp++ = FESC; dest.push_back(FESC);
*dp++ = TFEND; dest.push_back(TFEND);
break; break;
default: default:
*dp++ = c; dest.push_back(c);
break; break;
} }
} }
*dp++ = FEND; dest.push_back(FEND);
// copy the result to the input buffer memcpy( buf, dest.data(), dest.size() );
// and count the buffer size return dest.size();
int reply = 0;
for( sp = dest; sp < dp; ) {
*buf++ = *sp++;
reply++;
}
return reply;
} }
int FGKissWrapper::unwrap( size_t n, uint8_t * buf ) int FGKissWrapper::unwrap( size_t n, uint8_t * buf )
@ -189,40 +188,40 @@ public:
virtual int wrap( size_t n, uint8_t * buf ); virtual int wrap( size_t n, uint8_t * buf );
virtual int unwrap( size_t n, uint8_t * buf ); virtual int unwrap( size_t n, uint8_t * buf );
static const uint8_t STX = 0x02; static const uint8_t STX;
static const uint8_t ETX = 0x03; static const uint8_t ETX;
static const uint8_t DLE = 0x00; static const uint8_t DLE;
}; };
const uint8_t FGSTXETXWrapper::STX = 0x02;
const uint8_t FGSTXETXWrapper::ETX = 0x03;
const uint8_t FGSTXETXWrapper::DLE = 0x00;
int FGSTXETXWrapper::wrap( size_t n, uint8_t * buf ) int FGSTXETXWrapper::wrap( size_t n, uint8_t * buf )
{ {
// stuff payload as // stuff payload as
// <dle><stx>payload<dle><etx> // <dle><stx>payload<dle><etx>
// if payload contains <dle>, stuff <dle> as <dle><dle> // if payload contains <dle>, stuff <dle> as <dle><dle>
uint8_t dest[2*n+4*sizeof(uint8_t)]; // worst case, all payload is dle plus header plus footer std::vector<uint8_t> dest;
uint8_t *sp = buf, *dp = dest; uint8_t *sp = buf;
*dp++ = DLE; dest.push_back(DLE);
*dp++ = STX; dest.push_back(STX);
while( n > 0 ) { while( n > 0 ) {
n--; n--;
if( DLE == *sp ) if( DLE == *sp )
*dp++ = DLE; dest.push_back(DLE);
*dp++ = *sp++; dest.push_back(*sp++);
} }
*dp++ = DLE; dest.push_back(DLE);
*dp++ = ETX; dest.push_back(ETX);
for( n = 0, sp = dest; sp < dp; ) { memcpy( buf, dest.data(), dest.size() );
n++; return dest.size();
*buf++ = *sp++;
}
return n;
} }
int FGSTXETXWrapper::unwrap( size_t n, uint8_t * buf ) int FGSTXETXWrapper::unwrap( size_t n, uint8_t * buf )