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