orte/contrib/decoder/decode_msg.c

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <ctype.h>
   4 #include <stdlib.h>
   5 #include <endian.h>
   6 #include <stdint.h>
   7 #include <byteswap.h>
   8 #include <typedefs_defines_rtps.h>
   9
  10 // global variables
  11 FILE *ptr_in, *ptr_out;
  12
  13
  14 /**********************************************************************************/
  15 void
  16 conv_u32(uint32_t *x, uint8_t ef)
  17 {
  18   #if __BYTE_ORDER == __LITTLE_ENDIAN
  19   if (ef)
  20     bswap_32(*x);
  21   #elif __BYTE_ORDER == __BIG_ENDIAN
  22   if (!ef)
  23     bswap_32(*x);
  24   #endif
  25 }
  26
  27 /**********************************************************************************/
  28 void
  29 print_obj(void *obj)
  30 {
  31   fprintf(ptr_out, " (");
  32   switch (bswap_32(*((uint32_t *)obj))) {
  33     case OID_APP:
  34       fprintf(ptr_out, "ManagedApplicaton or Manager");
  35       break;
  36     case OID_WRITE_APPSELF:
  37       fprintf(ptr_out, "Write appself");
  38       break;
  39     case OID_WRITE_APP:
  40       fprintf(ptr_out, "Write app");
  41       break;
  42     case OID_READ_APP:
  43       fprintf(ptr_out, "Read app");
  44       break;
  45     case OID_WRITE_MGR:
  46       fprintf(ptr_out, "Write mgr");
  47       break;
  48     case OID_READ_MGR:
  49       fprintf(ptr_out, "Read mgr");
  50       break;
  51     case OID_WRITE_PUBL:
  52       fprintf(ptr_out, "Write publ");
  53       break;
  54     case OID_READ_PUBL:
  55       fprintf(ptr_out, "Read publ");
  56       break;
  57     case OID_WRITE_SUBS:
  58       fprintf(ptr_out, "Write subs");
  59       break;
  60     case OID_READ_SUBS:
  61       fprintf(ptr_out, "Read subs");
  62       break;
  63     default:
  64       fprintf(ptr_out, "unknown");
  65       break;
  66   }
  67   fprintf(ptr_out, ")\n");
  68 }
  69
  70 /**********************************************************************************/
  71 void
  72 print_app(uint8_t app)
  73 {
  74   switch (app) {
  75     case MANAGEDAPPLICATION:
  76       fprintf(ptr_out, "(managed application)\n");
  77       break;
  78     case MANAGER:
  79       fprintf(ptr_out, "(manager)\n");
  80       break;
  81     default:
  82       fprintf(ptr_out, "(unknown class)\n");
  83       break;
  84   }
  85 }
  86
  87 /**********************************************************************************/
  88 void
  89 decode_parametr_sequence(uint8_t *msg)
  90 {
  91   NtpTime            ts;
  92   ProtocolVersion    pv;
  93   VendorId           vid;
  94   SequenceNumber     sn;
  95   uint8_t      i, j, l;
  96   uint8_t      *k;
  97
  98   while (*((ParameterId *)msg) != PID_SENTINEL) {
  99     printf("id%d,len%d\n", *((ParameterId *)msg), *((ParameterLength *)(msg+2)));
 100     if ((*((ParameterLength *)(msg+2))) == 0)
 101       break;
 102     switch (*(ParameterId *)msg) {
 103       case PID_PAD:
 104         fprintf(ptr_out, "     PID_PAD                              :\n");
 105         break;
 106       case PID_EXPIRATION_TIME:
 107         ts = *((NtpTime *)(msg+4));
 108         fprintf(ptr_out, "     PID_EXPIRATION_TIME                  : %u,%u\n",
 109                 ts.seconds, ts.fraction);
 110         break;
 111       case PID_PERSISTENCE:
 112         ts = *((NtpTime *)(msg+4));
 113         fprintf(ptr_out, "     PID_PERSISTENCE                      : %u,%u\n",
 114                 ts.seconds, ts.fraction);
 115         break;
 116       case PID_MINIMUM_SEPARATION:
 117         ts = *((NtpTime *)(msg+4));
 118         fprintf(ptr_out, "     PID_MINIMUM_SEPARATION               : %u,%u\n",
 119                 ts.seconds, ts.fraction);
 120         break;
 121       case PID_TOPIC:
 122         fprintf(ptr_out, "     PID_TOPIC                            :\n");
 123         k = msg+4;
 124         //len of message is also in message (first ulong)
 125         l = (*(uint32_t *)(msg+4))+4;
 126         while (l != 0) {
 127           if (l > 8)
 128             j = 8;
 129           else
 130             j = l;
 131           l -= j;
 132           fprintf(ptr_out, "                    : ");
 133           for (i = 0; i < j; i++, k++)
 134             fprintf(ptr_out, "%02X ", k[0]);
 135           if (l == 0) {
 136             for (i = j; i < 8; i++)
 137               fprintf(ptr_out, "   ");
 138           }
 139           k -= j;
 140           for (i = 0; i < j; i++, k++)
 141             fprintf(ptr_out, "%c", k[0]);
 142           fprintf(ptr_out, "\n");
 143         }
 144         break;
 145       case PID_STRENGTH:
 146         fprintf(ptr_out, "     PID_STRENGTH                         : %u\n",
 147                 *(uint32_t *)(msg+4));
 148         break;
 149       case PID_TYPE_NAME:
 150         fprintf(ptr_out, "     PID_TYPE_NAME                        :\n");
 151         k = msg+4;
 152         //len of message is also in message (first ulong)
 153         l = (*(uint32_t *)(msg+4))+4;
 154         while (l != 0) {
 155           if (l > 8)
 156             j = 8;
 157           else
 158             j = l;
 159           l -= j;
 160           fprintf(ptr_out, "                    : ");
 161           for (i = 0; i < j; i++, k++)
 162             fprintf(ptr_out, "%02X ", k[0]);
 163           if (l == 0) {
 164             for (i = j; i < 8; i++)
 165               fprintf(ptr_out, "   ");
 166           }
 167           k -= j;
 168           for (i = 0; i < j; i++, k++)
 169             fprintf(ptr_out, "%c", k[0]);
 170           fprintf(ptr_out, "\n");
 171         }
 172         break;
 173       case PID_TYPE_CHECKSUM:
 174         fprintf(ptr_out, "     PID_TYPE_CHECKSUM                    : %u\n",
 175                 *(TypeChecksum *)(msg+4));
 176         break;
 177       case RTPS_PID_TYPE2_NAME:
 178         fprintf(ptr_out, "     RTPS_PID_TYPE2_NAME                  :\n");
 179         break;
 180       case RTPS_PID_TYPE2_CHECKSUM:
 181         fprintf(ptr_out, "     RTPS_PID_TYPE2_CHECKSUM              :\n");
 182         break;
 183       case PID_MATATRAFFIC_MULTICAST_IPADDRESS:
 184         fprintf(ptr_out, "     PID_MATATRAFFIC_MULTICAST_IPADDRESS  : %u.%u.%u.%u\n",
 185                 msg[7], msg[6], msg[5], msg[4]);
 186         break;
 187       case PID_APP_IPADDRESS:
 188         fprintf(ptr_out, "     PID_APP_IPADDRESS                    : %u.%u.%u.%u\n",
 189                 msg[7], msg[6], msg[5], msg[4]);
 190         break;
 191       case PID_METATRAFFIC_UNICAST_PORT:
 192         fprintf(ptr_out, "     PID_METATRAFFIC_UNICAST_PORT         : %u\n",
 193                 *(Port *)(msg+4));
 194         break;
 195       case PID_USERDATA_UNICAST_PORT:
 196         fprintf(ptr_out, "     PID_USERDATA_UNICAST_PORT            : %u\n",
 197                 *(Port *)(msg+4));
 198         break;
 199       case PID_EXPECTS_ACK:
 200         fprintf(ptr_out, "     PID_EXPECTS_ACK                      : %u\n",
 201                 *(uint32_t *)(msg+4));
 202         break;
 203       case PID_USERDATA_MULTICAST_IPADDRESS:
 204         fprintf(ptr_out, "     PID_USERDATA_MULTICAST_IPADDRESS     : %u.%u.%u.%u\n",
 205                 msg[7], msg[6], msg[5], msg[4]);
 206         break;
 207       case PID_MANAGER_KEY:
 208         fprintf(ptr_out, "     PID_MANAGER_KEY                      : 0x%02x%02x%02x%02x (%u.%u.%u.%u)\n",
 209                 msg[7], msg[6], msg[5], msg[4], msg[7], msg[6], msg[5], msg[4]);
 210         break;
 211       case PID_SEND_QUEUE_SIZE:
 212         fprintf(ptr_out, "     PID_SEND_QUEUE_SIZE                  : %u\n",
 213                 *(uint32_t *)(msg+4));
 214         break;
 215       case PID_PROTOCOL_VERSION:
 216         pv = *((ProtocolVersion *)(msg+4));
 217         fprintf(ptr_out, "     PID_PROTOCOL_VERSION                 : %u.%u\n",
 218                 pv.major, pv.minor);
 219         break;
 220       case PID_VENDOR_ID:
 221         vid = *((VendorId *)(msg+4));
 222         fprintf(ptr_out, "     PID_VENDOR_ID                        : %u.%u\n",
 223                 vid.major, vid.minor);
 224         break;
 225       case PID_VARGAPPS_SEQUENCE_NUMBER_LAST:
 226         sn = *((SequenceNumber *)(msg+4));
 227         fprintf(ptr_out, "     PID_VARGAPPS_SEQUENCE_NUMBER_LAST    : %u\n", sn.low);
 228         break;
 229       case PID_RECV_QUEUE_SIZE:
 230         fprintf(ptr_out, "     PID_RECV_QUEUE_SIZE                  : %u\n",
 231                 *(uint32_t *)(msg+4));
 232         break;
 233       case PID_RELIABILITY_OFFERED:
 234         fprintf(ptr_out, "     PID_RELIABILITY_OFFERED              : %u\n",
 235                 *(uint32_t *)(msg+4));
 236         break;
 237       case PID_RELIABILITY_REQUESTED:
 238         fprintf(ptr_out, "     PID_RELIABILITY_REQUESTED            : %u\n",
 239                 *(uint32_t *)(msg+4));
 240         break;
 241       default:
 242         fprintf(ptr_out, "     unknow paramerId                 : %d len : %d\n",
 243                 *((ParameterId *)msg), *((ParameterLength *)(msg+2)));
 244         break;
 245     }
 246     msg += (*((ParameterLength *)(msg+2)))+4;                      /* next message */
 247   }
 248   msg += (*((ParameterLength *)(msg+2)))+4;                        /* message end*/
 249 }
 250
 251
 252 /**********************************************************************************/
 253 int
 254 decode_header(uint8_t *msg, unsigned len, MessageInterpret *mi)
 255 {
 256   if (len < 16)
 257     return -1;                                    /* message is to small */
 258   if (msg[0] != 'R')
 259     return -2;                                    /* header is invalid */
 260   if (msg[1] != 'T')
 261     return -2;                                    /* header is invalid */
 262   if (msg[2] != 'P')
 263     return -2;                                    /* header is invalid */
 264   if (msg[3] != 'S')
 265     return -2;                                    /* header is invalid */
 266   mi->sourceVersion = *((ProtocolVersion *)(msg+4)); /* ProtocolVersion */
 267   mi->sourceVendorId = *((VendorId *)(msg+6));       /* Vendor Id */
 268   mi->sourceHostId = *((HostId *)(msg+8));           /* Host Id */
 269   mi->sourceAppId = *((AppId *)(msg+12));            /* App Id */
 270   mi->haveTimestamp = 0;                            /* false */
 271   return 0;
 272 }
 273
 274 /**********************************************************************************/
 275 int
 276 decode_submessage(uint8_t *msg, int ptr, MessageInterpret *mi)
 277 {
 278   IPAddress          ipa;
 279   Port               port;
 280   ObjectId           roid, woid, oid;
 281   SequenceNumber     fsn, lsn, wsn;
 282   uint8_t      i, j, l;
 283   uint8_t      *k;
 284   HostId             hid;
 285   AppId              aid;
 286   int                ptr_off;
 287
 288   switch ((SubmessageId)msg[ptr]) {
 289     case PAD:
 290       fprintf(ptr_out, "   PAD\n");
 291       break;
 292     case VAR:
 293       fprintf(ptr_out, "   VAR\n");
 294       fprintf(ptr_out, "     h-bit          : %d\n", (msg[ptr+1] & 8) ? 1 : 0);
 295       fprintf(ptr_out, "     alive-bit      : %d\n", (msg[ptr+1] & 4) ? 1 : 0);
 296       fprintf(ptr_out, "     p-bit          : %d\n", (msg[ptr+1] & 2) ? 1 : 0);
 297       roid = *((ObjectId *)(msg+ptr+4));
 298       fprintf(ptr_out, "     ObjectId       : %d.%d.%d.%02x",
 299               *(uint8_t *)&roid, *(((uint8_t *)&roid)+1), *(((uint8_t *)&roid)+2), *(((uint8_t *)&roid)+3));
 300       print_obj(&roid);
 301       woid = *((ObjectId *)(msg+ptr+8));
 302       fprintf(ptr_out, "     ObjectId       : %d.%d.%d.%02x",
 303               *(uint8_t *)&woid, *(((uint8_t *)&woid)+1), *(((uint8_t *)&woid)+2), *(((uint8_t *)&woid)+3));
 304       print_obj(&woid);
 305       if (msg[ptr+1] & 8) {
 306         hid = *((HostId *)(msg+ptr+12));                   /* hostId */
 307         aid = *((AppId *)(msg+ptr+16));                   /* appId */
 308         fprintf(ptr_out, "     HostId         : %d.%d.%d.%d\n",
 309                 *(uint8_t *)&hid, *(((uint8_t *)&hid)+1), *(((uint8_t *)&hid)+2), *(((uint8_t *)&hid)+3));
 310         fprintf(ptr_out, "     AppId          : %d.%d.%d.%d ",
 311                 *(uint8_t *)&aid, *(((uint8_t *)&aid)+1), *(((uint8_t *)&aid)+2), *(((uint8_t *)&aid)+3));
 312         print_app(*(((uint8_t *)&aid)+3));
 313         ptr_off = 8;
 314       } else
 315         ptr_off = 0;
 316       oid = *((ObjectId *)(msg+ptr+12+ptr_off));
 317       fprintf(ptr_out, "     ObjectId       : %d.%d.%d.%02x",
 318               *(uint8_t *)&oid, *(((uint8_t *)&oid)+1), *(((uint8_t *)&oid)+2), *(((uint8_t *)&oid)+3));
 319       print_obj(&oid);
 320       wsn = *((SequenceNumber *)(msg+ptr+16+ptr_off));
 321       fprintf(ptr_out, "     sequenceNumber : %u\n", wsn.low);
 322       if (msg[ptr+1] & 2) {
 323         decode_parametr_sequence(msg+ptr+24+ptr_off);
 324       }
 325       break;
 326     case ISSUE:
 327       fprintf(ptr_out, "   ISSUE\n");
 328       roid = *((ObjectId *)(msg+ptr+4));
 329       woid = *((ObjectId *)(msg+ptr+8));
 330       fsn = *((SequenceNumber *)(msg+ptr+12));
 331       fprintf(ptr_out, "     p-bit          : %d\n", (msg[ptr+1] & 2) ? 1 : 0);
 332       fprintf(ptr_out, "     readerObjectId : %d.%d.%d.%02x",
 333               *(uint8_t *)&roid, *(((uint8_t *)&roid)+1), *(((uint8_t *)&roid)+2), *(((uint8_t *)&roid)+3));
 334       print_obj(&roid);
 335       fprintf(ptr_out, "     writeObjectId  : %d.%d.%d.%02x",
 336               *(uint8_t *)&woid, *(((uint8_t *)&woid)+1), *(((uint8_t *)&woid)+2), *(((uint8_t *)&woid)+3));
 337       print_obj(&woid);
 338       fprintf(ptr_out, "     issueSeqNumber : %u\n", fsn.low);
 339       k = msg+ptr+20;
 340       if (msg[ptr+1] & 2) {
 341         decode_parametr_sequence(k);          /* after k is set to end of paramseq. */
 342       }
 343 //      l=(*(unsigned*)(msg+ptr+2))-(k-(msg+ptr+20)+16);
 344       //len of message is also in message (first ulong), internaly defined by RTI !!!
 345       l = (*(uint32_t *)k)+4;
 346       while (l != 0) {
 347         if (l > 8)
 348           j = 8;
 349         else
 350           j = l;
 351         l -= j;
 352         fprintf(ptr_out, "                    : ");
 353         for (i = 0; i < j; i++, k++)
 354           fprintf(ptr_out, "%02X ", k[0]);
 355         if (l == 0) {
 356           for (i = j; i < 8; i++)
 357             fprintf(ptr_out, "   ");
 358         }
 359         k -= j;
 360         for (i = 0; i < j; i++, k++)
 361           fprintf(ptr_out, "%c", k[0]);
 362         fprintf(ptr_out, "\n");
 363       }
 364       break;
 365     case ACK:
 366       fprintf(ptr_out, "   ACK\n");
 367       roid = *((ObjectId *)(msg+ptr+4));
 368       woid = *((ObjectId *)(msg+ptr+8));
 369       fprintf(ptr_out, "     final-bit      : %d\n", (msg[ptr+1] & 2) ? 1 : 0);
 370       fprintf(ptr_out, "     readerObjectId : %d.%d.%d.%02x",
 371               *(uint8_t *)&roid, *(((uint8_t *)&roid)+1), *(((uint8_t *)&roid)+2), *(((uint8_t *)&roid)+3));
 372       print_obj(&roid);
 373       fprintf(ptr_out, "     writeObjectId  : %d.%d.%d.%02x",
 374               *(uint8_t *)&woid, *(((uint8_t *)&woid)+1), *(((uint8_t *)&woid)+2), *(((uint8_t *)&woid)+3));
 375       print_obj(&woid);
 376       fprintf(ptr_out, "     bitmap         : %u/%u:", *((uint32_t *)(msg+ptr+16)),
 377               *((uint32_t *)(msg+ptr+20)));
 378       for (i = 0; i < msg[ptr+20]/8; ) {
 379         for (j = 128; j != 0; j >>= 1)
 380           fprintf(ptr_out, "%d", (msg[ptr+24+i] & j) ? 1 : 0);
 381         fprintf(ptr_out, " ");
 382         if ((!(++i % 4)) && (i < msg[ptr+20]/8))
 383           fprintf(ptr_out, "\n                          :");
 384       }
 385       fprintf(ptr_out, "\n");
 386       break;
 387     case HEARTBEAT:
 388       fprintf(ptr_out, "   HEARTBEAT\n");
 389       roid = *((ObjectId *)(msg+ptr+4));
 390       woid = *((ObjectId *)(msg+ptr+8));
 391       fsn = *((SequenceNumber *)(msg+ptr+12));
 392       lsn = *((SequenceNumber *)(msg+ptr+20));
 393       fprintf(ptr_out, "     final-bit      : %d\n", (msg[ptr+1] & 2) ? 1 : 0);
 394       fprintf(ptr_out, "     readerObjectId : %d.%d.%d.%02x",
 395               *(uint8_t *)&roid, *(((uint8_t *)&roid)+1), *(((uint8_t *)&roid)+2), *(((uint8_t *)&roid)+3));
 396       print_obj(&roid);
 397       fprintf(ptr_out, "     writeObjectId  : %d.%d.%d.%02x",
 398               *(uint8_t *)&woid, *(((uint8_t *)&woid)+1), *(((uint8_t *)&woid)+2), *(((uint8_t *)&woid)+3));
 399       print_obj(&woid);
 400       fprintf(ptr_out, "     firstSeqNumber : %u\n", fsn.low);
 401       fprintf(ptr_out, "     lastSeqNumber  : %u\n", lsn.low);
 402       break;
 403     case GAP:
 404       fprintf(ptr_out, "   GAP\n");
 405       roid = *((ObjectId *)(msg+ptr+4));
 406       woid = *((ObjectId *)(msg+ptr+8));
 407       fsn = *((SequenceNumber *)(msg+ptr+12));
 408       lsn = *((SequenceNumber *)(msg+ptr+20));
 409       fprintf(ptr_out, "     readerObjectId : %d.%d.%d.%02x",
 410               *(uint8_t *)&roid, *(((uint8_t *)&roid)+1), *(((uint8_t *)&roid)+2), *(((uint8_t *)&roid)+3));
 411       print_obj(&roid);
 412       fprintf(ptr_out, "     writeObjectId  : %d.%d.%d.%02x",
 413               *(uint8_t *)&woid, *(((uint8_t *)&woid)+1), *(((uint8_t *)&woid)+2), *(((uint8_t *)&woid)+3));
 414       print_obj(&woid);
 415       fprintf(ptr_out, "     firstSeqNumber : %u\n", fsn.low);
 416       fprintf(ptr_out, "     bitmap         : %u/%u:", lsn.low,
 417               *((uint32_t *)(msg+ptr+28)));
 418       for (i = 0; i < (*((uint32_t *)(msg+ptr+28))); i++) {
 419         l = ((*(((uint32_t *)(msg+ptr+32))+i/32)) & (1<<(31-i%32))) ? 1 : 0;
 420         fprintf(ptr_out, "%d", l);
 421       }
 422       fprintf(ptr_out, "\n");
 423       break;
 424     case INFO_TS:
 425       fprintf(ptr_out, "   INFO_TS\n");
 426       fprintf(ptr_out, "     i-bit          : %d\n", (msg[ptr+1] & 2) ? 1 : 0);
 427       if (!(msg[ptr+1]&2)) {
 428         mi->timestamp = *((NtpTime *)(msg+ptr+4));
 429         fprintf(ptr_out, "     ntpTimeStamp   : %u,%u\n",
 430                 mi->timestamp.seconds, mi->timestamp.fraction);
 431       }
 432       break;
 433     case INFO_SRC:
 434       fprintf(ptr_out, "   INFO_SRC\n");
 435       break;
 436     case INFO_REPLY:
 437       fprintf(ptr_out, "   INFO_REPLY\n");
 438       ipa = *((uint32_t *)(msg+ptr+4));                  /* unicastReplyIPAddress */
 439       port = *((uint32_t *)(msg+ptr+8));                 /* unicastReplyPort */
 440       fprintf(ptr_out, "     unicastReplyIPAddress   : %d.%d.%d.%d\n",
 441               *(uint8_t *)&ipa, *(((uint8_t *)&ipa)+1), *(((uint8_t *)&ipa)+2), *(((uint8_t *)&ipa)+3));
 442       fprintf(ptr_out, "     unicastReplyPort        : %u\n", port);
 443       if (ipa != IPADDRESS_INVALID) {
 444         mi->unicastReplyIPAddress = ipa;
 445       }
 446       mi->unicastReplyPort = port;
 447       if ((msg[ptr+1] & 0x02) != 0) {
 448         ipa = *((uint32_t *)(msg+ptr+12));               /* multicastReplyIPAddress */
 449         port = *((uint32_t *)(msg+ptr+16));              /* multicastReplyPort */
 450         fprintf(ptr_out, "     multicastReplyIPAddress : %d.%d.%d.%d\n",
 451                 *(uint8_t *)&ipa, *(((uint8_t *)&ipa)+1), *(((uint8_t *)&ipa)+2), *(((uint8_t *)&ipa)+3));
 452         fprintf(ptr_out, "     multicastReplyPort      : %u\n", port);
 453         mi->multicastReplyIPAddress = ipa;
 454         mi->multicastReplyPort = port;
 455       } else {
 456         mi->multicastReplyIPAddress = IPADDRESS_INVALID;
 457         mi->multicastReplyPort     = PORT_INVALID;
 458       }
 459       break;
 460     case INFO_DST:
 461       fprintf(ptr_out, "   INFO_DST\n");
 462       hid = *((HostId *)(msg+ptr+4));                   /* hostId */
 463       aid = *((AppId *)(msg+ptr+8));                   /* appId */
 464       fprintf(ptr_out, "     HostId                  : %d.%d.%d.%d\n",
 465               *(uint8_t *)&hid, *(((uint8_t *)&hid)+1), *(((uint8_t *)&hid)+2), *(((uint8_t *)&hid)+3));
 466       fprintf(ptr_out, "     AppId                   : %d.%d.%d.%d ",
 467               *(uint8_t *)&aid, *(((uint8_t *)&aid)+1), *(((uint8_t *)&aid)+2), *(((uint8_t *)&aid)+3));
 468       print_app(*(((uint8_t *)&aid)+3));
 469       break;
 470     default:
 471       fprintf(ptr_out, "   undefined submessage code\n");
 472       break;
 473   }
 474   return 0;
 475 }
 476
 477
 478 /**********************************************************************************/
 479 int
 480 main(int argc, char *argv[])
 481 {
 482   uint8_t rtps_msg[16384], rtps_msg_c[3] = {0, 0, 0};
 483   unsigned rtps_msg_ptr = 0, rtps_msg_len, rtps_submsg_cnt, submsg_len;
 484   int err, c, no_lchar = 1;
 485   MessageInterpret mi;
 486
 487   printf("-------------------------------------------------------------------------------\n");
 488   printf("             RTPS messages decoder                OCERA project                \n");
 489   printf("                                                                               \n");
 490   printf("-------------------------------------------------------------------------------\n");
 491   if (argc != 3) {
 492     printf(" syntax :    decode_msg 'filename to decode' 'output file'\n\n");
 493     return 0;
 494   }
 495   printf(" decode file    : %s\n output file    : %s\n\n", argv[1], argv[2]);
 496   ptr_in = fopen(argv[1], "r");                            /* open input file */
 497   if (ferror(ptr_in)) {
 498     printf(" unable to open %s\n", argv[1]);
 499   }
 500   ptr_out = fopen(argv[2], "w");                           /* create output file */
 501   if (ferror(ptr_out)) {
 502     printf(" unable to create %s\n", argv[2]);
 503   }
 504   while ((c = fgetc(ptr_in)) != EOF) {                     /* read one character */
 505     if (((no_lchar == 1) && (!isdigit(c))) || (no_lchar == 0)) { /* outside RTPS line */
 506       if (rtps_msg_ptr) {                                 /* proc. data read data ? */
 507         rtps_msg[rtps_msg_ptr] = 0;                         /* end of rtps data */
 508         rtps_msg_len = rtps_msg_ptr;
 509         rtps_msg_ptr = 0;
 510         printf(" message length : %d\n", rtps_msg_len);
 511         /* decoded message - msg stored in rtps_msg, length msg in rtps_msg_len */
 512         if ((err = decode_header(rtps_msg, rtps_msg_len, &mi)) == 0) {
 513           fprintf(ptr_out, "message length      : %d\n", rtps_msg_len);
 514           fprintf(ptr_out, " Header\n");
 515           fprintf(ptr_out, "   protocol version : %d.%d \
 516                          \n   vendorId         : %d.%d \
 517                          \n   hostId           : %d.%d.%d.%d \
 518                          \n   appId            : %d.%d.%d.%d ",
 519                   mi.sourceVersion.major, mi.sourceVersion.minor, mi.sourceVendorId.major,
 520                   mi.sourceVendorId.minor, *(u_char *)&mi.sourceHostId, *(((u_char *)&mi.sourceHostId)+1),
 521                   *(((u_char *)&mi.sourceHostId)+2), *(((u_char *)&mi.sourceHostId)+3), *(((u_char *)&mi.sourceAppId)),
 522                   *(((u_char *)&mi.sourceAppId)+1), *(((u_char *)&mi.sourceAppId)+2), *(((u_char *)&mi.sourceAppId)+3));
 523           print_app(*(((u_char *)&mi.sourceAppId)+3));
 524           rtps_msg_ptr = 16;
 525           rtps_submsg_cnt = 1;
 526           do {
 527             fprintf(ptr_out, " Submessage : %d", rtps_submsg_cnt++);
 528             if ((rtps_msg_ptr+3) <= rtps_msg_len) {  /* len. submessage header is OK */
 529               submsg_len = rtps_msg[rtps_msg_ptr+2]+rtps_msg[rtps_msg_ptr+3]*0x100;
 530               fprintf(ptr_out, " len : %d\n", submsg_len+4);
 531               if ((submsg_len+rtps_msg_ptr+3) <= rtps_msg_len) {
 532                 /* decode submessage */
 533                 decode_submessage(rtps_msg, rtps_msg_ptr, &mi);
 534                 rtps_msg_ptr += submsg_len+4;
 535               } else
 536                 rtps_msg_ptr = rtps_msg_len;       /* submessage is too small */
 537             } else {
 538               rtps_msg_ptr = rtps_msg_len;           /* submessage is too small */
 539               fprintf(ptr_out, "\n");
 540             }
 541           } while (rtps_msg_ptr < rtps_msg_len);
 542         } else {
 543           fprintf(ptr_out, "invalid header - error code : %d\n", err);
 544         }
 545         rtps_msg_ptr = 0;
 546       }
 547       no_lchar = 0;
 548       if (c == '\n')
 549         no_lchar = 1;
 550       fputc(c, ptr_out);                                   /* copy to output file */
 551     }
 552     if (no_lchar > 1) {                                     /* reading RTPS message*/
 553       no_lchar++;
 554       if (c == '\n')
 555         no_lchar = 1;
 556       else {
 557         if ((no_lchar > 5) && (no_lchar < 57)) {              /* converted data from */
 558           if ((rtps_msg_c[0] != 0) && (isxdigit(c))) {      /* position 6 to 56    */
 559             rtps_msg_c[1] = (uint8_t)c;
 560             rtps_msg[rtps_msg_ptr++] = (uint8_t)strtol(rtps_msg_c, NULL, 16);
 561             rtps_msg_c[0] = 0;
 562           } else {
 563             if ((rtps_msg_c[0] == 0) && (isxdigit(c)))
 564               rtps_msg_c[0] = (uint8_t)c;
 565           }
 566         }
 567       }
 568     }
 569     if ((no_lchar == 1) && (isxdigit(c)))
 570       no_lchar++;                                          /* is begin RTPS line ? */
 571   }
 572   fclose(ptr_in);
 573   fclose(ptr_out);
 574   return 0;
 575 }