simple-socket-server/udp_header.h at master · slayerserg/simple-socket-server · 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
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
#pragma once

static size_t kUpdHeaderLength = 8U;

typedef struct {
  uint16_t source_port;   /**< UDP source port */
  uint16_t destination_port;   /**< UDP destination port */
  uint16_t packet_length;   /**< UDP packet length (data + header) */
  uint16_t checksum;   /**< UDP checksum */
} UDPHeader;

void UDPHeaderSetSourcePort(UDPHeader *const header,
                            const uint16_t source_port) {
  header->source_port = source_port;
}

uint16_t UDPHeaderGetSourcePort(const UDPHeader *const header) {
  return header->source_port;
}

void UDPHeaderSetDestinationPort(UDPHeader *const header,
                                 const uint16_t destination_port) {
  header->destination_port = destination_port;
}

uint16_t UDPHeaderGetDestinationPort(const UDPHeader *const header) {
  return header->destination_port;
}

void UDPHeaderSetPacketLength(UDPHeader *const header,
                              const uint16_t packet_length) {
  header->packet_length = packet_length;
}

uint16_t UDPHeaderGetPacketLength(const UDPHeader *const header) {
  return header->packet_length;
}

void UDPHeaderSetChecksum(UDPHeader *const header,
                          const uint16_t checksum) {
  header->checksum = checksum;
}

uint16_t UDPHeaderGetChecksum(const UDPHeader *const header) {
  return header->checksum;
}

void UDPHeaderGenerate(const UDPHeader *header,
                       char *message) {
  *( (uint16_t *)message ) = htons(UDPHeaderGetSourcePort(header) );
  message += 2;
  *( (uint16_t *)message ) = htons(UDPHeaderGetDestinationPort(header) );
  message += 2;
  *( (uint16_t *)message ) = htons(UDPHeaderGetPacketLength(header) );
  message += 2;
  *( (uint16_t *)message ) = htons(UDPHeaderGetChecksum(header) );
  message += 2;
}

uint16_t UDPHeaderCalculateChecksum(const void *udp_packet,
                                    const size_t udp_packet_length,
                                    const in_addr_t source_addr,
                                    const in_addr_t destination_addr) {
  const uint16_t *udp_packet_words = (uint16_t *)udp_packet;
  uint32_t checksum = 0;
  size_t length = udp_packet_length;

  // Process UDP packet
  while(length > 1) {
    checksum += *udp_packet_words++;
    if(checksum & 0x8000000) {
      checksum = (checksum & 0xFFFF) + (checksum >> 16);
    }
    length -= 2;
  }

  if(0 != length % 2) {
    // Add padding if packet length is odd
    checksum += *( (uint8_t *)udp_packet_words );
  }

  //Process IP pseudo header
  uint16_t *source_addr_as_words = (uint16_t *)&source_addr;
  checksum += *source_addr_as_words + *(source_addr_as_words + 1);

  uint16_t *destination_addr_as_words = (uint16_t *)&destination_addr;
  checksum += *destination_addr_as_words + *(destination_addr_as_words + 1);

  checksum += htons(IPPROTO_UDP);
  checksum += htons(udp_packet_length);

  //Add the carries
  while(0 != checksum >> 16) {
    checksum = (checksum & 0xFFFF) + (checksum >> 16);
  }

  // Return one's complement
  return (uint16_t)(~checksum);
}


    // UDPHeader header = {
    // .source_port = 2222,
    // .destination_port = ntohs(addr.sin_port),
    // .packet_length = kUpdHeaderLength + SEND_DATA_SIZE,
    // .checksum = 0
    // };

    // char complete_message[512];
    // memcpy(complete_message + kUpdHeaderLength, send_data, SEND_DATA_SIZE);

    // UDPHeaderGenerate(&header, (char *)complete_message);
    // UDPHeaderSetChecksum(&header,
    //                     htons(UDPHeaderCalculateChecksum(complete_message, kUpdHeaderLength + SEND_DATA_SIZE,
    //                                                      inet_addr("192.168.93.1"), addr.sin_addr.s_addr)));
    // UDPHeaderGenerate(&header, (char *)complete_message);

    // printf("\n%lld ", get_time_ms());
    // printf("Send heartbeat to client: %d\n", send_data[20]);
    // print_buff((unsigned char*)complete_message, SEND_DATA_SIZE + kUpdHeaderLength);
    // sendto(udp_send_sock, (char*)complete_message, SEND_DATA_SIZE + kUpdHeaderLength, 0, (struct sockaddr *)&addr, sizeof(addr));