src/vpn/packet.c

   1 #include <errno.h>
   2 #include <stdlib.h>
   3 #include <stdio.h>
   4 #include <string.h>
   5 #include <unistd.h>
   6 #include <sys/uio.h>
   7
   8 #include <linux/if_tun.h>
   9
  10 #include "debug.h"
  11 #include "packet.h"
  12
  13 static long payload(struct ip6_pkt* pkt) {
  14         return (pkt->paylgth[0] << 8) + pkt->paylgth[1];
  15 }
  16
  17 static char* pretty = /*{{{*/
  18 /*     0       1         2         3         4        5          6
  19  0123456789012345678901234567890123456789012345678901234567890123456789 */
  20 "IPv6-Paket from xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx    \n" //60
  21 "             to xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx    \n" //120
  22 "        flow    0xXXX (        )                           \n" //180
  23 "        length  0xXX  (   )                                \n" //240
  24 "        nexthdr 0xXX  (                                    \n" //300
  25 "        hoplmt  0xXX  (   )                                \n" //360
  26 "first 128 bytes of payload:                                \n" //420
  27 /*     0       1         2         3         4        5          6
  28  0123456789012345678901234567890123456789012345678901234567890123456789 */
  29 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //490
  30 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //560
  31 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //630
  32 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //700
  33 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //770
  34 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //840
  35 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n" //910
  36 "XX XX XX XX XX XX XX XX  XX XX XX XX XX XX XX XX | ................  \n";//980
  37 /*}}}*/
  38
  39 void send_pkt(int fd, struct ip6_pkt* pkt) {{{
  40         int sz = payload(pkt);
  41         int w = 0;
  42         char* buf = (char*)malloc(sz+40);
  43
  44         buf[0] = (6 << 4) | (pkt->tclass >> 4);
  45         buf[1] = (pkt->tclass << 4) | (pkt->flowlbl[0] >> 4);
  46         buf[2] = pkt->flowlbl[1];
  47         buf[3] = pkt->flowlbl[2];
  48         buf[4] = pkt->paylgth[0];
  49         buf[5] = pkt->paylgth[1];
  50         buf[6] = pkt->nxthdr;
  51         buf[7] = pkt->hoplmt;
  52
  53         for (w = 0; w < 16; w++) {
  54                 buf[8+w] = pkt->sadr[w];
  55                 buf[24+w] = pkt->dadr[w];
  56         }
  57
  58         memcpy(buf+40, pkt->data, sz);
  59
  60         w = 0;
  61         while ( w > 0) {
  62                 int t = write(fd, buf+w, (sz + 40) - w);
  63                 if (t < 0)
  64                         debug(1, 0, "packet: write : %s\n", strerror(errno));
  65                 else
  66                         w+=t;
  67         }
  68
  69         free(buf);
  70 }}}
  71
  72 int recv_ipv6pkt(int fd, struct pkt_tun** pkt, unsigned char* data) {{{
  73         int size = (data[4] << 8) + data[5] + 40;
  74
  75         debug(1, 0, "read the size: %d\n", size);
  76
  77         (*pkt)->data = (unsigned char*)malloc(size);
  78
  79         memcpy((*pkt)->data, data, size);
  80
  81         return size;
  82 }}}
  83
  84 int recv_pkt(int fd, struct pkt_tun** pkt) {{{
  85         struct pkt_tun* _pkt = (struct pkt_tun*)malloc(sizeof(struct pkt_tun));
  86         *pkt = _pkt;
  87
  88         unsigned char data[1500];
  89         unsigned char buf[4];
  90
  91         struct iovec vect[2];
  92         vect[0].iov_len = sizeof(struct tun_pi);
  93         vect[0].iov_base = &buf;
  94         vect[1].iov_len = 1500;
  95         vect[1].iov_base = data;
  96
  97         int r = 0;
  98
  99         debug(1, 0, "beginning to read...\n");
 100
 101         r = readv(fd, vect, 2);
 102
 103         _pkt->flags[0] = buf[0];
 104         _pkt->flags[1] = buf[1];
 105         _pkt->type[0] = buf[2];
 106         _pkt->type[1] = buf[3];
 107
 108         debug(1, 0, "read the flags: %02x%02x\n", _pkt->flags[0], _pkt->flags[1]);
 109         debug(1, 0, "read the type: %02x%02x\n", _pkt->type[0], _pkt->type[1]);
 110
 111         switch((_pkt->type[0] << 8) + _pkt->type[1]) {
 112                 case 0x86dd:
 113                         return recv_ipv6pkt(fd, pkt, data);
 114                         break;
 115                 case 0x0800:
 116                         debug(1, 0, "unknown pkt-type: IPv4\n");
 117                         //IPv4 TODO
 118                         break;
 119                 default:
 120                         debug(1, 0, "unknown pkt-type: 0x%02x\n", 0x800);
 121                         //Whatever TODO
 122                         break;
 123         }
 124         return -1;
 125 }}}
 126
 127 struct ip6_pkt* parse_ip6(struct pkt_tun* pkt) {{{
 128         struct ip6_pkt* pkt6 = (struct ip6_pkt*)malloc(sizeof(struct ip6_pkt));
 129
 130         pkt6->tclass = pkt->data[0] << 4 | pkt->data[1] >> 4;
 131         pkt6->flowlbl[0] = pkt->data[1]>>4;
 132         pkt6->flowlbl[1] = pkt->data[2];
 133         pkt6->flowlbl[2] = pkt->data[3];
 134
 135         pkt6->paylgth[0] = pkt->data[4];
 136         pkt6->paylgth[1] = pkt->data[5];
 137
 138         pkt6->nxthdr = pkt->data[6];
 139         pkt6->hoplmt = pkt->data[7];
 140
 141         for (int w = 0; w < 16; w++) {
 142                 pkt6->sadr[w] = pkt->data[8+w];
 143                 pkt6->dadr[w] = pkt->data[24+w];
 144         }
 145
 146         pkt6->data = (unsigned char*)malloc(payload(pkt6));
 147         memcpy(pkt6->data, pkt->data+40, payload(pkt6));
 148
 149         return pkt6;
 150 }}}
 151
 152 static void pp_ip6adr(unsigned char* adr, char* dest) {{{
 153         char tmp[3];
 154
 155         sprintf(tmp, "%02X", adr[0]);
 156         memcpy(dest+0, tmp, 2);
 157         sprintf(tmp, "%02X", adr[1]);
 158         memcpy(dest+2, tmp, 2);
 159
 160         sprintf(tmp, "%02X", adr[2]);
 161         memcpy(dest+5, tmp, 2);
 162         sprintf(tmp, "%02X", adr[3]);
 163         memcpy(dest+7, tmp, 2);
 164
 165         sprintf(tmp, "%02X", adr[4]);
 166         memcpy(dest+10, tmp, 2);
 167         sprintf(tmp, "%02X", adr[5]);
 168         memcpy(dest+12, tmp, 2);
 169
 170         sprintf(tmp, "%02X", adr[6]);
 171         memcpy(dest+15, tmp, 2);
 172         sprintf(tmp, "%02X", adr[7]);
 173         memcpy(dest+17, tmp, 2);
 174
 175         sprintf(tmp, "%02X", adr[8]);
 176         memcpy(dest+20, tmp, 2);
 177         sprintf(tmp, "%02X", adr[9]);
 178         memcpy(dest+22, tmp, 2);
 179
 180         sprintf(tmp, "%02X", adr[10]);
 181         memcpy(dest+25, tmp, 2);
 182         sprintf(tmp, "%02X", adr[11]);
 183         memcpy(dest+27, tmp, 2);
 184
 185         sprintf(tmp, "%02X", adr[12]);
 186         memcpy(dest+30, tmp, 2);
 187         sprintf(tmp, "%02X", adr[13]);
 188         memcpy(dest+32, tmp, 2);
 189
 190         sprintf(tmp, "%02X", adr[14]);
 191         memcpy(dest+35, tmp, 2);
 192         sprintf(tmp, "%02X", adr[15]);
 193         memcpy(dest+37, tmp, 2);
 194 }}}
 195
 196 void pp_hexdump(unsigned char* data, char* dest, int max) {
 197         char tmp[3];
 198         int to = max > 8 ? 8 : max;
 199         for (int i = 0; i < to; i++) {
 200                 sprintf(tmp, "%02x", data[i]);
 201                 memcpy(dest+(3*i), tmp, 2);
 202         }
 203 }
 204
 205 void pkt_printf(struct ip6_pkt* pkt) {
 206         char* buf = (char*)malloc(strlen(pretty)+1);
 207         char tmp[4];
 208
 209         memcpy(buf, pretty, strlen(pretty)+1);
 210
 211         pp_ip6adr(pkt->sadr, buf+16);
 212         pp_ip6adr(pkt->dadr, buf+76);
 213
 214         sprintf(tmp, "%03x", (pkt->flowlbl[0] << 16) + (pkt->flowlbl[1] << 8) + (pkt->flowlbl[2]));
 215         memcpy(buf+138, tmp, 3);
 216
 217         sprintf(tmp, "%02x", (pkt->paylgth[0] << 8) + (pkt->paylgth[1]));
 218         memcpy(buf+198, tmp, 2);
 219
 220         sprintf(tmp, "%02x", pkt->nxthdr);
 221         memcpy(buf+258, tmp, 2);
 222
 223         sprintf(tmp, "%02x", pkt->hoplmt);
 224         memcpy(buf+318, tmp, 2);
 225
 226         int size = payload(pkt);
 227         for(int i = 0; i < 8; i++) {
 228                 if (16*i > size) break;
 229                 pp_hexdump(pkt->data + (16*i), buf + 420 + (i*70), size - 16*i);
 230                 pp_hexdump(pkt->data + (16*i) + 8, buf + 445 + (i*70), size - (16*i + 8));
 231         }
 232
 233         printf(buf);
 234         free(buf);
 235 }