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
|
#include "proto.h"
#include "util.h"
#include <stdlib.h>
enum {
Version = 0,
HdrLen = 0,
PktLen = 2,
Id = 4,
TTL = 8,
Proto = 9,
Checksum = 10,
SrcIP = 12,
DstIP = 16,
};
void ipv4_parse(const uint8_t *buf, size_t len, struct ethernet ether) {
uint8_t version, headerlen;
uint16_t packetlen, id;
version = buf[Version] >> 4;
if (version != 4) return;
headerlen = (buf[HdrLen] & 0xf) * 4;
packetlen = nget16(buf + PktLen);
if (packetlen < headerlen) return;
id = nget16(buf + Id);
// TODO checksum
// TODO fragmentation
struct ipv4 ip = (struct ipv4){
.e = ether,
.src = nget32(buf + SrcIP),
.dst = nget32(buf + DstIP),
.proto = buf[Proto],
.header = buf,
.hlen = headerlen,
};
switch (ip.proto) {
case 0x01:
icmp_parse(buf + headerlen, packetlen - headerlen, ip);
break;
case 0x11:
udp_parse(buf + headerlen, packetlen - headerlen, ip);
break;
}
}
uint8_t *ipv4_start(size_t len, struct ipv4 ip) {
ip.e.type = ET_IPv4;
if (!ip.src) ip.src = state.ip;
if (!ip.e.dst && ip.dst == 0xFFFFFFFF)
ip.e.dst = &MAC_BROADCAST;
size_t hdrlen = 20;
uint8_t *pkt = ether_start(len + hdrlen, ip.e);
pkt[Version] = 0x40;
pkt[HdrLen] |= hdrlen / 4;
nput16(pkt + PktLen, len + hdrlen);
pkt[TTL] = 0xFF;
pkt[Proto] = ip.proto;
nput32(pkt + SrcIP, ip.src);
nput32(pkt + DstIP, ip.dst);
nput16(pkt + Checksum, ip_checksum(pkt, hdrlen));
return pkt + hdrlen;
}
void ipv4_finish(uint8_t *pkt) {
ether_finish(pkt - 20);
}
|
