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