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