src/vpn/pretty-print.c

   1 #include <stdlib.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4 #include <ctype.h>
   5 #include <arpa/inet.h>
   6
   7 #include "packet.h"
   8
   9 static char* pretty = /*{{{*/
  10 /*     0       1         2         3         4        5          6
  11  0123456789012345678901234567890123456789012345678901234567890123456789 */
  12 "IPv6-Paket from xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx    \n" //60
  13 "             to xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx    \n" //120
  14 /*     0       1         2         3         4        5          6
  15  0123456789012345678901234567890123456789012345678901234567890123456789 */
  16 "        flow    0xXXX (        )                           \n" //180
  17 "        length  0xXX  (   )                                \n" //240
  18 "        nexthdr 0xXX  (                                    \n" //300
  19 "        hoplmt  0xXX  (   )                                \n" //360
  20 "first 128 bytes of payload:                                \n" //420
  21 /*     0       1         2         3         4        5          6
  22  0123456789012345678901234567890123456789012345678901234567890123456789 */
  23 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //490
  24 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //560
  25 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //630
  26 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //700
  27 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //770
  28 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //840
  29 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //910
  30 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n";//980
  31 /*}}}*/
  32
  33 static void pp_ip6adr(unsigned char* adr, char* dest) {{{
  34         char tmp[3];
  35
  36         sprintf(tmp, "%02X", adr[0]);
  37         memcpy(dest+0, tmp, 2);
  38         sprintf(tmp, "%02X", adr[1]);
  39         memcpy(dest+2, tmp, 2);
  40
  41         sprintf(tmp, "%02X", adr[2]);
  42         memcpy(dest+5, tmp, 2);
  43         sprintf(tmp, "%02X", adr[3]);
  44         memcpy(dest+7, tmp, 2);
  45
  46         sprintf(tmp, "%02X", adr[4]);
  47         memcpy(dest+10, tmp, 2);
  48         sprintf(tmp, "%02X", adr[5]);
  49         memcpy(dest+12, tmp, 2);
  50
  51         sprintf(tmp, "%02X", adr[6]);
  52         memcpy(dest+15, tmp, 2);
  53         sprintf(tmp, "%02X", adr[7]);
  54         memcpy(dest+17, tmp, 2);
  55
  56         sprintf(tmp, "%02X", adr[8]);
  57         memcpy(dest+20, tmp, 2);
  58         sprintf(tmp, "%02X", adr[9]);
  59         memcpy(dest+22, tmp, 2);
  60
  61         sprintf(tmp, "%02X", adr[10]);
  62         memcpy(dest+25, tmp, 2);
  63         sprintf(tmp, "%02X", adr[11]);
  64         memcpy(dest+27, tmp, 2);
  65
  66         sprintf(tmp, "%02X", adr[12]);
  67         memcpy(dest+30, tmp, 2);
  68         sprintf(tmp, "%02X", adr[13]);
  69         memcpy(dest+32, tmp, 2);
  70
  71         sprintf(tmp, "%02X", adr[14]);
  72         memcpy(dest+35, tmp, 2);
  73         sprintf(tmp, "%02X", adr[15]);
  74         memcpy(dest+37, tmp, 2);
  75 }}}
  76
  77 void pp_hexdump(unsigned char* data, char* dest, int max) {{{
  78         char tmp[3];
  79         char tmp2[2];
  80         int off = 0;
  81         int to = max > 16 ? 16 : max;
  82         for (int i = 0; i < to; i++) {
  83                 if (i == 8) off = 1;
  84                 sprintf(tmp, "%02x", data[i]);
  85                 memcpy(dest+(3*i)+off, tmp, 2);
  86                 if (isprint(data[i])) {
  87                         sprintf(tmp2, "%c", data[i]);
  88                         memcpy(dest+51+i, tmp2, 1);
  89                 }
  90         }
  91 }}}
  92
  93 void pp_write_header(char* dest, struct ip6_pkt* pkt) {{{
  94         switch (pkt->hdr.nxthdr) {
  95                 case 0x3a:
  96                         memcpy(dest, "ICMPv6)", 7);
  97                         break;
  98                 case 0x06:
  99                         memcpy(dest, "TCP)", 4);
 100                         break;
 101                 default:
 102                         memcpy(dest, "unknown)", 8);
 103                         break;
 104         }
 105 }}}
 106
 107 void pkt_printf(struct ip6_pkt* pkt) {{{
 108         char* buf = (char*)malloc(strlen(pretty)+1);
 109         char tmp[9];
 110
 111         memcpy(buf, pretty, strlen(pretty)+1);
 112
 113         pp_ip6adr(pkt->hdr.sadr, buf+16);
 114         pp_ip6adr(pkt->hdr.dadr, buf+76);
 115
 116         int flow = (ntohl(pkt->hdr.flowlbl));
 117         sprintf(tmp, "%03x", flow);
 118         memcpy(buf+138, tmp, 3);
 119         sprintf(tmp, "%-8d", flow);
 120         memcpy(buf+143, tmp, 8);
 121
 122         int length = ntohs(pkt->hdr.paylgth);
 123         sprintf(tmp, "%02x", length);
 124         memcpy(buf+198, tmp, 2);
 125         sprintf(tmp, "%-3d", length);
 126         memcpy(buf+203, tmp, 3);
 127
 128         sprintf(tmp, "%02x", pkt->hdr.nxthdr);
 129         memcpy(buf+258, tmp, 2);
 130         pp_write_header(buf+263, pkt);
 131
 132         sprintf(tmp, "%02x", pkt->hdr.hoplmt);
 133         memcpy(buf+318, tmp, 2);
 134         sprintf(tmp, "%-3d", pkt->hdr.hoplmt);
 135         memcpy(buf+323, tmp, 3);
 136
 137         int size = payload(&pkt->hdr);
 138         for(int i = 0; i < 8; i++) {
 139                 if (16*i > size) break;
 140                 pp_hexdump(pkt->data + (16*i), buf + 420 + (i*70), size - 16*i);
 141         }
 142
 143         printf(buf);
 144         free(buf);
 145 }}}
 146
 147 void pkt_printf_ip6tcp(struct ip6_tcp* pkt) {{{
 148         printf("spt: %u\n", ntohs(pkt->data.spt));
 149         printf("dpt: %u\n", ntohs(pkt->data.dpt));
 150         printf("seq: %u\n", ntohs(pkt->data.seq));
 151         printf("ack: %u\n", ntohs(pkt->data.ack));
 152         printf("off: %u\n", ntohs(pkt->data.off));
 153         printf("wsz: %u\n", ntohs(pkt->data.wsz));
 154         printf("crc: 0x%x\n", ntohs(pkt->data.crc));
 155         printf("urg: %u\n", ntohs(pkt->data.urg));
 156         printf("flags: %c%c%c%c%c%c%c%c\n",
 157                         pkt->data.flg & 0x80 ? 'C' : '.',
 158                         pkt->data.flg & 0x40 ? 'E' : '.',
 159                         pkt->data.flg & 0x20 ? 'U' : '.',
 160                         pkt->data.flg & 0x10 ? 'A' : '.',
 161                         pkt->data.flg & 0x08 ? 'P' : '.',
 162                         pkt->data.flg & 0x04 ? 'R' : '.',
 163                         pkt->data.flg & 0x02 ? 'S' : '.',
 164                         pkt->data.flg & 0x01 ? 'F' : '.'
 165                         );
 166 }}}