rc2014-inet/icmp.c at main · blaknite/rc2014-inet · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "slip.h"
#include "ip.h"
#include "icmp.h"

static void (*rx_cb)(struct ip_hdr *iph, struct icmp_hdr *icmph) = NULL;

void icmp_debug(struct ip_hdr *iph) {
  struct icmp_hdr *icmph = (struct icmp_hdr *)ip_data(iph);

  printf(" type=%u code=%u", icmph->type, icmph->code);

  switch (icmph->type) {
    case ICMP_ECHO_REQUEST:
    case ICMP_ECHO_REPLY:
      printf(" id=%u seq=%u", ntohs(icmph->id), ntohs(icmph->seq));
      break;
  }
}

void icmp_rx(struct ip_hdr *iph) {
  struct icmp_hdr *icmph = (struct icmp_hdr *)ip_data(iph);
  uint16_t icmp_len = ip_data_len(iph);
  uint16_t csum = checksum((uint16_t *)icmph, icmp_len, 0);

  if (csum != 0 && csum != 0xFFFF) {
    return;
  }

  switch (icmph->type) {
    case ICMP_ECHO_REQUEST:
      icmp_tx_reply(iph);
      break;

    case ICMP_ECHO_REPLY:
      if (rx_cb) {
        rx_cb(iph, icmph);
      }
      break;

    case ICMP_DST_UNREACHABLE:
      break;
  }
}

void icmp_tx_reply(struct ip_hdr *rx_iph) {
  struct icmp_hdr *rx_icmph = (struct icmp_hdr *)ip_data(rx_iph);

  struct ip_hdr *tx_iph = ip_hdr_init();
  struct icmp_hdr *tx_icmph = (struct icmp_hdr *)ip_data(tx_iph);

  uint16_t rx_data_len = ip_data_len(rx_iph);
  uint16_t icmp_dlen;

  if (rx_data_len < 8) {
    return;
  }

  icmp_dlen = rx_data_len - 8;

  tx_iph->len = 28 + icmp_dlen;
  tx_iph->proto = ICMP;

  memcpy(tx_iph->daddr, rx_iph->saddr, 4);

  tx_icmph->type = ICMP_ECHO_REPLY;
  tx_icmph->id = rx_icmph->id;
  tx_icmph->seq = rx_icmph->seq;

  memcpy((uint8_t *)tx_icmph + 8, (uint8_t *)rx_icmph + 8, icmp_dlen);

  tx_icmph->csum = checksum((uint16_t *)tx_icmph, icmp_dlen + 8, 0);

  ip_tx(tx_iph);
}

void icmp_tx_request(uint8_t *daddr, uint16_t seq) {
  struct ip_hdr *iph = ip_hdr_init();
  struct icmp_hdr *icmph = (struct icmp_hdr *)ip_data(iph);

  uint16_t icmp_len = 0;

  iph->len = 28 + icmp_len;
  iph->proto = ICMP;

  memcpy(iph->daddr, daddr, 4);

  icmph->type = ICMP_ECHO_REQUEST;
  icmph->id = 0;
  icmph->seq = htons(seq);

  icmph->csum = checksum((uint16_t *)icmph, icmp_len + 8, 0);

  ip_tx(iph);
}

void icmp_listen(void (*callback)(struct ip_hdr *iph, struct icmp_hdr *icmph)) {
  rx_cb = callback;
}