1 // generic.cxx -- generic protocol class
3 // Written by Curtis Olson, started November 1999.
5 // Copyright (C) 1999 Curtis L. Olson - http://www.flightgear.org/~curt
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
27 #include <string.h> // strstr()
28 #include <stdlib.h> // strtod(), atoi()
31 #include <simgear/debug/logstream.hxx>
32 #include <simgear/io/iochannel.hxx>
33 #include <simgear/structure/exception.hxx>
34 #include <simgear/misc/sg_path.hxx>
35 #include <simgear/misc/stdint.hxx>
36 #include <simgear/misc/strutils.hxx>
37 #include <simgear/props/props.hxx>
38 #include <simgear/props/props_io.hxx>
39 #include <simgear/math/SGMath.hxx>
41 #include <Main/globals.hxx>
42 #include <Main/fg_props.hxx>
43 #include <Main/fg_os.hxx>
44 #include <Main/util.hxx>
45 #include "generic.hxx"
47 using simgear::strutils::unescape;
49 class FGProtocolWrapper {
51 virtual ~FGProtocolWrapper() {}
52 virtual int wrap( size_t n, uint8_t * buf ) = 0;
53 virtual int unwrap( size_t n, uint8_t * buf ) = 0;
57 * http://www.ka9q.net/papers/kiss.html
59 class FGKissWrapper : public FGProtocolWrapper {
61 virtual int wrap( size_t n, uint8_t * buf );
62 virtual int unwrap( size_t n, uint8_t * buf );
64 static const uint8_t FEND;
65 static const uint8_t FESC;
66 static const uint8_t TFEND;
67 static const uint8_t TFESC;
70 const uint8_t FGKissWrapper::FEND = 0xC0;
71 const uint8_t FGKissWrapper::FESC = 0xDB;
72 const uint8_t FGKissWrapper::TFEND = 0xDC;
73 const uint8_t FGKissWrapper::TFESC = 0xDD;
75 int FGKissWrapper::wrap( size_t n, uint8_t * buf )
77 std::vector<uint8_t> dest;
81 dest.push_back(0); // command/channel always zero
82 for( size_t i = 0; i < n; i++ ) {
87 dest.push_back(TFESC);
92 dest.push_back(TFEND);
100 dest.push_back(FEND);
102 memcpy( buf, dest.data(), dest.size() );
106 int FGKissWrapper::unwrap( size_t n, uint8_t * buf )
111 while( 0 < n && FEND != *sp ) {
116 // ignore all leading FEND
117 while( 0 < n && FEND == *sp ) {
122 if( 0 == n ) return 0;
124 std::vector<uint8_t> dest;
126 bool escaped = false;
137 dest.push_back( FESC );
141 dest.push_back( FEND );
144 default: // this is an error - ignore and continue
159 SG_LOG(SG_IO, SG_WARN,
160 "KISS frame detected FEND before end of frame. Trailing data dropped." );
173 memcpy( buf, dest.data(), dest.size() );
178 class FGSTXETXWrapper : public FGProtocolWrapper {
180 virtual int wrap( size_t n, uint8_t * buf );
181 virtual int unwrap( size_t n, uint8_t * buf );
183 static const uint8_t STX;
184 static const uint8_t ETX;
185 static const uint8_t DLE;
188 const uint8_t FGSTXETXWrapper::STX = 0x02;
189 const uint8_t FGSTXETXWrapper::ETX = 0x03;
190 const uint8_t FGSTXETXWrapper::DLE = 0x00;
192 int FGSTXETXWrapper::wrap( size_t n, uint8_t * buf )
195 // <dle><stx>payload<dle><etx>
196 // if payload contains <dle>, stuff <dle> as <dle><dle>
197 std::vector<uint8_t> dest;
209 dest.push_back(*sp++);
215 memcpy( buf, dest.data(), dest.size() );
219 int FGSTXETXWrapper::unwrap( size_t n, uint8_t * buf )
224 FGGeneric::FGGeneric(vector<string> tokens) : exitOnError(false), initOk(false), wrapper(NULL)
227 if (tokens[1] == "socket") {
229 } else if (tokens[1] == "file") {
235 if ((configToken >= tokens.size())||(tokens[ configToken ] == "")) {
236 SG_LOG(SG_NETWORK, SG_ALERT,
237 "Not enough tokens passed for generic '" << tokens[1] << "' protocol. ");
241 string config = tokens[ configToken ];
242 file_name = config+".xml";
243 direction = tokens[2];
245 if (direction != "in" && direction != "out" && direction != "bi") {
246 SG_LOG(SG_NETWORK, SG_ALERT, "Unsuported protocol direction: "
254 FGGeneric::~FGGeneric() {
268 // generate the message
269 bool FGGeneric::gen_message_binary() {
270 string generic_sentence;
274 for (unsigned int i = 0; i < _out_message.size(); i++) {
276 switch (_out_message[i].type) {
279 val = _out_message[i].offset +
280 _out_message[i].prop->getIntValue() * _out_message[i].factor;
281 int32_t intVal = val;
282 if (binary_byte_order != BYTE_ORDER_MATCHES_NETWORK_ORDER) {
283 intVal = (int32_t) sg_bswap_32((uint32_t)intVal);
285 memcpy(&buf[length], &intVal, sizeof(int32_t));
286 length += sizeof(int32_t);
291 buf[length] = (char) (_out_message[i].prop->getBoolValue() ? true : false);
297 val = _out_message[i].offset +
298 _out_message[i].prop->getFloatValue() * _out_message[i].factor;
300 int32_t fixed = (int)(val * 65536.0f);
301 if (binary_byte_order != BYTE_ORDER_MATCHES_NETWORK_ORDER) {
302 fixed = (int32_t) sg_bswap_32((uint32_t)fixed);
304 memcpy(&buf[length], &fixed, sizeof(int32_t));
305 length += sizeof(int32_t);
311 val = _out_message[i].offset +
312 _out_message[i].prop->getFloatValue() * _out_message[i].factor;
314 tmpun32.floatVal = static_cast<float>(val);
316 if (binary_byte_order != BYTE_ORDER_MATCHES_NETWORK_ORDER) {
317 tmpun32.intVal = sg_bswap_32(tmpun32.intVal);
319 memcpy(&buf[length], &tmpun32.intVal, sizeof(uint32_t));
320 length += sizeof(uint32_t);
326 val = _out_message[i].offset +
327 _out_message[i].prop->getDoubleValue() * _out_message[i].factor;
329 tmpun64.doubleVal = val;
331 if (binary_byte_order != BYTE_ORDER_MATCHES_NETWORK_ORDER) {
332 tmpun64.longVal = sg_bswap_64(tmpun64.longVal);
334 memcpy(&buf[length], &tmpun64.longVal, sizeof(uint64_t));
335 length += sizeof(uint64_t);
341 val = _out_message[i].offset +
342 _out_message[i].prop->getIntValue() * _out_message[i].factor;
343 int8_t byteVal = val;
344 memcpy(&buf[length], &byteVal, sizeof(int8_t));
345 length += sizeof(int8_t);
351 val = _out_message[i].offset +
352 _out_message[i].prop->getIntValue() * _out_message[i].factor;
353 int16_t wordVal = val;
354 memcpy(&buf[length], &wordVal, sizeof(int16_t));
355 length += sizeof(int16_t);
359 default: // SG_STRING
360 const char *strdata = _out_message[i].prop->getStringValue();
361 int32_t strlength = strlen(strdata);
363 if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
364 SG_LOG( SG_IO, SG_ALERT, "Generic protocol: "
365 "FG_STRING will be written in host byte order.");
367 /* Format for strings is
368 * [length as int, 4 bytes][ASCII data, length bytes]
370 if (binary_byte_order != BYTE_ORDER_MATCHES_NETWORK_ORDER) {
371 strlength = sg_bswap_32(strlength);
373 memcpy(&buf[length], &strlength, sizeof(int32_t));
374 length += sizeof(int32_t);
375 strncpy(&buf[length], strdata, strlength);
377 /* FIXME padding for alignment? Something like:
378 * length += (strlength % 4 > 0 ? sizeof(int32_t) - strlength % 4 : 0;
385 // add the footer to the packet ("line")
386 switch (binary_footer_type) {
388 binary_footer_value = length;
396 if (binary_footer_type != FOOTER_NONE) {
397 int32_t intValue = binary_footer_value;
398 if (binary_byte_order != BYTE_ORDER_MATCHES_NETWORK_ORDER) {
399 intValue = sg_bswap_32(binary_footer_value);
401 memcpy(&buf[length], &intValue, sizeof(int32_t));
402 length += sizeof(int32_t);
405 if( wrapper ) length = wrapper->wrap( length, reinterpret_cast<uint8_t*>(buf) );
410 bool FGGeneric::gen_message_ascii() {
411 string generic_sentence;
416 for (unsigned int i = 0; i < _out_message.size(); i++) {
419 generic_sentence += var_separator;
422 string format = simgear::strutils::sanitizePrintfFormat(_out_message[i].format);
424 switch (_out_message[i].type) {
428 val = _out_message[i].offset +
429 _out_message[i].prop->getIntValue() * _out_message[i].factor;
430 snprintf(tmp, 255, format.c_str(), (int)val);
434 snprintf(tmp, 255, format.c_str(),
435 _out_message[i].prop->getBoolValue());
439 val = _out_message[i].offset +
440 _out_message[i].prop->getFloatValue() * _out_message[i].factor;
441 snprintf(tmp, 255, format.c_str(), (float)val);
445 val = _out_message[i].offset +
446 _out_message[i].prop->getFloatValue() * _out_message[i].factor;
447 snprintf(tmp, 255, format.c_str(), (float)val);
451 val = _out_message[i].offset +
452 _out_message[i].prop->getDoubleValue() * _out_message[i].factor;
453 snprintf(tmp, 255, format.c_str(), (double)val);
456 default: // SG_STRING
457 snprintf(tmp, 255, format.c_str(),
458 _out_message[i].prop->getStringValue());
461 generic_sentence += tmp;
464 /* After each lot of variables has been added, put the line separator
467 generic_sentence += line_separator;
469 length = generic_sentence.length();
470 strncpy( buf, generic_sentence.c_str(), length );
475 bool FGGeneric::gen_message() {
477 return gen_message_binary();
479 return gen_message_ascii();
483 bool FGGeneric::parse_message_binary(int length) {
489 while ((++i < (int)_in_message.size()) && (p1 < p2)) {
491 switch (_in_message[i].type) {
493 if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
494 tmp32 = sg_bswap_32(*(int32_t *)p1);
496 tmp32 = *(int32_t *)p1;
498 updateValue(_in_message[i], (int)tmp32);
499 p1 += sizeof(int32_t);
503 updateValue(_in_message[i], p1[0] != 0);
508 if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
509 tmp32 = sg_bswap_32(*(int32_t *)p1);
511 tmp32 = *(int32_t *)p1;
513 updateValue(_in_message[i], (float)tmp32 / 65536.0f);
514 p1 += sizeof(int32_t);
519 if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
520 tmpun32.intVal = sg_bswap_32(*(uint32_t *)p1);
522 tmpun32.floatVal = *(float *)p1;
524 updateValue(_in_message[i], tmpun32.floatVal);
525 p1 += sizeof(int32_t);
530 if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
531 tmpun64.longVal = sg_bswap_64(*(uint64_t *)p1);
533 tmpun64.doubleVal = *(double *)p1;
535 updateValue(_in_message[i], tmpun64.doubleVal);
536 p1 += sizeof(int64_t);
540 tmp32 = *(int8_t *)p1;
541 updateValue(_in_message[i], (int)tmp32);
542 p1 += sizeof(int8_t);
546 if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
547 tmp32 = sg_bswap_16(*(int16_t *)p1);
549 tmp32 = *(int16_t *)p1;
551 updateValue(_in_message[i], (int)tmp32);
552 p1 += sizeof(int16_t);
555 default: // SG_STRING
556 SG_LOG( SG_IO, SG_ALERT, "Generic protocol: "
557 "Ignoring unsupported binary input chunk type.");
565 bool FGGeneric::parse_message_ascii(int length) {
568 int chunks = _in_message.size();
569 int line_separator_size = line_separator.size();
571 if (length < line_separator_size ||
572 line_separator.compare(buf + length - line_separator_size) != 0) {
574 SG_LOG(SG_IO, SG_WARN,
575 "Input line does not end with expected line separator." );
577 buf[length - line_separator_size] = 0;
580 size_t varsep_len = var_separator.length();
581 while ((++i < chunks) && p1) {
586 p2 = strstr(p1, var_separator.c_str());
593 switch (_in_message[i].type) {
597 updateValue(_in_message[i], atoi(p1));
601 updateValue(_in_message[i], atof(p1) != 0.0);
606 updateValue(_in_message[i], (float)strtod(p1, 0));
610 updateValue(_in_message[i], (double)strtod(p1, 0));
613 default: // SG_STRING
614 _in_message[i].prop->setStringValue(p1);
624 bool FGGeneric::parse_message_len(int length) {
626 return parse_message_binary(length);
628 return parse_message_ascii(length);
633 // open hailing frequencies
634 bool FGGeneric::open() {
635 if ( is_enabled() ) {
636 SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
637 << "is already in use, ignoring" );
641 SGIOChannel *io = get_io_channel();
643 if ( ! io->open( get_direction() ) ) {
644 SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
650 if ( ((get_direction() == SG_IO_OUT )||
651 (get_direction() == SG_IO_BI))
652 && ! preamble.empty() ) {
653 if ( ! io->write( preamble.c_str(), preamble.size() ) ) {
654 SG_LOG( SG_IO, SG_WARN, "Error writing preamble." );
663 // process work for this port
664 bool FGGeneric::process() {
665 SGIOChannel *io = get_io_channel();
667 if ( (get_direction() == SG_IO_OUT) ||
668 (get_direction() == SG_IO_BI) ) {
670 if ( ! io->write( buf, length ) ) {
671 SG_LOG( SG_IO, SG_WARN, "Error writing data." );
676 if (( get_direction() == SG_IO_IN ) ||
677 (get_direction() == SG_IO_BI) ) {
678 if ( io->get_type() == sgFileType ) {
680 length = io->readline( buf, FG_MAX_MSG_SIZE );
682 parse_message_len( length );
684 SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
688 length = io->read( buf, binary_record_length );
689 if ( length == binary_record_length ) {
690 parse_message_len( length );
692 SG_LOG( SG_IO, SG_ALERT,
693 "Generic protocol: Received binary "
694 "record of unexpected size, expected: "
695 << binary_record_length << " but received: "
701 while ((length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) {
702 parse_message_len( length );
705 while ((length = io->read( buf, binary_record_length ))
706 == binary_record_length ) {
707 parse_message_len( length );
711 SG_LOG( SG_IO, SG_ALERT,
712 "Generic protocol: Received binary "
713 "record of unexpected size, expected: "
714 << binary_record_length << " but received: "
724 return true; // should not get there, but please the compiler
731 bool FGGeneric::close() {
732 SGIOChannel *io = get_io_channel();
734 if ( ((get_direction() == SG_IO_OUT)||
735 (get_direction() == SG_IO_BI))
736 && ! postamble.empty() ) {
737 if ( ! io->write( postamble.c_str(), postamble.size() ) ) {
738 SG_LOG( SG_IO, SG_ALERT, "Error writing postamble." );
743 set_enabled( false );
745 if ( ! io->close() ) {
756 SGPath path( globals->get_fg_root() );
757 path.append("Protocol");
758 path.append(file_name.c_str());
760 SG_LOG(SG_NETWORK, SG_INFO, "Reading communication protocol from "
765 readProperties(path.str(), &root);
766 } catch (const sg_exception & ex) {
767 SG_LOG(SG_NETWORK, SG_ALERT,
768 "Unable to load the protocol configuration file: " << ex.getFormattedMessage() );
772 if (direction == "out") {
773 SGPropertyNode *output = root.getNode("generic/output");
775 _out_message.clear();
776 if (!read_config(output, _out_message))
782 } else if (direction == "in") {
783 SGPropertyNode *input = root.getNode("generic/input");
786 if (!read_config(input, _in_message))
791 if (!binary_mode && (line_separator.empty() ||
792 *line_separator.rbegin() != '\n')) {
794 SG_LOG(SG_IO, SG_WARN,
795 "Warning: Appending newline to line separator in generic input.");
796 line_separator.push_back('\n');
806 FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg)
808 binary_mode = root->getBoolValue("binary_mode");
811 /* These variables specified in the $FG_ROOT/data/Protocol/xxx.xml
812 * file for each format
814 * var_sep_string = the string/charachter to place between variables
815 * line_sep_string = the string/charachter to place at the end of each
818 preamble = unescape(root->getStringValue("preamble"));
819 postamble = unescape(root->getStringValue("postamble"));
820 var_sep_string = unescape(root->getStringValue("var_separator"));
821 line_sep_string = unescape(root->getStringValue("line_separator"));
823 if ( var_sep_string == "newline" ) {
824 var_separator = '\n';
825 } else if ( var_sep_string == "tab" ) {
826 var_separator = '\t';
827 } else if ( var_sep_string == "space" ) {
829 } else if ( var_sep_string == "formfeed" ) {
830 var_separator = '\f';
831 } else if ( var_sep_string == "carriagereturn" ) {
832 var_sep_string = '\r';
833 } else if ( var_sep_string == "verticaltab" ) {
834 var_separator = '\v';
836 var_separator = var_sep_string;
839 if ( line_sep_string == "newline" ) {
840 line_separator = '\n';
841 } else if ( line_sep_string == "tab" ) {
842 line_separator = '\t';
843 } else if ( line_sep_string == "space" ) {
844 line_separator = ' ';
845 } else if ( line_sep_string == "formfeed" ) {
846 line_separator = '\f';
847 } else if ( line_sep_string == "carriagereturn" ) {
848 line_separator = '\r';
849 } else if ( line_sep_string == "verticaltab" ) {
850 line_separator = '\v';
852 line_separator = line_sep_string;
855 // default values: no footer and record_length = sizeof(representation)
856 binary_footer_type = FOOTER_NONE;
857 binary_record_length = -1;
859 // default choice is network byte order (big endian)
860 if (sgIsLittleEndian()) {
861 binary_byte_order = BYTE_ORDER_NEEDS_CONVERSION;
863 binary_byte_order = BYTE_ORDER_MATCHES_NETWORK_ORDER;
866 if ( root->hasValue("binary_footer") ) {
867 string footer_type = root->getStringValue("binary_footer");
868 if ( footer_type == "length" ) {
869 binary_footer_type = FOOTER_LENGTH;
870 } else if ( footer_type.substr(0, 5) == "magic" ) {
871 binary_footer_type = FOOTER_MAGIC;
872 binary_footer_value = strtol(footer_type.substr(6,
873 footer_type.length() - 6).c_str(), (char**)0, 0);
874 } else if ( footer_type != "none" ) {
875 SG_LOG(SG_IO, SG_ALERT,
876 "generic protocol: Unknown generic binary protocol "
877 "footer '" << footer_type << "', using no footer.");
881 if ( root->hasValue("record_length") ) {
882 binary_record_length = root->getIntValue("record_length");
885 if ( root->hasValue("byte_order") ) {
886 string byte_order = root->getStringValue("byte_order");
887 if (byte_order == "network" ) {
888 if ( sgIsLittleEndian() ) {
889 binary_byte_order = BYTE_ORDER_NEEDS_CONVERSION;
891 binary_byte_order = BYTE_ORDER_MATCHES_NETWORK_ORDER;
893 } else if ( byte_order == "host" ) {
894 binary_byte_order = BYTE_ORDER_MATCHES_NETWORK_ORDER;
896 SG_LOG(SG_IO, SG_ALERT,
897 "generic protocol: Undefined generic binary protocol"
898 "byte order, using HOST byte order.");
902 if( root->hasValue( "wrapper" ) ) {
903 string w = root->getStringValue( "wrapper" );
904 if( w == "kiss" ) wrapper = new FGKissWrapper();
905 else if( w == "stxetx" ) wrapper = new FGSTXETXWrapper();
906 else SG_LOG(SG_IO, SG_ALERT,
907 "generic protocol: Undefined binary protocol wrapper '" + w + "' ignored" );
911 int record_length = 0; // Only used for binary protocols.
912 vector<SGPropertyNode_ptr> chunks = root->getChildren("chunk");
914 for (unsigned int i = 0; i < chunks.size(); i++) {
918 // chunk.name = chunks[i]->getStringValue("name");
919 chunk.format = unescape(chunks[i]->getStringValue("format", "%d"));
920 chunk.offset = chunks[i]->getDoubleValue("offset");
921 chunk.factor = chunks[i]->getDoubleValue("factor", 1.0);
922 chunk.min = chunks[i]->getDoubleValue("min");
923 chunk.max = chunks[i]->getDoubleValue("max");
924 chunk.wrap = chunks[i]->getBoolValue("wrap");
925 chunk.rel = chunks[i]->getBoolValue("relative");
927 if( chunks[i]->hasChild("const") ) {
928 chunk.prop = new SGPropertyNode();
929 chunk.prop->setStringValue( chunks[i]->getStringValue("const", "" ) );
931 string node = chunks[i]->getStringValue("node", "/null");
932 chunk.prop = fgGetNode(node.c_str(), true);
935 string type = chunks[i]->getStringValue("type");
937 // Note: officially the type is called 'bool' but for backward
938 // compatibility 'boolean' will also be supported.
939 if (type == "bool" || type == "boolean") {
940 chunk.type = FG_BOOL;
942 } else if (type == "float") {
943 chunk.type = FG_FLOAT;
944 record_length += sizeof(int32_t);
945 } else if (type == "double") {
946 chunk.type = FG_DOUBLE;
947 record_length += sizeof(int64_t);
948 } else if (type == "fixed") {
949 chunk.type = FG_FIXED;
950 record_length += sizeof(int32_t);
951 } else if (type == "string") {
952 chunk.type = FG_STRING;
953 } else if (type == "byte") {
954 chunk.type = FG_BYTE;
955 record_length += sizeof(int8_t);
956 } else if (type == "word") {
957 chunk.type = FG_WORD;
958 record_length += sizeof(int16_t);
961 record_length += sizeof(int32_t);
963 msg.push_back(chunk);
969 if ((chunks.size() > 1)&&(var_sep_string.length() == 0))
971 // ASCII protocols really need a separator when there is more than one chunk per line
972 SG_LOG(SG_IO, SG_ALERT,
973 "generic protocol: Invalid configuration. "
974 "'var_separator' must not be empty for protocols which have more than one chunk per line.");
980 if (binary_record_length == -1) {
981 binary_record_length = record_length;
982 } else if (binary_record_length < record_length) {
983 SG_LOG(SG_IO, SG_ALERT,
984 "generic protocol: Requested binary record length shorter than "
985 " requested record representation.");
986 binary_record_length = record_length;
993 void FGGeneric::updateValue(FGGeneric::_serial_prot& prot, bool val)
997 // value inverted if received true, otherwise leave unchanged
999 setValue(prot.prop, !getValue<bool>(prot.prop));
1003 setValue(prot.prop, val);