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