8 #include <typedefs_defines_rtps.h>
11 FILE *ptr_in, *ptr_out;
14 /**********************************************************************************/
16 conv_u32(uint32_t *x, uint8_t ef)
18 #if __BYTE_ORDER == __LITTLE_ENDIAN
21 #elif __BYTE_ORDER == __BIG_ENDIAN
27 /**********************************************************************************/
31 fprintf(ptr_out, " (");
32 switch (bswap_32(*((uint32_t *)obj))) {
34 fprintf(ptr_out, "ManagedApplicaton or Manager");
36 case OID_WRITE_APPSELF:
37 fprintf(ptr_out, "Write appself");
40 fprintf(ptr_out, "Write app");
43 fprintf(ptr_out, "Read app");
46 fprintf(ptr_out, "Write mgr");
49 fprintf(ptr_out, "Read mgr");
52 fprintf(ptr_out, "Write publ");
55 fprintf(ptr_out, "Read publ");
58 fprintf(ptr_out, "Write subs");
61 fprintf(ptr_out, "Read subs");
64 fprintf(ptr_out, "unknown");
67 fprintf(ptr_out, ")\n");
70 /**********************************************************************************/
72 print_app(uint8_t app)
75 case MANAGEDAPPLICATION:
76 fprintf(ptr_out, "(managed application)\n");
79 fprintf(ptr_out, "(manager)\n");
82 fprintf(ptr_out, "(unknown class)\n");
87 /**********************************************************************************/
89 decode_parametr_sequence(uint8_t *msg)
98 while (*((ParameterId *)msg) != PID_SENTINEL) {
99 printf("id%d,len%d\n", *((ParameterId *)msg), *((ParameterLength *)(msg+2)));
100 if ((*((ParameterLength *)(msg+2))) == 0)
102 switch (*(ParameterId *)msg) {
104 fprintf(ptr_out, " PID_PAD :\n");
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);
111 case PID_PERSISTENCE:
112 ts = *((NtpTime *)(msg+4));
113 fprintf(ptr_out, " PID_PERSISTENCE : %u,%u\n",
114 ts.seconds, ts.fraction);
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);
122 fprintf(ptr_out, " PID_TOPIC :\n");
124 //len of message is also in message (first ulong)
125 l = (*(uint32_t *)(msg+4))+4;
132 fprintf(ptr_out, " : ");
133 for (i = 0; i < j; i++, k++)
134 fprintf(ptr_out, "%02X ", k[0]);
136 for (i = j; i < 8; i++)
137 fprintf(ptr_out, " ");
140 for (i = 0; i < j; i++, k++)
141 fprintf(ptr_out, "%c", k[0]);
142 fprintf(ptr_out, "\n");
146 fprintf(ptr_out, " PID_STRENGTH : %u\n",
147 *(uint32_t *)(msg+4));
150 fprintf(ptr_out, " PID_TYPE_NAME :\n");
152 //len of message is also in message (first ulong)
153 l = (*(uint32_t *)(msg+4))+4;
160 fprintf(ptr_out, " : ");
161 for (i = 0; i < j; i++, k++)
162 fprintf(ptr_out, "%02X ", k[0]);
164 for (i = j; i < 8; i++)
165 fprintf(ptr_out, " ");
168 for (i = 0; i < j; i++, k++)
169 fprintf(ptr_out, "%c", k[0]);
170 fprintf(ptr_out, "\n");
173 case PID_TYPE_CHECKSUM:
174 fprintf(ptr_out, " PID_TYPE_CHECKSUM : %u\n",
175 *(TypeChecksum *)(msg+4));
177 case RTPS_PID_TYPE2_NAME:
178 fprintf(ptr_out, " RTPS_PID_TYPE2_NAME :\n");
180 case RTPS_PID_TYPE2_CHECKSUM:
181 fprintf(ptr_out, " RTPS_PID_TYPE2_CHECKSUM :\n");
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]);
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]);
191 case PID_METATRAFFIC_UNICAST_PORT:
192 fprintf(ptr_out, " PID_METATRAFFIC_UNICAST_PORT : %u\n",
195 case PID_USERDATA_UNICAST_PORT:
196 fprintf(ptr_out, " PID_USERDATA_UNICAST_PORT : %u\n",
199 case PID_EXPECTS_ACK:
200 fprintf(ptr_out, " PID_EXPECTS_ACK : %u\n",
201 *(uint32_t *)(msg+4));
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]);
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]);
211 case PID_SEND_QUEUE_SIZE:
212 fprintf(ptr_out, " PID_SEND_QUEUE_SIZE : %u\n",
213 *(uint32_t *)(msg+4));
215 case PID_PROTOCOL_VERSION:
216 pv = *((ProtocolVersion *)(msg+4));
217 fprintf(ptr_out, " PID_PROTOCOL_VERSION : %u.%u\n",
221 vid = *((VendorId *)(msg+4));
222 fprintf(ptr_out, " PID_VENDOR_ID : %u.%u\n",
223 vid.major, vid.minor);
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);
229 case PID_RECV_QUEUE_SIZE:
230 fprintf(ptr_out, " PID_RECV_QUEUE_SIZE : %u\n",
231 *(uint32_t *)(msg+4));
233 case PID_RELIABILITY_OFFERED:
234 fprintf(ptr_out, " PID_RELIABILITY_OFFERED : %u\n",
235 *(uint32_t *)(msg+4));
237 case PID_RELIABILITY_REQUESTED:
238 fprintf(ptr_out, " PID_RELIABILITY_REQUESTED : %u\n",
239 *(uint32_t *)(msg+4));
242 fprintf(ptr_out, " unknow paramerId : %d len : %d\n",
243 *((ParameterId *)msg), *((ParameterLength *)(msg+2)));
246 msg += (*((ParameterLength *)(msg+2)))+4; /* next message */
248 msg += (*((ParameterLength *)(msg+2)))+4; /* message end*/
252 /**********************************************************************************/
254 decode_header(uint8_t *msg, unsigned len, MessageInterpret *mi)
257 return -1; /* message is to small */
259 return -2; /* header is invalid */
261 return -2; /* header is invalid */
263 return -2; /* header is invalid */
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 */
274 /**********************************************************************************/
276 decode_submessage(uint8_t *msg, int ptr, MessageInterpret *mi)
280 ObjectId roid, woid, oid;
281 SequenceNumber fsn, lsn, wsn;
288 switch ((SubmessageId)msg[ptr]) {
290 fprintf(ptr_out, " PAD\n");
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));
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));
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));
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));
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);
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));
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));
338 fprintf(ptr_out, " issueSeqNumber : %u\n", fsn.low);
340 if (msg[ptr+1] & 2) {
341 decode_parametr_sequence(k); /* after k is set to end of paramseq. */
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;
352 fprintf(ptr_out, " : ");
353 for (i = 0; i < j; i++, k++)
354 fprintf(ptr_out, "%02X ", k[0]);
356 for (i = j; i < 8; i++)
357 fprintf(ptr_out, " ");
360 for (i = 0; i < j; i++, k++)
361 fprintf(ptr_out, "%c", k[0]);
362 fprintf(ptr_out, "\n");
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));
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));
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 :");
385 fprintf(ptr_out, "\n");
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));
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));
400 fprintf(ptr_out, " firstSeqNumber : %u\n", fsn.low);
401 fprintf(ptr_out, " lastSeqNumber : %u\n", lsn.low);
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));
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));
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);
422 fprintf(ptr_out, "\n");
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);
434 fprintf(ptr_out, " INFO_SRC\n");
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;
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;
456 mi->multicastReplyIPAddress = IPADDRESS_INVALID;
457 mi->multicastReplyPort = PORT_INVALID;
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));
471 fprintf(ptr_out, " undefined submessage code\n");
478 /**********************************************************************************/
480 main(int argc, char *argv[])
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;
487 printf("-------------------------------------------------------------------------------\n");
488 printf(" RTPS messages decoder OCERA project \n");
490 printf("-------------------------------------------------------------------------------\n");
492 printf(" syntax : decode_msg 'filename to decode' 'output file'\n\n");
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]);
500 ptr_out = fopen(argv[2], "w"); /* create output file */
501 if (ferror(ptr_out)) {
502 printf(" unable to create %s\n", argv[2]);
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;
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));
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;
536 rtps_msg_ptr = rtps_msg_len; /* submessage is too small */
538 rtps_msg_ptr = rtps_msg_len; /* submessage is too small */
539 fprintf(ptr_out, "\n");
541 } while (rtps_msg_ptr < rtps_msg_len);
543 fprintf(ptr_out, "invalid header - error code : %d\n", err);
550 fputc(c, ptr_out); /* copy to output file */
552 if (no_lchar > 1) { /* reading RTPS message*/
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);
563 if ((rtps_msg_c[0] == 0) && (isxdigit(c)))
564 rtps_msg_c[0] = (uint8_t)c;
569 if ((no_lchar == 1) && (isxdigit(c)))
570 no_lchar++; /* is begin RTPS line ? */