looper/wlanDHCP.cpp at master · AnEntrypoint/looper · 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
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
#include "wlanDHCP.h"
#include <circle/timer.h>
#include <circle/logger.h>
#include <circle/util.h>

#define ETH_HDR  14
#define IP_HDR   20
#define UDP_HDR  8
#define DHCP_HDR 236
#define OPT_OFF  (ETH_HDR + IP_HDR + UDP_HDR + DHCP_HDR)
#define FRAME_SZ 600

static u8 s_ip[4];
static bool s_got;
static u32 s_xid;
static u8 s_mac[6];
static CBcm4343Device *s_pWLAN;
static unsigned s_deadline;

static inline u16 sw16(u16 v) { return __builtin_bswap16(v); }

static u16 ipCsum(const u8 *h, int n) {
	u32 s = 0;
	for (int i = 0; i < n; i += 2) s += ((u16)h[i] << 8) | h[i+1];
	while (s >> 16) s = (s & 0xffff) + (s >> 16);
	return (u16)~s;
}

static void buildDiscover(u8 *f, int *outLen) {
	memset(f, 0, FRAME_SZ);
	const u8 bcast[6] = {0xff,0xff,0xff,0xff,0xff,0xff};
	const u8 bcastIP[4] = {255,255,255,255};
	const u8 zeroIP[4] = {0,0,0,0};
	memcpy(f, bcast, 6); memcpy(f+6, s_mac, 6);
	f[12]=0x08; f[13]=0x00;
	u8 *ip = f+ETH_HDR;
	ip[0]=0x45; ip[8]=64; ip[9]=17;
	memcpy(ip+12, zeroIP, 4); memcpy(ip+16, bcastIP, 4);
	u8 *udp = ip+IP_HDR;
	u16 sp=sw16(68), dp=sw16(67);
	memcpy(udp, &sp, 2); memcpy(udp+2, &dp, 2);
	u8 *d = udp+UDP_HDR;
	d[0]=1; d[1]=1; d[2]=6;
	memcpy(d+4, &s_xid, 4);
	memcpy(d+28, s_mac, 6);
	const u8 magic[4]={99,130,83,99};
	memcpy(d+DHCP_HDR, magic, 4);
	u8 *o = d+DHCP_HDR+4;
	*o++=53; *o++=1; *o++=1; *o++=255;
	int plen = (int)(o - d);
	int udpLen = UDP_HDR + plen, ipLen = IP_HDR + udpLen;
	u16 uL=sw16(udpLen), iL=sw16(ipLen);
	memcpy(udp+4, &uL, 2); memcpy(ip+2, &iL, 2);
	u16 cs = sw16(ipCsum(ip, IP_HDR));
	memcpy(ip+10, &cs, 2);
	*outLen = ETH_HDR + ipLen;
}

static void buildRequest(u8 *f, const u8 *offer, int *outLen) {
	memset(f, 0, FRAME_SZ);
	const u8 bcast[6]={0xff,0xff,0xff,0xff,0xff,0xff};
	const u8 bcastIP[4]={255,255,255,255};
	const u8 zeroIP[4]={0,0,0,0};
	memcpy(f, bcast, 6); memcpy(f+6, s_mac, 6);
	f[12]=0x08; f[13]=0x00;
	u8 *ip=f+ETH_HDR;
	ip[0]=0x45; ip[8]=64; ip[9]=17;
	memcpy(ip+12, zeroIP, 4); memcpy(ip+16, bcastIP, 4);
	u8 *udp=ip+IP_HDR;
	u16 sp=sw16(68), dp=sw16(67);
	memcpy(udp, &sp, 2); memcpy(udp+2, &dp, 2);
	u8 *d=udp+UDP_HDR;
	d[0]=1; d[1]=1; d[2]=6;
	memcpy(d+4, &s_xid, 4);
	memcpy(d+28, s_mac, 6);
	const u8 magic[4]={99,130,83,99};
	memcpy(d+DHCP_HDR, magic, 4);
	u8 *o=d+DHCP_HDR+4;
	*o++=53; *o++=1; *o++=3;
	*o++=50; *o++=4; memcpy(o, offer, 4); o+=4;
	*o++=255;
	int plen=(int)(o-d);
	int udpLen=UDP_HDR+plen, ipLen=IP_HDR+udpLen;
	u16 uL=sw16(udpLen), iL=sw16(ipLen);
	memcpy(udp+4, &uL, 2); memcpy(ip+2, &iL, 2);
	u16 cs=sw16(ipCsum(ip, IP_HDR));
	memcpy(ip+10, &cs, 2);
	*outLen=ETH_HDR+ipLen;
}

static bool parseOffer(const u8 *f, int len, u8 *outIP) {
	if (len < OPT_OFF+4) return false;
	if (f[12]!=0x08 || f[13]!=0x00) return false;
	const u8 *ip=f+ETH_HDR;
	if (ip[9]!=17) return false;
	const u8 *udp=ip+IP_HDR;
	u16 dp; memcpy(&dp, udp+2, 2);
	if (sw16(dp)!=68) return false;
	const u8 *d=udp+UDP_HDR;
	if (d[0]!=2) return false;
	u32 rxid; memcpy(&rxid, d+4, 4);
	if (rxid!=s_xid) return false;
	memcpy(outIP, d+16, 4);
	const u8 *o=d+DHCP_HDR+4;
	const u8 *end=f+len;
	while (o+2<=end && *o!=255) {
		if (*o==53 && o[1]>=1 && (o[2]==2||o[2]==5)) return true;
		o+=2+o[1];
	}
	return false;
}

void wlanDhcpSendDiscover(CBcm4343Device *pWLAN, const u8 *mac) {
	s_pWLAN = pWLAN;
	s_got = false;
	memset(s_ip, 0, 4);
	memcpy(s_mac, mac, 6);
	s_xid = (u32)(CTimer::GetClockTicks() & 0xFFFFFFFF);
	s_deadline = CTimer::GetClockTicks() + 10 * CLOCKHZ;
	u8 frame[FRAME_SZ]; int flen;
	buildDiscover(frame, &flen);
	pWLAN->SendFrame(frame, flen);
	CLogger::Get()->Write("wdhcp", LogNotice, "DISCOVER sent");
}

bool wlanDhcpPoll(CBcm4343Device *pWLAN) {
	if (s_got) return true;
	if (CTimer::GetClockTicks() > s_deadline) {
		CLogger::Get()->Write("wdhcp", LogWarning, "DHCP timeout");
		return true;
	}
	u8 buf[FRAME_SZ]; unsigned rlen;
	u8 offer[4]={0};
	while (pWLAN->ReceiveFrame(buf, &rlen)) {
		if (parseOffer(buf, rlen, offer)) {
			CLogger::Get()->Write("wdhcp", LogNotice, "OFFER %d.%d.%d.%d", offer[0],offer[1],offer[2],offer[3]);
			u8 frame[FRAME_SZ]; int flen;
			buildRequest(frame, offer, &flen);
			pWLAN->SendFrame(frame, flen);
			memcpy(s_ip, offer, 4);
			s_got = true;
			return true;
		}
	}
	return false;
}

bool wlanDhcpOK(void) { return s_got; }
const u8 *wlanDhcpIP(void) { return s_ip; }
void wlanApSetIP(const u8 *ip) { memcpy(s_ip, ip, 4); s_got = true; }