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
|
#include <camellia/syscalls.h>
#include <stdlib.h>
#include <string.h>
#include "proto.h"
#include "util.h"
enum {
DstMAC = 0,
SrcMAC = 6,
EtherType = 12,
Payload = 14,
};
struct queue_entry *ether_queue;
void ether_parse(const uint8_t *buf, size_t len) {
struct ethernet ether = (struct ethernet){
.src = buf + SrcMAC,
.dst = buf + DstMAC,
.type = nget16(buf + EtherType),
};
struct queue_entry **iter = ðer_queue;
while (iter && *iter) {
struct queue_entry *qe = *iter;
_syscall_fs_respond(qe->h, buf, len, 0);
/* remove entry */
/* yes, doing it this way here doesn't make sense. i'm preparing for filtering */
*iter = qe->next;
free(qe);
}
switch (ether.type) {
case ET_IPv4:
ipv4_parse(buf + Payload, len - Payload, ether);
break;
case ET_ARP:
arp_parse(buf + Payload, len - Payload);
break;
}
}
static const size_t fhoff = sizeof(size_t);
uint8_t *ether_start(size_t len, struct ethernet ether) {
if (len < 60 - Payload) len = 60 - Payload;
if (!ether.dst) eprintf("NULL ether.dst!"); // TODO arp? i guess?
if (!ether.src) ether.src = &state.mac;
uint8_t *buf = malloc(fhoff + Payload + len);
memset(buf, 0, fhoff + Payload + len);
*(size_t*)buf = len + Payload;
memcpy(buf + fhoff + DstMAC, ether.dst, 6);
memcpy(buf + fhoff + SrcMAC, ether.src, 6);
nput16(buf + fhoff + EtherType, ether.type);
return buf + fhoff + Payload;
}
void ether_finish(uint8_t *pkt) {
uint8_t *buf = pkt - Payload - fhoff;
size_t len = *(size_t*)buf;
_syscall_write(state.raw_h, buf + fhoff, len, 0, 0);
free(buf);
}
|
