src/Network/generic.cxx

   1 // generic.cxx -- generic protocal class
   2 //
   3 // Written by Curtis Olson, started November 1999.
   4 //
   5 // Copyright (C) 1999  Curtis L. Olson - http://www.flightgear.org/~curt
   6 //
   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.
  11 //
  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.
  16 //
  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.
  20 //
  21 // $Id$
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include "config.h"
  25 #endif
  26
  27 #include <string.h>                // strstr()
  28 #include <stdlib.h>                // strtod(), atoi()
  29
  30 #include <simgear/debug/logstream.hxx>
  31 #include <simgear/io/iochannel.hxx>
  32 #include <simgear/structure/exception.hxx>
  33 #include <simgear/misc/sg_path.hxx>
  34 #include <simgear/misc/stdint.hxx>
  35 #include <simgear/props/props.hxx>
  36 #include <simgear/props/props_io.hxx>
  37
  38 #include <Main/globals.hxx>
  39 #include <Main/fg_props.hxx>
  40 #include <Main/util.hxx>
  41
  42 #include "generic.hxx"
  43
  44
  45
  46 FGGeneric::FGGeneric(vector<string> tokens) : exitOnError(false)
  47 {
  48     size_t configToken;
  49     if (tokens[1] == "socket") {
  50         configToken = 7;
  51     } else if (tokens[1] == "file") {
  52         configToken = 5;
  53     } else {
  54         configToken = 6;
  55     }
  56
  57     if (configToken >= tokens.size()) {
  58        SG_LOG(SG_GENERAL, SG_ALERT,
  59               "Not enough tokens passed for generic protocol");
  60        return;
  61     }
  62
  63     string config = tokens[ configToken ];
  64     file_name = config+".xml";
  65
  66     if (tokens[2] != "in" && tokens[2] != "out") {
  67         SG_LOG(SG_GENERAL, SG_ALERT, "Unsuported protocol direction: "
  68                << tokens[2]);
  69     }
  70
  71     reinit();
  72 }
  73
  74 FGGeneric::~FGGeneric() {
  75 }
  76
  77 union u32 {
  78     uint32_t intVal;
  79     float floatVal;
  80 };
  81
  82 union u64 {
  83     uint64_t longVal;
  84     double doubleVal;
  85 };
  86
  87 // generate the message
  88 bool FGGeneric::gen_message_binary() {
  89     string generic_sentence;
  90     length = 0;
  91
  92     double val;
  93     for (unsigned int i = 0; i < _out_message.size(); i++) {
  94
  95         switch (_out_message[i].type) {
  96         case FG_INT:
  97             val = _out_message[i].offset +
  98                   _out_message[i].prop->getIntValue() * _out_message[i].factor;
  99
 100             if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
 101                 *((int32_t*)&buf[length]) = (int32_t)val;
 102             } else {
 103                 *((uint32_t*)&buf[length]) = sg_bswap_32((uint32_t)val);
 104             }
 105             length += sizeof(int32_t);
 106             break;
 107
 108         case FG_BOOL:
 109             *((int8_t*)&buf[length])
 110                       = _out_message[i].prop->getBoolValue() ? true : false;
 111             length += 1;
 112             break;
 113
 114         case FG_FIXED:
 115         {
 116             val = _out_message[i].offset +
 117                  _out_message[i].prop->getFloatValue() * _out_message[i].factor;
 118
 119             int fixed = (int)(val * 65536.0f);
 120             if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
 121                 *((int32_t*)&buf[length]) = (int32_t)fixed;
 122             } else {
 123                 *((uint32_t*)&buf[length]) = sg_bswap_32((uint32_t)fixed);
 124             }
 125             length += sizeof(int32_t);
 126             break;
 127         }
 128         case FG_FLOAT:
 129             val = _out_message[i].offset +
 130                  _out_message[i].prop->getFloatValue() * _out_message[i].factor;
 131
 132             if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
 133                 *((float*)&buf[length]) = val;
 134             } else {
 135                 u32 tmpun32;
 136                 tmpun32.floatVal = static_cast<float>(val);
 137                 *((uint32_t*)&buf[length]) = sg_bswap_32(tmpun32.intVal);
 138             }
 139             length += sizeof(uint32_t);
 140             break;
 141
 142         case FG_DOUBLE:
 143             val = _out_message[i].offset +
 144                  _out_message[i].prop->getFloatValue() * _out_message[i].factor;
 145
 146             if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
 147                 *((double*)&buf[length]) = val;
 148             } else {
 149                 u64 tmpun64;
 150                 tmpun64.doubleVal = val;
 151                 *((uint64_t*)&buf[length]) = sg_bswap_64(tmpun64.longVal);
 152             }
 153             length += sizeof(int64_t);
 154             break;
 155
 156         default: // SG_STRING
 157             const char *strdata = _out_message[i].prop->getStringValue();
 158             int strlength = strlen(strdata);
 159
 160             if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
 161                 SG_LOG( SG_IO, SG_ALERT, "Generic protocol: "
 162                         "FG_STRING will be written in host byte order.");
 163             }
 164             /* Format for strings is
 165              * [length as int, 4 bytes][ASCII data, length bytes]
 166              */
 167             if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
 168                 *((int32_t*)&buf[length]) = strlength;
 169             } else {
 170                 *((int32_t*)&buf[length]) = sg_bswap_32(strlength);
 171             }
 172             length += sizeof(int32_t);
 173             strncpy(&buf[length], strdata, strlength);
 174             length += strlength;
 175             /* FIXME padding for alignment? Something like:
 176              * length += (strlength % 4 > 0 ? sizeof(int32_t) - strlength % 4 : 0;
 177              */
 178         }
 179     }
 180
 181     // add the footer to the packet ("line")
 182     switch (binary_footer_type) {
 183         case FOOTER_LENGTH:
 184             binary_footer_value = length;
 185             break;
 186
 187         case FOOTER_MAGIC:
 188         case FOOTER_NONE:
 189             break;
 190     }
 191
 192     if (binary_footer_type != FOOTER_NONE) {
 193         if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
 194             *((int32_t*)&buf[length]) = binary_footer_value;
 195         } else {
 196             *((int32_t*)&buf[length]) = sg_bswap_32(binary_footer_value);
 197         }
 198         length += sizeof(int32_t);
 199     }
 200
 201     return true;
 202 }
 203
 204 bool FGGeneric::gen_message_ascii() {
 205     string generic_sentence;
 206     char tmp[255];
 207     length = 0;
 208
 209     double val;
 210     for (unsigned int i = 0; i < _out_message.size(); i++) {
 211
 212         if (i > 0) {
 213             generic_sentence += var_separator;
 214         }
 215
 216         switch (_out_message[i].type) {
 217         case FG_INT:
 218             val = _out_message[i].offset +
 219                   _out_message[i].prop->getIntValue() * _out_message[i].factor;
 220             snprintf(tmp, 255, _out_message[i].format.c_str(), (int)val);
 221             break;
 222
 223         case FG_BOOL:
 224             snprintf(tmp, 255, _out_message[i].format.c_str(),
 225                                _out_message[i].prop->getBoolValue());
 226             break;
 227
 228         case FG_FIXED:
 229             val = _out_message[i].offset +
 230                 _out_message[i].prop->getFloatValue() * _out_message[i].factor;
 231             snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val);
 232             break;
 233
 234         case FG_FLOAT:
 235             val = _out_message[i].offset +
 236                 _out_message[i].prop->getFloatValue() * _out_message[i].factor;
 237             snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val);
 238             break;
 239
 240         case FG_DOUBLE:
 241             val = _out_message[i].offset +
 242                 _out_message[i].prop->getFloatValue() * _out_message[i].factor;
 243             snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val);
 244             break;
 245
 246         default: // SG_STRING
 247             snprintf(tmp, 255, _out_message[i].format.c_str(),
 248                                    _out_message[i].prop->getStringValue());
 249         }
 250
 251         generic_sentence += tmp;
 252     }
 253
 254     /* After each lot of variables has been added, put the line separator
 255      * char/string
 256      */
 257     generic_sentence += line_separator;
 258
 259     length =  generic_sentence.length();
 260     strncpy( buf, generic_sentence.c_str(), length );
 261
 262     return true;
 263 }
 264
 265 bool FGGeneric::gen_message() {
 266     if (binary_mode) {
 267         return gen_message_binary();
 268     } else {
 269         return gen_message_ascii();
 270     }
 271 }
 272
 273 bool FGGeneric::parse_message_binary() {
 274     char *p2, *p1 = buf;
 275     int32_t tmp32;
 276     double val;
 277     int i = -1;
 278
 279     p2 = p1 + FG_MAX_MSG_SIZE;
 280     while ((++i < (int)_in_message.size()) && (p1  < p2)) {
 281
 282         switch (_in_message[i].type) {
 283         case FG_INT:
 284             if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
 285                 tmp32 = sg_bswap_32(*(int32_t *)p1);
 286             } else {
 287                 tmp32 = *(int32_t *)p1;
 288             }
 289
 290             val = _in_message[i].offset + (double)tmp32 * _in_message[i].factor;
 291
 292             _in_message[i].prop->setIntValue((int)val);
 293             p1 += sizeof(int32_t);
 294             break;
 295
 296         case FG_BOOL:
 297             _in_message[i].prop->setBoolValue( p1[0] != 0 );
 298             p1 += 1;
 299             break;
 300
 301         case FG_FIXED:
 302             if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
 303                 tmp32 = sg_bswap_32(*(int32_t *)p1);
 304             } else {
 305                 tmp32 = *(int32_t *)p1;
 306             }
 307
 308             val = _in_message[i].offset +
 309                   ((double)tmp32 / 65536.0f) * _in_message[i].factor;
 310
 311             _in_message[i].prop->setFloatValue(val);
 312             p1 += sizeof(int32_t);
 313             break;
 314
 315         case FG_FLOAT:
 316             u32 tmpun32;
 317             if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
 318                 tmpun32.intVal = sg_bswap_32(*(uint32_t *)p1);
 319             } else {
 320                 tmpun32.floatVal = *(float *)p1;
 321             }
 322
 323             val = _in_message[i].offset +
 324                   tmpun32.floatVal * _in_message[i].factor;
 325
 326             _in_message[i].prop->setFloatValue(val);
 327             p1 += sizeof(int32_t);
 328             break;
 329
 330         case FG_DOUBLE:
 331             u64 tmpun64;
 332             if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
 333                 tmpun64.longVal = sg_bswap_64(*(uint64_t *)p1);
 334             } else {
 335                 tmpun64.doubleVal = *(double *)p1;
 336             }
 337
 338             val = _in_message[i].offset +
 339                    tmpun64.doubleVal * _in_message[i].factor;
 340
 341             _in_message[i].prop->setDoubleValue(val);
 342             p1 += sizeof(int64_t);
 343             break;
 344
 345         default: // SG_STRING
 346             SG_LOG( SG_IO, SG_ALERT, "Generic protocol: "
 347                     "Ignoring unsupported binary input chunk type.");
 348         }
 349     }
 350
 351     return true;
 352 }
 353
 354 bool FGGeneric::parse_message_ascii() {
 355     char *p2, *p1 = buf;
 356     double val;
 357     int i = -1;
 358
 359     while ((++i < (int)_in_message.size()) &&
 360            p1 && strcmp(p1, line_separator.c_str())) {
 361
 362         p2 = strstr(p1, var_separator.c_str());
 363         if (p2) {
 364             *p2 = 0;
 365             p2 += var_separator.length();
 366         }
 367
 368         switch (_in_message[i].type) {
 369         case FG_INT:
 370             val = _in_message[i].offset + atoi(p1) * _in_message[i].factor;
 371             _in_message[i].prop->setIntValue((int)val);
 372             break;
 373
 374         case FG_BOOL:
 375             _in_message[i].prop->setBoolValue( atof(p1) != 0.0 );
 376             break;
 377
 378         case FG_FIXED:
 379         case FG_FLOAT:
 380         case FG_DOUBLE:
 381             val = _in_message[i].offset + strtod(p1, 0) * _in_message[i].factor;
 382             _in_message[i].prop->setFloatValue((float)val);
 383             break;
 384
 385         default: // SG_STRING
 386             _in_message[i].prop->setStringValue(p1);
 387         }
 388
 389         p1 = p2;
 390     }
 391
 392     return true;
 393 }
 394
 395 bool FGGeneric::parse_message() {
 396     if (binary_mode) {
 397         return parse_message_binary();
 398     } else {
 399         return parse_message_ascii();
 400     }
 401 }
 402
 403
 404 // open hailing frequencies
 405 bool FGGeneric::open() {
 406     if ( is_enabled() ) {
 407         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
 408                 << "is already in use, ignoring" );
 409         return false;
 410     }
 411
 412     SGIOChannel *io = get_io_channel();
 413
 414     if ( ! io->open( get_direction() ) ) {
 415         SG_LOG( SG_IO, SG_ALERT, "Error opening channel communication layer." );
 416         return false;
 417     }
 418
 419     set_enabled( true );
 420
 421     if ( get_direction() == SG_IO_OUT && ! preamble.empty() ) {
 422         if ( ! io->write( preamble.c_str(), preamble.size() ) ) {
 423             SG_LOG( SG_IO, SG_WARN, "Error writing preamble." );
 424             return false;
 425         }
 426     }
 427
 428     return true;
 429 }
 430
 431
 432 // process work for this port
 433 bool FGGeneric::process() {
 434     SGIOChannel *io = get_io_channel();
 435
 436     if ( get_direction() == SG_IO_OUT ) {
 437         gen_message();
 438         if ( ! io->write( buf, length ) ) {
 439             SG_LOG( SG_IO, SG_WARN, "Error writing data." );
 440             goto error_out;
 441         }
 442     } else if ( get_direction() == SG_IO_IN ) {
 443         if ( io->get_type() == sgFileType ) {
 444             if (!binary_mode) {
 445                 length = io->readline( buf, FG_MAX_MSG_SIZE );
 446                 if ( length > 0 ) {
 447                     parse_message();
 448                 } else {
 449                     SG_LOG( SG_IO, SG_ALERT, "Error reading data." );
 450                     return false;
 451                 }
 452             } else {
 453                 length = io->read( buf, binary_record_length );
 454                 if ( length == binary_record_length ) {
 455                     parse_message();
 456                 } else {
 457                     SG_LOG( SG_IO, SG_ALERT,
 458                             "Generic protocol: Received binary "
 459                             "record of unexpected size, expected: "
 460                             << binary_record_length << " but received: "
 461                             << length);
 462                 }
 463             }
 464         } else {
 465             do {
 466                 if (!binary_mode) {
 467                     length = io->readline( buf, FG_MAX_MSG_SIZE );
 468                     if ( length > 0 ) {
 469                         parse_message();
 470                     }
 471                 } else {
 472                     length = io->read( buf, binary_record_length );
 473                     if ( length == binary_record_length ) {
 474                         parse_message();
 475                     } else if ( length > 0 ) {
 476                         SG_LOG( SG_IO, SG_ALERT,
 477                             "Generic protocol: Received binary "
 478                             "record of unexpected size, expected: "
 479                             << binary_record_length << " but received: "
 480                             << length);
 481                     }
 482                 }
 483             } while ( length == binary_record_length );
 484         }
 485     }
 486     return true;
 487 error_out:
 488     if (exitOnError) {
 489         fgExit(1);
 490         return true; // should not get there, but please the compiler
 491     } else
 492         return false;
 493 }
 494
 495
 496 // close the channel
 497 bool FGGeneric::close() {
 498     SGIOChannel *io = get_io_channel();
 499
 500     if ( get_direction() == SG_IO_OUT && ! postamble.empty() ) {
 501         if ( ! io->write( postamble.c_str(), postamble.size() ) ) {
 502             SG_LOG( SG_IO, SG_ALERT, "Error writing postamble." );
 503             return false;
 504         }
 505     }
 506
 507     set_enabled( false );
 508
 509     if ( ! io->close() ) {
 510         return false;
 511     }
 512
 513     return true;
 514 }
 515
 516
 517 void
 518 FGGeneric::reinit()
 519 {
 520     SGPath path( globals->get_fg_root() );
 521     path.append("Protocol");
 522     path.append(file_name.c_str());
 523
 524     SG_LOG(SG_GENERAL, SG_INFO, "Reading communication protocol from "
 525                                 << path.str());
 526
 527     SGPropertyNode root;
 528     try {
 529         readProperties(path.str(), &root);
 530     } catch (const sg_exception &) {
 531         SG_LOG(SG_GENERAL, SG_ALERT,
 532          "Unable to load the protocol configuration file");
 533          return;
 534     }
 535
 536     SGPropertyNode *output = root.getNode("generic/output");
 537     if (output) {
 538         read_config(output, _out_message);
 539     }
 540
 541     SGPropertyNode *input = root.getNode("generic/input");
 542     if (input) {
 543         read_config(input, _in_message);
 544     }
 545 }
 546
 547
 548 void
 549 FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg)
 550 {
 551     binary_mode = root->getBoolValue("binary_mode");
 552
 553     if (!binary_mode) {
 554         /* These variables specified in the $FG_ROOT/data/Protocol/xxx.xml
 555          * file for each format
 556          *
 557          * var_sep_string  = the string/charachter to place between variables
 558          * line_sep_string = the string/charachter to place at the end of each
 559          *                   lot of variables
 560          */
 561         preamble = fgUnescape(root->getStringValue("preamble"));
 562         postamble = fgUnescape(root->getStringValue("postamble"));
 563         var_sep_string = fgUnescape(root->getStringValue("var_separator"));
 564         line_sep_string = fgUnescape(root->getStringValue("line_separator"));
 565
 566         if ( var_sep_string == "newline" ) {
 567             var_separator = '\n';
 568         } else if ( var_sep_string == "tab" ) {
 569             var_separator = '\t';
 570         } else if ( var_sep_string == "space" ) {
 571             var_separator = ' ';
 572         } else if ( var_sep_string == "formfeed" ) {
 573             var_separator = '\f';
 574         } else if ( var_sep_string == "carriagereturn" ) {
 575             var_sep_string = '\r';
 576         } else if ( var_sep_string == "verticaltab" ) {
 577             var_separator = '\v';
 578         } else {
 579             var_separator = var_sep_string;
 580         }
 581
 582         if ( line_sep_string == "newline" ) {
 583             line_separator = '\n';
 584         } else if ( line_sep_string == "tab" ) {
 585             line_separator = '\t';
 586         } else if ( line_sep_string == "space" ) {
 587             line_separator = ' ';
 588         } else if ( line_sep_string == "formfeed" ) {
 589             line_separator = '\f';
 590         } else if ( line_sep_string == "carriagereturn" ) {
 591             line_separator = '\r';
 592         } else if ( line_sep_string == "verticaltab" ) {
 593             line_separator = '\v';
 594         } else {
 595             line_separator = line_sep_string;
 596         }
 597     } else {
 598         // default values: no footer and record_length = sizeof(representation)
 599         binary_footer_type = FOOTER_NONE;
 600         binary_record_length = -1;
 601
 602         // default choice is network byte order (big endian)
 603         if (sgIsLittleEndian()) {
 604            binary_byte_order = BYTE_ORDER_NEEDS_CONVERSION;
 605         } else {
 606            binary_byte_order = BYTE_ORDER_MATCHES_NETWORK_ORDER;
 607         }
 608
 609         if ( root->hasValue("binary_footer") ) {
 610             string footer_type = root->getStringValue("binary_footer");
 611             if ( footer_type == "length" ) {
 612                 binary_footer_type = FOOTER_LENGTH;
 613             } else if ( footer_type.substr(0, 5) == "magic" ) {
 614                 binary_footer_type = FOOTER_MAGIC;
 615                 binary_footer_value = strtol(footer_type.substr(6,
 616                             footer_type.length() - 6).c_str(), (char**)0, 0);
 617             } else if ( footer_type != "none" ) {
 618                 SG_LOG(SG_IO, SG_ALERT,
 619                        "generic protocol: Unknown generic binary protocol "
 620                        "footer '" << footer_type << "', using no footer.");
 621             }
 622         }
 623
 624         if ( root->hasValue("record_length") ) {
 625             binary_record_length = root->getIntValue("record_length");
 626         }
 627
 628         if ( root->hasValue("byte_order") ) {
 629             string byte_order = root->getStringValue("byte_order");
 630             if (byte_order == "network" ) {
 631                 if ( sgIsLittleEndian() ) {
 632                     binary_byte_order = BYTE_ORDER_NEEDS_CONVERSION;
 633                 } else {
 634                     binary_byte_order = BYTE_ORDER_MATCHES_NETWORK_ORDER;
 635                 }
 636             } else if ( byte_order == "host" ) {
 637                 binary_byte_order = BYTE_ORDER_MATCHES_NETWORK_ORDER;
 638             } else {
 639                 SG_LOG(SG_IO, SG_ALERT,
 640                        "generic protocol: Undefined generic binary protocol"
 641                        "byte order, using HOST byte order.");
 642             }
 643         }
 644     }
 645
 646     int record_length = 0; // Only used for binary protocols.
 647     vector<SGPropertyNode_ptr> chunks = root->getChildren("chunk");
 648     for (unsigned int i = 0; i < chunks.size(); i++) {
 649
 650         _serial_prot chunk;
 651
 652         // chunk.name = chunks[i]->getStringValue("name");
 653         chunk.format = fgUnescape(chunks[i]->getStringValue("format", "%d"));
 654         chunk.offset = chunks[i]->getDoubleValue("offset");
 655         chunk.factor = chunks[i]->getDoubleValue("factor", 1.0);
 656
 657         string node = chunks[i]->getStringValue("node", "/null");
 658         chunk.prop = fgGetNode(node.c_str(), true);
 659
 660         string type = chunks[i]->getStringValue("type");
 661
 662         // Note: officially the type is called 'bool' but for backward
 663         //       compatibility 'boolean' will also be supported.
 664         if (type == "bool" || type == "boolean") {
 665             chunk.type = FG_BOOL;
 666             record_length += 1;
 667         } else if (type == "float") {
 668             chunk.type = FG_FLOAT;
 669             record_length += sizeof(int32_t);
 670         } else if (type == "double") {
 671             chunk.type = FG_DOUBLE;
 672             record_length += sizeof(int64_t);
 673         } else if (type == "fixed") {
 674             chunk.type = FG_FIXED;
 675             record_length += sizeof(int32_t);
 676         } else if (type == "string")
 677             chunk.type = FG_STRING;
 678         else {
 679             chunk.type = FG_INT;
 680             record_length += sizeof(int32_t);
 681         }
 682         msg.push_back(chunk);
 683
 684     }
 685
 686     if (binary_record_length == -1) {
 687         binary_record_length = record_length;
 688     } else if (binary_record_length < record_length) {
 689         SG_LOG(SG_IO, SG_ALERT,
 690                "generic protocol: Requested binary record length shorter than "
 691                " requested record representation.");
 692         binary_record_length = record_length;
 693     }
 694 }