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
|
#include "netlib.h"
/* netd - a minimal IP stack over SLIP. Our address is 10.0.0.2, peer 10.0.0.1.
Milestones: A) ICMP echo, B) UDP echo. Raw byte arrays, network byte order
(big-endian) handled explicitly. */
static unsigned char pkt[256];
/* one's-complement sum (RFC 1071) with a running seed, carry-folded each add */
static unsigned int sum16(unsigned char *d, unsigned int len, unsigned int sum) {
unsigned int w, i = 0;
while (i + 1 < len) {
w = ((unsigned int)d[i] << 8) | d[i + 1];
sum += w; if (sum < w) sum++;
i += 2;
}
if (len & 1) { w = (unsigned int)d[len - 1] << 8; sum += w; if (sum < w) sum++; }
return sum;
}
/* pseudo-header sum for UDP/TCP: src+dst IP, protocol, segment length */
static unsigned int pseudo(unsigned char proto, unsigned int seglen) {
unsigned int sum = sum16(pkt + 12, 8, 0), w;
w = proto; sum += w; if (sum < w) sum++;
w = seglen; sum += w; if (sum < w) sum++;
return sum;
}
static void swap4(unsigned char a, unsigned char b) {
unsigned char i, t;
for (i = 0; i < 4; i++) { t = pkt[a + i]; pkt[a + i] = pkt[b + i]; pkt[b + i] = t; }
}
static void swap2(unsigned char a, unsigned char b) {
unsigned char t;
t = pkt[a]; pkt[a] = pkt[b]; pkt[b] = t;
t = pkt[a + 1]; pkt[a + 1] = pkt[b + 1]; pkt[b + 1] = t;
}
/* recompute the IP header checksum over ihl bytes, then send 'len' total */
static void ip_send(unsigned char ihl, unsigned int len) {
unsigned int c;
pkt[10] = 0; pkt[11] = 0;
c = ~sum16(pkt, ihl, 0);
pkt[10] = (unsigned char)(c >> 8); pkt[11] = (unsigned char)c;
slip_send((char *)pkt, (unsigned char)len);
}
void main(void) {
unsigned char len, ihl;
unsigned int c, seglen;
puts("netd: IP up (10.0.0.2)"); nl();
for (;;) {
len = slip_recv((char *)pkt, 255);
if (len < 20 || (pkt[0] >> 4) != 4) continue;
ihl = (pkt[0] & 0x0f) << 2;
pkt[8] = 64; /* TTL for any reply */
if (pkt[9] == 1) { /* ICMP */
if (len < (unsigned char)(ihl + 8) || pkt[ihl] != 8) continue; /* echo request */
swap4(12, 16);
pkt[ihl] = 0; /* -> echo reply */
pkt[ihl + 2] = 0; pkt[ihl + 3] = 0;
c = ~sum16(pkt + ihl, (unsigned int)(len - ihl), 0);
pkt[ihl + 2] = (unsigned char)(c >> 8); pkt[ihl + 3] = (unsigned char)c;
ip_send(ihl, len);
} else if (pkt[9] == 17) { /* UDP echo */
if (len < (unsigned char)(ihl + 8)) continue;
seglen = (unsigned int)(len - ihl);
swap4(12, 16); /* swap IP addresses */
swap2(ihl, ihl + 2); /* swap UDP ports */
pkt[ihl + 6] = 0; pkt[ihl + 7] = 0;
c = ~sum16(pkt + ihl, seglen, pseudo(17, seglen));
if (c == 0) c = 0xffff; /* UDP: 0 means "none" */
pkt[ihl + 6] = (unsigned char)(c >> 8); pkt[ihl + 7] = (unsigned char)c;
ip_send(ihl, len);
}
}
}
|
