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