X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FNetwork%2Fgeneric.cxx;h=eebc6de8ac8912e5d280f21afa4453435fe3f26d;hb=46ec7a6ff78b0718c6eb09e5abb45c67c153d2f5;hp=9ab7d76999e13597ebf7336a16fe9a0bfd3b9504;hpb=5a7f838ff297776fe398134a28f721e018efbfe1;p=flightgear.git diff --git a/src/Network/generic.cxx b/src/Network/generic.cxx index 9ab7d7699..eebc6de8a 100644 --- a/src/Network/generic.cxx +++ b/src/Network/generic.cxx @@ -1,4 +1,4 @@ -// generic.cxx -- generic protocal class +// generic.cxx -- generic protocol class // // Written by Curtis Olson, started November 1999. // @@ -26,24 +26,202 @@ #include // strstr() #include // strtod(), atoi() +#include #include #include #include #include #include +#include #include #include +#include #include
#include
+#include
#include
- #include "generic.hxx" +using simgear::strutils::unescape; + +class FGProtocolWrapper { +public: + virtual ~FGProtocolWrapper() {} + virtual int wrap( size_t n, uint8_t * buf ) = 0; + virtual int unwrap( size_t n, uint8_t * buf ) = 0; +}; + +/** + * http://www.ka9q.net/papers/kiss.html + */ +class FGKissWrapper : public FGProtocolWrapper { +public: + virtual int wrap( size_t n, uint8_t * buf ); + virtual int unwrap( size_t n, uint8_t * buf ); +private: + static const uint8_t FEND; + static const uint8_t FESC; + static const uint8_t TFEND; + 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 ) +{ + std::vector dest; + uint8_t *sp = buf; + + dest.push_back(FEND); + dest.push_back(0); // command/channel always zero + for( size_t i = 0; i < n; i++ ) { + uint8_t c = *sp++; + switch( c ) { + case FESC: + dest.push_back(FESC); + dest.push_back(TFESC); + break; + + case FEND: + dest.push_back(FESC); + dest.push_back(TFEND); + break; + + default: + dest.push_back(c); + break; + } + } + dest.push_back(FEND); + memcpy( buf, dest.data(), dest.size() ); + return dest.size(); +} -FGGeneric::FGGeneric(vector tokens) : exitOnError(false) +int FGKissWrapper::unwrap( size_t n, uint8_t * buf ) +{ + uint8_t * sp = buf; + + // look for FEND + while( 0 < n && FEND != *sp ) { + sp++; + n--; + } + + // ignore all leading FEND + while( 0 < n && FEND == *sp ) { + sp++; + n--; + } + + if( 0 == n ) return 0; + + std::vector dest; + { + bool escaped = false; + + while( 0 < n ) { + + n--; + uint8_t c = *sp++; + + if( escaped ) { + switch( c ) { + + case TFESC: + dest.push_back( FESC ); + break; + + case TFEND: + dest.push_back( FEND ); + break; + + default: // this is an error - ignore and continue + break; + } + + escaped = false; + + } else { + + switch( c ) { + case FESC: + escaped = true; + break; + + case FEND: + if( 0 != n ) { + SG_LOG(SG_IO, SG_WARN, + "KISS frame detected FEND before end of frame. Trailing data dropped." ); + } + n = 0; + break; + + default: + dest.push_back( c ); + break; + } + } + } + } + + memcpy( buf, dest.data(), dest.size() ); + return dest.size(); +} + + +class FGSTXETXWrapper : public FGProtocolWrapper { +public: + virtual int wrap( size_t n, uint8_t * buf ); + virtual int unwrap( size_t n, uint8_t * buf ); + + static const uint8_t STX; + static const uint8_t ETX; + 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 ) +{ + // stuff payload as + // payload + // if payload contains , stuff as + std::vector dest; + uint8_t *sp = buf; + + dest.push_back(DLE); + dest.push_back(STX); + + while( n > 0 ) { + n--; + + if( DLE == *sp ) + dest.push_back(DLE); + + dest.push_back(*sp++); + } + + dest.push_back(DLE); + dest.push_back(ETX); + + memcpy( buf, dest.data(), dest.size() ); + return dest.size(); +} + +int FGSTXETXWrapper::unwrap( size_t n, uint8_t * buf ) +{ + return n; +} + +FGGeneric::FGGeneric(vector tokens) : exitOnError(false), initOk(false), wrapper(NULL) { size_t configToken; if (tokens[1] == "socket") { @@ -54,9 +232,9 @@ FGGeneric::FGGeneric(vector tokens) : exitOnError(false) configToken = 6; } - if (configToken >= tokens.size()) { - SG_LOG(SG_GENERAL, SG_ALERT, - "Not enough tokens passed for generic protocol"); + if ((configToken >= tokens.size())||(tokens[ configToken ] == "")) { + SG_LOG(SG_NETWORK, SG_ALERT, + "Not enough tokens passed for generic '" << tokens[1] << "' protocol. "); return; } @@ -64,15 +242,17 @@ FGGeneric::FGGeneric(vector tokens) : exitOnError(false) file_name = config+".xml"; direction = tokens[2]; - if (direction != "in" && direction != "out") { - SG_LOG(SG_GENERAL, SG_ALERT, "Unsuported protocol direction: " + if (direction != "in" && direction != "out" && direction != "bi") { + SG_LOG(SG_NETWORK, SG_ALERT, "Unsuported protocol direction: " << direction); + return; } reinit(); } FGGeneric::~FGGeneric() { + delete wrapper; } union u32 { @@ -144,7 +324,7 @@ bool FGGeneric::gen_message_binary() { case FG_DOUBLE: { val = _out_message[i].offset + - _out_message[i].prop->getFloatValue() * _out_message[i].factor; + _out_message[i].prop->getDoubleValue() * _out_message[i].factor; u64 tmpun64; tmpun64.doubleVal = val; @@ -156,6 +336,26 @@ bool FGGeneric::gen_message_binary() { break; } + case FG_BYTE: + { + val = _out_message[i].offset + + _out_message[i].prop->getIntValue() * _out_message[i].factor; + int8_t byteVal = val; + memcpy(&buf[length], &byteVal, sizeof(int8_t)); + length += sizeof(int8_t); + break; + } + + case FG_WORD: + { + val = _out_message[i].offset + + _out_message[i].prop->getIntValue() * _out_message[i].factor; + int16_t wordVal = val; + memcpy(&buf[length], &wordVal, sizeof(int16_t)); + length += sizeof(int16_t); + break; + } + default: // SG_STRING const char *strdata = _out_message[i].prop->getStringValue(); int32_t strlength = strlen(strdata); @@ -177,6 +377,8 @@ bool FGGeneric::gen_message_binary() { /* FIXME padding for alignment? Something like: * length += (strlength % 4 > 0 ? sizeof(int32_t) - strlength % 4 : 0; */ + break; + } } @@ -200,6 +402,8 @@ bool FGGeneric::gen_message_binary() { length += sizeof(int32_t); } + if( wrapper ) length = wrapper->wrap( length, reinterpret_cast(buf) ); + return true; } @@ -214,39 +418,43 @@ bool FGGeneric::gen_message_ascii() { if (i > 0) { generic_sentence += var_separator; } + + string format = simgear::strutils::sanitizePrintfFormat(_out_message[i].format); switch (_out_message[i].type) { + case FG_BYTE: + case FG_WORD: case FG_INT: val = _out_message[i].offset + _out_message[i].prop->getIntValue() * _out_message[i].factor; - snprintf(tmp, 255, _out_message[i].format.c_str(), (int)val); + snprintf(tmp, 255, format.c_str(), (int)val); break; case FG_BOOL: - snprintf(tmp, 255, _out_message[i].format.c_str(), + snprintf(tmp, 255, format.c_str(), _out_message[i].prop->getBoolValue()); break; case FG_FIXED: val = _out_message[i].offset + _out_message[i].prop->getFloatValue() * _out_message[i].factor; - snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val); + snprintf(tmp, 255, format.c_str(), (float)val); break; case FG_FLOAT: val = _out_message[i].offset + _out_message[i].prop->getFloatValue() * _out_message[i].factor; - snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val); + snprintf(tmp, 255, format.c_str(), (float)val); break; case FG_DOUBLE: val = _out_message[i].offset + _out_message[i].prop->getDoubleValue() * _out_message[i].factor; - snprintf(tmp, 255, _out_message[i].format.c_str(), (double)val); + snprintf(tmp, 255, format.c_str(), (double)val); break; default: // SG_STRING - snprintf(tmp, 255, _out_message[i].format.c_str(), + snprintf(tmp, 255, format.c_str(), _out_message[i].prop->getStringValue()); } @@ -272,13 +480,12 @@ bool FGGeneric::gen_message() { } } -bool FGGeneric::parse_message_binary() { +bool FGGeneric::parse_message_binary(int length) { char *p2, *p1 = buf; int32_t tmp32; - double val; int i = -1; - p2 = p1 + FG_MAX_MSG_SIZE; + p2 = p1 + length; while ((++i < (int)_in_message.size()) && (p1 < p2)) { switch (_in_message[i].type) { @@ -288,15 +495,12 @@ bool FGGeneric::parse_message_binary() { } else { tmp32 = *(int32_t *)p1; } - - val = _in_message[i].offset + (double)tmp32 * _in_message[i].factor; - - _in_message[i].prop->setIntValue((int)val); + updateValue(_in_message[i], (int)tmp32); p1 += sizeof(int32_t); break; case FG_BOOL: - _in_message[i].prop->setBoolValue( p1[0] != 0 ); + updateValue(_in_message[i], p1[0] != 0); p1 += 1; break; @@ -306,11 +510,7 @@ bool FGGeneric::parse_message_binary() { } else { tmp32 = *(int32_t *)p1; } - - val = _in_message[i].offset + - ((double)tmp32 / 65536.0f) * _in_message[i].factor; - - _in_message[i].prop->setFloatValue(val); + updateValue(_in_message[i], (float)tmp32 / 65536.0f); p1 += sizeof(int32_t); break; @@ -321,11 +521,7 @@ bool FGGeneric::parse_message_binary() { } else { tmpun32.floatVal = *(float *)p1; } - - val = _in_message[i].offset + - tmpun32.floatVal * _in_message[i].factor; - - _in_message[i].prop->setFloatValue(val); + updateValue(_in_message[i], tmpun32.floatVal); p1 += sizeof(int32_t); break; @@ -336,60 +532,87 @@ bool FGGeneric::parse_message_binary() { } else { tmpun64.doubleVal = *(double *)p1; } + updateValue(_in_message[i], tmpun64.doubleVal); + p1 += sizeof(int64_t); + break; - val = _in_message[i].offset + - tmpun64.doubleVal * _in_message[i].factor; + case FG_BYTE: + tmp32 = *(int8_t *)p1; + updateValue(_in_message[i], (int)tmp32); + p1 += sizeof(int8_t); + break; - _in_message[i].prop->setDoubleValue(val); - p1 += sizeof(int64_t); + case FG_WORD: + if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) { + tmp32 = sg_bswap_16(*(int16_t *)p1); + } else { + tmp32 = *(int16_t *)p1; + } + updateValue(_in_message[i], (int)tmp32); + p1 += sizeof(int16_t); break; default: // SG_STRING SG_LOG( SG_IO, SG_ALERT, "Generic protocol: " "Ignoring unsupported binary input chunk type."); + break; } } return true; } -bool FGGeneric::parse_message_ascii() { - char *p2, *p1 = buf; - double val; +bool FGGeneric::parse_message_ascii(int length) { + char *p1 = buf; int i = -1; + int chunks = _in_message.size(); + int line_separator_size = line_separator.size(); + + if (length < line_separator_size || + line_separator.compare(buf + length - line_separator_size) != 0) { + + SG_LOG(SG_IO, SG_WARN, + "Input line does not end with expected line separator." ); + } else { + buf[length - line_separator_size] = 0; + } - while ((++i < (int)_in_message.size()) && - p1 && strcmp(p1, line_separator.c_str())) { + size_t varsep_len = var_separator.length(); + while ((++i < chunks) && p1) { + char* p2 = NULL; - p2 = strstr(p1, var_separator.c_str()); - if (p2) { - *p2 = 0; - p2 += var_separator.length(); + if (varsep_len > 0) + { + p2 = strstr(p1, var_separator.c_str()); + if (p2) { + *p2 = 0; + p2 += varsep_len; + } } switch (_in_message[i].type) { + case FG_BYTE: + case FG_WORD: case FG_INT: - val = _in_message[i].offset + atoi(p1) * _in_message[i].factor; - _in_message[i].prop->setIntValue((int)val); + updateValue(_in_message[i], atoi(p1)); break; case FG_BOOL: - _in_message[i].prop->setBoolValue( atof(p1) != 0.0 ); + updateValue(_in_message[i], atof(p1) != 0.0); break; case FG_FIXED: case FG_FLOAT: - val = _in_message[i].offset + strtod(p1, 0) * _in_message[i].factor; - _in_message[i].prop->setFloatValue((float)val); + updateValue(_in_message[i], (float)strtod(p1, 0)); break; case FG_DOUBLE: - val = _in_message[i].offset + strtod(p1, 0) * _in_message[i].factor; - _in_message[i].prop->setDoubleValue(val); + updateValue(_in_message[i], (double)strtod(p1, 0)); break; default: // SG_STRING _in_message[i].prop->setStringValue(p1); + break; } p1 = p2; @@ -398,11 +621,11 @@ bool FGGeneric::parse_message_ascii() { return true; } -bool FGGeneric::parse_message() { +bool FGGeneric::parse_message_len(int length) { if (binary_mode) { - return parse_message_binary(); + return parse_message_binary(length); } else { - return parse_message_ascii(); + return parse_message_ascii(length); } } @@ -424,7 +647,9 @@ bool FGGeneric::open() { set_enabled( true ); - if ( get_direction() == SG_IO_OUT && ! preamble.empty() ) { + if ( ((get_direction() == SG_IO_OUT )|| + (get_direction() == SG_IO_BI)) + && ! preamble.empty() ) { if ( ! io->write( preamble.c_str(), preamble.size() ) ) { SG_LOG( SG_IO, SG_WARN, "Error writing preamble." ); return false; @@ -439,18 +664,22 @@ bool FGGeneric::open() { bool FGGeneric::process() { SGIOChannel *io = get_io_channel(); - if ( get_direction() == SG_IO_OUT ) { + if ( (get_direction() == SG_IO_OUT) || + (get_direction() == SG_IO_BI) ) { gen_message(); if ( ! io->write( buf, length ) ) { SG_LOG( SG_IO, SG_WARN, "Error writing data." ); goto error_out; } - } else if ( get_direction() == SG_IO_IN ) { + } + + if (( get_direction() == SG_IO_IN ) || + (get_direction() == SG_IO_BI) ) { if ( io->get_type() == sgFileType ) { if (!binary_mode) { length = io->readline( buf, FG_MAX_MSG_SIZE ); if ( length > 0 ) { - parse_message(); + parse_message_len( length ); } else { SG_LOG( SG_IO, SG_ALERT, "Error reading data." ); return false; @@ -458,7 +687,7 @@ bool FGGeneric::process() { } else { length = io->read( buf, binary_record_length ); if ( length == binary_record_length ) { - parse_message(); + parse_message_len( length ); } else { SG_LOG( SG_IO, SG_ALERT, "Generic protocol: Received binary " @@ -470,12 +699,12 @@ bool FGGeneric::process() { } else { if (!binary_mode) { while ((length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) { - parse_message(); + parse_message_len( length ); } } else { while ((length = io->read( buf, binary_record_length )) == binary_record_length ) { - parse_message(); + parse_message_len( length ); } if ( length > 0 ) { @@ -491,7 +720,7 @@ bool FGGeneric::process() { return true; error_out: if (exitOnError) { - fgExit(1); + fgOSExit(1); return true; // should not get there, but please the compiler } else return false; @@ -502,7 +731,9 @@ error_out: bool FGGeneric::close() { SGIOChannel *io = get_io_channel(); - if ( get_direction() == SG_IO_OUT && ! postamble.empty() ) { + if ( ((get_direction() == SG_IO_OUT)|| + (get_direction() == SG_IO_BI)) + && ! postamble.empty() ) { if ( ! io->write( postamble.c_str(), postamble.size() ) ) { SG_LOG( SG_IO, SG_ALERT, "Error writing postamble." ); return false; @@ -526,15 +757,15 @@ FGGeneric::reinit() path.append("Protocol"); path.append(file_name.c_str()); - SG_LOG(SG_GENERAL, SG_INFO, "Reading communication protocol from " - << path.str()); + SG_LOG(SG_NETWORK, SG_INFO, "Reading communication protocol from " + << path); SGPropertyNode root; try { - readProperties(path.str(), &root); - } catch (const sg_exception &) { - SG_LOG(SG_GENERAL, SG_ALERT, - "Unable to load the protocol configuration file"); + readProperties(path, &root); + } catch (const sg_exception & ex) { + SG_LOG(SG_NETWORK, SG_ALERT, + "Unable to load the protocol configuration file: " << ex.getFormattedMessage() ); return; } @@ -542,19 +773,36 @@ FGGeneric::reinit() SGPropertyNode *output = root.getNode("generic/output"); if (output) { _out_message.clear(); - read_config(output, _out_message); + if (!read_config(output, _out_message)) + { + // bad configuration + return; + } } } else if (direction == "in") { SGPropertyNode *input = root.getNode("generic/input"); if (input) { _in_message.clear(); - read_config(input, _in_message); + if (!read_config(input, _in_message)) + { + // bad configuration + return; + } + if (!binary_mode && (line_separator.empty() || + *line_separator.rbegin() != '\n')) { + + SG_LOG(SG_IO, SG_WARN, + "Warning: Appending newline to line separator in generic input."); + line_separator.push_back('\n'); + } } } + + initOk = true; } -void +bool FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg) { binary_mode = root->getBoolValue("binary_mode"); @@ -567,10 +815,10 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg) * line_sep_string = the string/charachter to place at the end of each * lot of variables */ - preamble = fgUnescape(root->getStringValue("preamble")); - postamble = fgUnescape(root->getStringValue("postamble")); - var_sep_string = fgUnescape(root->getStringValue("var_separator")); - line_sep_string = fgUnescape(root->getStringValue("line_separator")); + preamble = unescape(root->getStringValue("preamble")); + postamble = unescape(root->getStringValue("postamble")); + var_sep_string = unescape(root->getStringValue("var_separator")); + line_sep_string = unescape(root->getStringValue("line_separator")); if ( var_sep_string == "newline" ) { var_separator = '\n'; @@ -650,21 +898,39 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg) "byte order, using HOST byte order."); } } + + if( root->hasValue( "wrapper" ) ) { + string w = root->getStringValue( "wrapper" ); + if( w == "kiss" ) wrapper = new FGKissWrapper(); + else if( w == "stxetx" ) wrapper = new FGSTXETXWrapper(); + else SG_LOG(SG_IO, SG_ALERT, + "generic protocol: Undefined binary protocol wrapper '" + w + "' ignored" ); + } } int record_length = 0; // Only used for binary protocols. vector chunks = root->getChildren("chunk"); + for (unsigned int i = 0; i < chunks.size(); i++) { _serial_prot chunk; // chunk.name = chunks[i]->getStringValue("name"); - chunk.format = fgUnescape(chunks[i]->getStringValue("format", "%d")); + chunk.format = unescape(chunks[i]->getStringValue("format", "%d")); chunk.offset = chunks[i]->getDoubleValue("offset"); chunk.factor = chunks[i]->getDoubleValue("factor", 1.0); - - string node = chunks[i]->getStringValue("node", "/null"); - chunk.prop = fgGetNode(node.c_str(), true); + chunk.min = chunks[i]->getDoubleValue("min"); + chunk.max = chunks[i]->getDoubleValue("max"); + chunk.wrap = chunks[i]->getBoolValue("wrap"); + chunk.rel = chunks[i]->getBoolValue("relative"); + + if( chunks[i]->hasChild("const") ) { + chunk.prop = new SGPropertyNode(); + chunk.prop->setStringValue( chunks[i]->getStringValue("const", "" ) ); + } else { + string node = chunks[i]->getStringValue("node", "/null"); + chunk.prop = fgGetNode(node.c_str(), true); + } string type = chunks[i]->getStringValue("type"); @@ -682,9 +948,15 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg) } else if (type == "fixed") { chunk.type = FG_FIXED; record_length += sizeof(int32_t); - } else if (type == "string") + } else if (type == "string") { chunk.type = FG_STRING; - else { + } else if (type == "byte") { + chunk.type = FG_BYTE; + record_length += sizeof(int8_t); + } else if (type == "word") { + chunk.type = FG_WORD; + record_length += sizeof(int16_t); + } else { chunk.type = FG_INT; record_length += sizeof(int32_t); } @@ -692,7 +964,19 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg) } - if( binary_mode ) { + if( !binary_mode ) + { + if ((chunks.size() > 1)&&(var_sep_string.length() == 0)) + { + // ASCII protocols really need a separator when there is more than one chunk per line + SG_LOG(SG_IO, SG_ALERT, + "generic protocol: Invalid configuration. " + "'var_separator' must not be empty for protocols which have more than one chunk per line."); + return false; + } + } + else + { if (binary_record_length == -1) { binary_record_length = record_length; } else if (binary_record_length < record_length) { @@ -702,4 +986,20 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg) binary_record_length = record_length; } } + + return true; +} + +void FGGeneric::updateValue(FGGeneric::_serial_prot& prot, bool val) +{ + if( prot.rel ) + { + // value inverted if received true, otherwise leave unchanged + if( val ) + setValue(prot.prop, !getValue(prot.prop)); + } + else + { + setValue(prot.prop, val); + } }