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