-
Notifications
You must be signed in to change notification settings - Fork 1
/
udp.c
119 lines (96 loc) · 3.01 KB
/
udp.c
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
/* file: udp.c
* description: UDP protocol implementation (RFC768) and basic services
* date: 12/2020
* author: Sergio Johann Filho <sergio.johann@acad.pucrs.br>
*/
#include <stdio.h>
#include <string.h>
#include "include/ustack.h"
static int32_t (*udp_callback)(uint8_t *packet);
static uint16_t udpchksum(uint8_t *packet, uint16_t len)
{
uint32_t sum = 0;
uint16_t i, val, *ptr = (uint16_t *)(packet + sizeof(struct ip_s) - 8);
sum += IP_PROTO_UDP;
sum += len;
for (i = 0; i < len + 8 - 1; i += 2) {
val = *ptr++;
sum += ntohs(val);
}
if (len & 1) {
val = *ptr;
sum += ntohs(val) & 0xff00;
}
sum = (sum >> 16) + (sum & 0xffff);
sum += (sum >> 16);
return ~sum;
}
int32_t udp_out(uint8_t dst_addr[4], uint16_t src_port, uint16_t dst_port, uint8_t *packet, uint16_t len)
{
int32_t val;
uint16_t chksum;
struct ip_udp_s *udp = (struct ip_udp_s *)packet;
udp->udp.src_port = htons(src_port);
udp->udp.dst_port = htons(dst_port);
udp->udp.len = htons(len);
udp->udp.chksum = htons(0);
memcpy(&udp->ip.src_addr, myip, 4);
memcpy(&udp->ip.dst_addr, dst_addr, 4);
chksum = udpchksum(packet, len);
udp->udp.chksum = htons(chksum);
#ifdef USTACK_DEBUG_UDP
printf("[DEBUG] UDP packet out: src %d.%d.%d.%d:%d, dst %d.%d.%d.%d:%d, size %d, sum %04x\n",
udp->ip.src_addr[0], udp->ip.src_addr[1], udp->ip.src_addr[2], udp->ip.src_addr[3], ntohs(udp->udp.src_port),
dst_addr[0], dst_addr[1], dst_addr[2], dst_addr[3], ntohs(udp->udp.dst_port),
ntohs(udp->udp.len), ntohs(udp->udp.chksum));
#endif
val = ip_out(dst_addr, IP_PROTO_UDP, packet, len + sizeof(struct ip_s));
return val;
}
int32_t udp_in(uint8_t *packet)
{
int32_t val = -1;
uint8_t dst_addr[4];
uint16_t src_port, dst_port, datalen = 0, chksum;
struct ip_udp_s *udp = (struct ip_udp_s *)packet;
chksum = ntohs(udp->udp.chksum);
datalen = ntohs(udp->udp.len);
if (chksum) {
udp->udp.chksum = htons(0);
if (chksum != udpchksum(packet, datalen)) {
#ifdef USTACK_DEBUG_ERR
printf("[ERROR] UDP checksum error\n");
#endif
return -1;
}
}
src_port = ntohs(udp->udp.src_port);
dst_port = ntohs(udp->udp.dst_port);
#ifdef USTACK_DEBUG_UDP
printf("[DEBUG] UDP packet in: src %d.%d.%d.%d:%d, dst %d.%d.%d.%d:%d, size %d, sum %04x\n",
udp->ip.src_addr[0], udp->ip.src_addr[1], udp->ip.src_addr[2], udp->ip.src_addr[3], ntohs(udp->udp.src_port),
udp->ip.dst_addr[0], udp->ip.dst_addr[1], udp->ip.dst_addr[2], udp->ip.dst_addr[3], ntohs(udp->udp.dst_port),
ntohs(udp->udp.len), chksum);
#endif
switch (dst_port) {
case PORT_ECHO: /* Echo protocol, RFC862 */
memcpy(dst_addr, &udp->ip.src_addr, 4);
udp_out(dst_addr, dst_port, src_port, packet, datalen);
break;
case PORT_DISCARD: /* Discard protocol, RFC863 */
break;
default:
if (udp_callback)
val = udp_callback(packet);
#ifdef USTACK_DEBUG_ERR
if (val < 0)
printf("[ERROR] UDP destination port error\n");
#endif
return val;
}
return datalen;
}
void udp_set_callback(int32_t (*callback)(uint8_t *packet))
{
udp_callback = callback;
}