extlibs/tinydtls/tests/pcap.c

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <getopt.h>
   4 #include <pcap/pcap.h>
   5
   6 #include "tinydtls.h"
   7 #include "debug.h"
   8 #include "dtls.h"
   9
  10 #define TRANSPORT_HEADER_SIZE (14+20+8) /* Ethernet + IP + UDP */
  11
  12 /* the pre_master_secret is generated from the PSK at startup */
  13 unsigned char pre_master_secret[60];
  14 size_t pre_master_len = 0;
  15
  16 unsigned char master_secret[DTLS_MASTER_SECRET_LENGTH];
  17 size_t master_secret_len = 0;
  18
  19 dtls_security_parameters_t security_params[2];
  20 int config = 0;
  21 unsigned int epoch[2] = { 0, 0 };
  22
  23 #if DTLS_VERSION == 0xfeff
  24 dtls_hash_t hs_hash[2];
  25 #elif DTLS_VERSION == 0xfefd
  26 dtls_hash_t hs_hash[1];
  27 #endif
  28
  29 static inline void
  30 update_hash(uint8 *record, size_t rlength,
  31             uint8 *data, size_t data_length) {
  32   int i;
  33
  34   if (!hs_hash[0])
  35     return;
  36
  37   for (i = 0; i < sizeof(hs_hash) / sizeof(dtls_hash_t *); ++i) {
  38     dtls_hash_update(hs_hash[i], data, data_length);
  39   }
  40 }
  41
  42 static inline void
  43 finalize_hash(uint8 *buf) {
  44 #if DTLS_VERSION == 0xfeff
  45   unsigned char statebuf[sizeof(md5_state_t) + sizeof(SHA_CTX)];
  46 #elif DTLS_VERSION == 0xfefd
  47   unsigned char statebuf[sizeof(dtls_sha256_ctx)];
  48 #endif
  49
  50   if (!hs_hash[0])
  51     return;
  52
  53   /* temporarily store hash status for roll-back after finalize */
  54 #if DTLS_VERSION == 0xfeff
  55   memcpy(statebuf, hs_hash[0], sizeof(md5_state_t));
  56   memcpy(statebuf + sizeof(md5_state_t),
  57          hs_hash[1],
  58          sizeof(SHA_CTX));
  59 #elif DTLS_VERSION == 0xfefd
  60   memcpy(statebuf, hs_hash[0], sizeof(statebuf));
  61 #endif
  62
  63   dtls_hash_finalize(buf, hs_hash[0]);
  64 #if DTLS_VERSION == 0xfeff
  65   dtls_hash_finalize(buf + 16, hs_hash[1]);
  66 #endif
  67
  68   /* restore hash status */
  69 #if DTLS_VERSION == 0xfeff
  70   memcpy(hs_hash[0], statebuf, sizeof(md5_state_t));
  71   memcpy(hs_hash[1],
  72          statebuf + sizeof(md5_state_t),
  73          sizeof(SHA_CTX));
  74 #elif DTLS_VERSION == 0xfefd
  75   memcpy(hs_hash[0], statebuf, sizeof(statebuf));
  76 #endif
  77 }
  78
  79 static inline void
  80 clear_hash() {
  81   int i;
  82
  83   for (i = 0; i < sizeof(hs_hash) / sizeof(dtls_hash_t *); ++i)
  84     free(hs_hash[i]);
  85   memset(hs_hash, 0, sizeof(hs_hash));
  86 }
  87
  88 #undef CURRENT_CONFIG
  89 #undef OTHER_CONFIG
  90 #undef SWITCH_CONFIG
  91 #define CURRENT_CONFIG (&security_params[config])
  92 #define OTHER_CONFIG   (&security_params[!(config & 0x01)])
  93 #define SWITCH_CONFIG  (config = !(config & 0x01))
  94
  95 int
  96 pcap_verify(dtls_security_parameters_t *sec,
  97             int is_client,
  98             const unsigned char *record, size_t record_length,
  99             const unsigned char *cleartext, size_t cleartext_length) {
 100
 101   unsigned char mac[DTLS_HMAC_MAX];
 102   dtls_hmac_context_t hmac_ctx;
 103   int ok;
 104
 105   if (cleartext_length < dtls_kb_digest_size(sec))
 106     return 0;
 107
 108   dtls_hmac_init(&hmac_ctx,
 109                  is_client
 110                  ? dtls_kb_client_mac_secret(sec)
 111                  : dtls_kb_server_mac_secret(sec),
 112                  dtls_kb_mac_secret_size(sec));
 113
 114   cleartext_length -= dtls_kb_digest_size(sec);
 115
 116   /* calculate MAC even if padding is wrong */
 117   dtls_mac(&hmac_ctx,
 118            record,              /* the pre-filled record header */
 119            cleartext, cleartext_length,
 120            mac);
 121
 122   ok = memcmp(mac, cleartext + cleartext_length,
 123               dtls_kb_digest_size(sec)) == 0;
 124 #ifndef NDEBUG
 125   printf("MAC (%s): ", ok ? "valid" : "invalid");
 126   dump(mac, dtls_kb_digest_size(sec));
 127   printf("\n");
 128 #endif
 129   return ok;
 130 }
 131
 132 int
 133 decrypt_verify(int is_client, const uint8 *packet, size_t length,
 134                uint8 **cleartext, size_t *clen) {
 135   int res, ok = 0;
 136   dtls_cipher_context_t *cipher;
 137
 138   static unsigned char buf[1000];
 139
 140   switch (CURRENT_CONFIG->cipher) {
 141   case AES128:                  /* TLS_PSK_WITH_AES128_CBC_SHA */
 142     *cleartext = buf;
 143     *clen = length - sizeof(dtls_record_header_t);
 144
 145     if (is_client)
 146       cipher = CURRENT_CONFIG->read_cipher;
 147     else
 148       cipher = CURRENT_CONFIG->write_cipher;
 149
 150     res = dtls_decrypt(cipher,
 151                        (uint8 *)packet + sizeof(dtls_record_header_t), *clen,
 152                        buf, NULL, 0);
 153
 154     if (res < 0) {
 155       warn("decryption failed!\n");
 156     } else {
 157       ok = pcap_verify(CURRENT_CONFIG, is_client, (uint8 *)packet, length,
 158                        *cleartext, res);
 159
 160       if (ok)
 161         *clen = res - dtls_kb_digest_size(CURRENT_CONFIG);
 162     }
 163     break;
 164   default:                      /* no cipher suite selected */
 165     *cleartext = (uint8 *)packet + sizeof(dtls_record_header_t);
 166     *clen = length - sizeof(dtls_record_header_t);
 167
 168     ok = 1;
 169   }
 170
 171   if (ok)
 172     printf("verify OK\n");
 173   else
 174     printf("verification failed!\n");
 175   return ok;
 176 }
 177
 178 #define SKIP_ETH_HEADER(M,L)                    \
 179   if ((L) < 14)                                 \
 180     return;                                     \
 181   else {                                        \
 182     (M) += 14;                                  \
 183     (L) -= 14;                                  \
 184   }
 185
 186 #define SKIP_IP_HEADER(M,L)                             \
 187   if (((M)[0] & 0xF0) == 0x40) {        /* IPv4 */      \
 188     (M) += (M[0] & 0x0F) * 4;                           \
 189     (L) -= (M[0] & 0x0F) * 4;                           \
 190   } else                                                \
 191     if (((M)[0] & 0xF0) == 0x60) { /* IPv6 */           \
 192       (M) += 40;                                        \
 193       (L) -= 40;                                        \
 194     }
 195
 196 #define SKIP_UDP_HEADER(M,L) {                  \
 197     (M) += 8;                                   \
 198     (L) -= 8;                                   \
 199   }
 200
 201 void
 202 handle_packet(const u_char *packet, int length) {
 203   static int n = 0;
 204   static unsigned char initial_hello[] = {
 205     0x16, 0xfe, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
 206   };
 207   uint8 *data;
 208   size_t data_length, rlen;
 209   int i, res;
 210 #if DTLS_VERSION == 0xfeff
 211 #ifndef SHA1_DIGEST_LENGTH
 212 #define SHA1_DIGEST_LENGTH 20
 213 #endif
 214   uint8 hash_buf[16 + SHA1_DIGEST_LENGTH];
 215 #elif DTLS_VERSION == 0xfefd
 216   uint8 hash_buf[DTLS_SHA256_DIGEST_LENGTH];
 217 #endif
 218 #define verify_data_length 12
 219   int is_client;
 220   n++;
 221
 222   SKIP_ETH_HEADER(packet, length);
 223   SKIP_IP_HEADER(packet, length);
 224
 225   /* determine from port if this is a client */
 226   is_client = dtls_uint16_to_int(packet) != 20220;
 227
 228   SKIP_UDP_HEADER(packet, length);
 229
 230   while (length) {
 231     rlen = dtls_uint16_to_int(packet + 11) + sizeof(dtls_record_header_t);
 232
 233     if (!rlen) {
 234       fprintf(stderr, "invalid length!\n");
 235       return;
 236     }
 237
 238     /* skip packet if it is from a different epoch */
 239     if (dtls_uint16_to_int(packet + 3) != epoch[is_client])
 240       goto next;
 241
 242     res = decrypt_verify(is_client, packet, rlen,
 243                          &data, &data_length);
 244
 245     if (res <= 0)
 246       goto next;
 247
 248     printf("packet %d (from %s):\n", n, is_client ? "client" : "server");
 249     hexdump(packet, sizeof(dtls_record_header_t));
 250     printf("\n");
 251     hexdump(data, data_length);
 252     printf("\n");
 253
 254     if (packet[0] == 22 && data[0] == 1) { /* ClientHello */
 255       if (memcmp(packet, initial_hello, sizeof(initial_hello)) == 0)
 256         goto next;
 257
 258       memcpy(dtls_kb_client_iv(OTHER_CONFIG), data + 14, 32);
 259
 260         clear_hash();
 261 #if DTLS_VERSION == 0xfeff
 262       hs_hash[0] = dtls_new_hash(HASH_MD5);
 263       hs_hash[1] = dtls_new_hash(HASH_SHA1);
 264
 265       hs_hash[0]->init(hs_hash[0]->data);
 266       hs_hash[1]->init(hs_hash[1]->data);
 267 #elif DTLS_VERSION == 0xfefd
 268       dtls_hash_init(hs_hash[0]);
 269 #endif
 270     }
 271
 272     if (packet[0] == 22 && data[0] == 2) { /* ServerHello */
 273       memcpy(dtls_kb_server_iv(OTHER_CONFIG), data + 14, 32);
 274       /* FIXME: search in ciphers */
 275       OTHER_CONFIG->cipher = TLS_PSK_WITH_AES_128_CCM_8;
 276     }
 277
 278     if (packet[0] == 20 && data[0] == 1) { /* ChangeCipherSpec */
 279       printf("client random: ");
 280       dump(dtls_kb_client_iv(OTHER_CONFIG), 32);
 281       printf("\nserver random: ");
 282       dump(dtls_kb_server_iv(OTHER_CONFIG), 32);
 283       printf("\n");
 284       master_secret_len =
 285         dtls_prf(pre_master_secret, pre_master_len,
 286                  (unsigned char *)"master secret", 13,
 287                  dtls_kb_client_iv(OTHER_CONFIG), 32,
 288                  dtls_kb_server_iv(OTHER_CONFIG), 32,
 289                  master_secret, DTLS_MASTER_SECRET_LENGTH);
 290
 291       printf("master_secret:\n  ");
 292       for(i = 0; i < master_secret_len; i++)
 293         printf("%02x", master_secret[i]);
 294       printf("\n");
 295
 296       /* create key_block from master_secret
 297        * key_block = PRF(master_secret,
 298                      "key expansion" + server_random + client_random) */
 299       dtls_prf(master_secret, master_secret_len,
 300                (unsigned char *)"key expansion", 13,
 301                dtls_kb_server_iv(OTHER_CONFIG), 32,
 302                dtls_kb_client_iv(OTHER_CONFIG), 32,
 303                OTHER_CONFIG->key_block,
 304                dtls_kb_size(OTHER_CONFIG));
 305
 306       OTHER_CONFIG->read_cipher =
 307         dtls_cipher_new(OTHER_CONFIG->cipher,
 308                         dtls_kb_client_write_key(OTHER_CONFIG),
 309                         dtls_kb_key_size(OTHER_CONFIG));
 310
 311       if (!OTHER_CONFIG->read_cipher) {
 312         warn("cannot create read cipher\n");
 313       } else {
 314         dtls_cipher_set_iv(OTHER_CONFIG->read_cipher,
 315                            dtls_kb_client_iv(OTHER_CONFIG),
 316                            dtls_kb_iv_size(OTHER_CONFIG));
 317       }
 318
 319       OTHER_CONFIG->write_cipher =
 320         dtls_cipher_new(OTHER_CONFIG->cipher,
 321                         dtls_kb_server_write_key(OTHER_CONFIG),
 322                         dtls_kb_key_size(OTHER_CONFIG));
 323
 324       if (!OTHER_CONFIG->write_cipher) {
 325         warn("cannot create write cipher\n");
 326       } else {
 327         dtls_cipher_set_iv(OTHER_CONFIG->write_cipher,
 328                            dtls_kb_server_iv(OTHER_CONFIG),
 329                            dtls_kb_iv_size(OTHER_CONFIG));
 330       }
 331
 332       /* if (is_client) */
 333         SWITCH_CONFIG;
 334       epoch[is_client]++;
 335
 336       printf("key_block:\n");
 337       printf("  client_MAC_secret:\t");
 338       dump(dtls_kb_client_mac_secret(CURRENT_CONFIG),
 339            dtls_kb_mac_secret_size(CURRENT_CONFIG));
 340       printf("\n");
 341
 342       printf("  server_MAC_secret:\t");
 343       dump(dtls_kb_server_mac_secret(CURRENT_CONFIG),
 344            dtls_kb_mac_secret_size(CURRENT_CONFIG));
 345       printf("\n");
 346
 347       printf("  client_write_key:\t");
 348       dump(dtls_kb_client_write_key(CURRENT_CONFIG),
 349            dtls_kb_key_size(CURRENT_CONFIG));
 350       printf("\n");
 351
 352       printf("  server_write_key:\t");
 353       dump(dtls_kb_server_write_key(CURRENT_CONFIG),
 354            dtls_kb_key_size(CURRENT_CONFIG));
 355       printf("\n");
 356
 357       printf("  client_IV:\t\t");
 358       dump(dtls_kb_client_iv(CURRENT_CONFIG),
 359            dtls_kb_iv_size(CURRENT_CONFIG));
 360       printf("\n");
 361
 362       printf("  server_IV:\t\t");
 363       dump(dtls_kb_server_iv(CURRENT_CONFIG),
 364            dtls_kb_iv_size(CURRENT_CONFIG));
 365       printf("\n");
 366
 367     }
 368
 369     if (packet[0] == 22) {
 370       if (data[0] == 20) { /* Finished */
 371         finalize_hash(hash_buf);
 372         /* clear_hash(); */
 373
 374         update_hash((unsigned char *)packet, sizeof(dtls_record_header_t),
 375                     data, data_length);
 376
 377         dtls_prf(master_secret, master_secret_len,
 378                  is_client
 379                  ? (unsigned char *)"client finished"
 380                  : (unsigned char *)"server finished"
 381                  , 15,
 382                  hash_buf, sizeof(hash_buf),
 383                  NULL, 0,
 384                  data + sizeof(dtls_handshake_header_t),
 385                  verify_data_length);
 386         printf("verify_data:\n");
 387         dump(data, data_length);
 388         printf("\n");
 389       } else {
 390         update_hash((unsigned char *)packet, sizeof(dtls_record_header_t),
 391                     data, data_length);
 392       }
 393     }
 394
 395     if (packet[0] == 23) {      /* Application Data */
 396       printf("Application Data:\n");
 397       dump(data, data_length);
 398       printf("\n");
 399     }
 400
 401   next:
 402     length -= rlen;
 403     packet += rlen;
 404   }
 405 }
 406
 407 void init() {
 408   memset(security_params, 0, sizeof(security_params));
 409   CURRENT_CONFIG->cipher = -1;
 410
 411   memset(hs_hash, 0, sizeof(hs_hash));
 412
 413   /* set pre_master_secret to default if no PSK was given */
 414   if (!pre_master_len) {
 415     /* unsigned char psk[] = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06 }; */
 416     pre_master_len =
 417       dtls_pre_master_secret((unsigned char *)"secretPSK", 9,
 418                              pre_master_secret);
 419   }
 420 }
 421
 422 int main(int argc, char **argv) {
 423   pcap_t *pcap;
 424   char errbuf[PCAP_ERRBUF_SIZE];
 425   struct pcap_pkthdr *pkthdr;
 426   const u_char *packet;
 427   int res = 0;
 428   int c, option_index = 0;
 429
 430   static struct option opts[] = {
 431     { "psk",  1, 0, 'p' },
 432     { 0, 0, 0, 0 }
 433   };
 434
 435   /* handle command line options */
 436   while (1) {
 437     c = getopt_long(argc, argv, "p:", opts, &option_index);
 438     if (c == -1)
 439       break;
 440
 441     switch (c) {
 442     case 'p':
 443       pre_master_len = dtls_pre_master_secret((unsigned char *)optarg,
 444                                       strlen(optarg), pre_master_secret);
 445       break;
 446     }
 447   }
 448
 449   if (argc <= optind) {
 450     fprintf(stderr, "usage: %s [-p|--psk PSK] pcapfile\n", argv[0]);
 451     return -1;
 452   }
 453
 454   init();
 455
 456   pcap = pcap_open_offline(argv[optind], errbuf);
 457   if (!pcap) {
 458     fprintf(stderr, "pcap_open_offline: %s\n", errbuf);
 459     return -2;
 460   }
 461
 462   for (;;) {
 463     res = pcap_next_ex(pcap, &pkthdr, &packet);
 464
 465     switch(res) {
 466     case -2: goto done;
 467     case -1: pcap_perror(pcap, "read packet"); break;
 468     case  1: handle_packet(packet, pkthdr->caplen); break;
 469     default:
 470       ;
 471     }
 472   }
 473  done:
 474
 475   pcap_close(pcap);
 476
 477   return 0;
 478 }