1 /*
2 * Copyright (C) 2005-7, Hugo Santos <hugo@fivebits.net>
3 *
4 * This program is free software; you can redistribute it and/or modify it
5 * under the terms of the GNU General Public License as published by the Free
6 * Software Foundation; either version 2 of the License, or any later version.
7 *
8 * This program is distributed in the hope that it will be useful, but WITHOUT
9 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
10 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
11 * more details.
12 *
13 * $Id: protocol.cpp 406 2007-07-12 10:14:49Z faidon $
14 */
15
16 #include "dbeacon.h"
17 #include "protocol.h"
18 #include <math.h>
19 #include <netinet/in.h>
20 #include <stdio.h>
21 #include <string.h>
22 #include <ctype.h>
23
24 using namespace std;
25
26 /* Helper method to write a TLV with a string value */
write_tlv_string(uint8_t * buf,int maxlen,int & pointer,uint8_t type,const char * str)27 static inline bool write_tlv_string(uint8_t *buf, int maxlen, int &pointer,
28 uint8_t type, const char *str) {
29 int len = strlen(str);
30 if ((pointer + 2 + len) > maxlen)
31 return false;
32 buf[pointer + 0] = type;
33 buf[pointer + 1] = len;
34 memcpy(buf + pointer + 2, str, len);
35 pointer += len + 2;
36 return true;
37 }
38
39 /* Helper method to start a TLV block */
write_tlv_start(uint8_t * buff,int maxlen,int & ptr,uint8_t type,int len)40 static bool write_tlv_start(uint8_t *buff, int maxlen, int &ptr,
41 uint8_t type, int len) {
42 if ((ptr + len + 2) > maxlen)
43 return false;
44
45 buff[ptr + 0] = type;
46 buff[ptr + 1] = len;
47
48 ptr += 2;
49
50 return true;
51 }
52
53 /* Helper method to write a TLV with a 32bit unsigned integer value */
write_tlv_uint(uint8_t * buff,int maxlen,int & ptr,uint8_t type,uint32_t val)54 static bool write_tlv_uint(uint8_t *buff, int maxlen, int &ptr, uint8_t type,
55 uint32_t val) {
56 if (!write_tlv_start(buff, maxlen, ptr, type, 4))
57 return false;
58 uint32_t v = htonl(val);
59 memcpy(buff + ptr, &v, 4);
60 ptr += 4;
61 return true;
62 }
63
64 /* Protocol method. writes a Stats block into a TLV block */
write_tlv_stats(uint8_t * buff,int maxlen,int & ptr,uint8_t type,uint32_t age,int sttl,const beaconMcastState & st)65 static bool write_tlv_stats(uint8_t *buff, int maxlen, int &ptr, uint8_t type,
66 uint32_t age, int sttl, const beaconMcastState &st) {
67 if (!write_tlv_start(buff, maxlen, ptr, type, 20))
68 return false;
69
70 uint8_t *b = buff + ptr;
71
72 uint32_t val;
73
74 /* We use memcpy due to non-aligned write problems in some archs */
75 val = htonl((uint32_t)st.s.timestamp);
76 memcpy(b + 0, &val, sizeof(val));
77 val = htonl(age);
78 memcpy(b + 4, &val, sizeof(val));
79
80 // *((uint32_t *)(b + 0)) = htonl((uint32_t)st.s.timestamp);
81 // *((uint32_t *)(b + 4)) = htonl(age);
82
83 b[8] = (sttl ? sttl : defaultTTL) - st.s.rttl;
84
85 uint32_t *stats = (uint32_t *)(b + 9);
86
87 union {
88 uint32_t u;
89 float f;
90 } tu;
91
92 tu.f = st.s.avgdelay;
93 val = htonl(tu.u);
94 memcpy(stats + 0, &val, sizeof(val));
95
96 tu.f = st.s.avgjitter;
97 val = htonl(tu.u);
98 memcpy(stats + 1, &val, sizeof(val));
99
100 /* average loss in 0..255 range */
101 b[17] = (uint8_t)(st.s.avgloss * 0xff);
102 b[18] = st.s.avgdup > 10. ? 0xff : ((uint8_t)ceil(st.s.avgdup * 25.5));
103 b[19] = (uint8_t)(st.s.avgooo * 0xff);
104
105 ptr += 20;
106
107 return true;
108 }
109
build_report(uint8_t * buff,int maxlen,int type,bool publishsources)110 int build_report(uint8_t *buff, int maxlen, int type, bool publishsources) {
111 if (maxlen < 4)
112 return -1;
113
114 // 0-2 magic
115 *((uint16_t *)buff) = htons(0xbeac);
116
117 // 3 version
118 buff[2] = PROTO_VER;
119
120 // 4 packet type
121 buff[3] = 1; // Report
122
123 buff[4] = defaultTTL; // Original Hop Limit
124
125 int ptr = 5;
126
127 if (!write_tlv_string(buff, maxlen, ptr, T_BEAC_NAME, beaconName.c_str()))
128 return -1;
129 if (!write_tlv_string(buff, maxlen, ptr, T_ADMIN_CONTACT, adminContact.c_str()))
130 return -1;
131
132 if (type == WEBSITE_REPORT) {
133 for (WebSites::const_iterator j = webSites.begin(); j != webSites.end(); j++)
134 if (!write_tlv_string(buff, maxlen, ptr, j->first, j->second.c_str()))
135 return -1;
136 if (!twoLetterCC.empty()) {
137 if (!write_tlv_string(buff, maxlen, ptr, T_CC, twoLetterCC.c_str()))
138 return -1;
139 }
140 if (!write_tlv_uint(buff, maxlen, ptr, T_SOURCE_FLAGS, flags))
141 return -1;
142 return ptr;
143 } else if (type == LEAVE_REPORT) {
144 if (!write_tlv_start(buff, maxlen, ptr, T_LEAVE, 0))
145 return -1;
146 return ptr;
147 }
148
149 if (publishsources) {
150 uint64_t now = get_timestamp();
151
152 for (Sources::const_iterator i = sources.begin(); i != sources.end(); i++) {
153 if (type == MAP_REPORT && !i->second.identified)
154 continue;
155
156 if (!i->second.ASM.s.valid && !i->second.SSM.s.valid)
157 continue;
158
159 int len = 18;
160
161 if (i->first.family() == AF_INET)
162 len = 6;
163
164 if (type == MAP_REPORT) {
165 int namelen = i->second.name.size();
166 int contactlen = i->second.adminContact.size();
167 len += 2 + namelen + 2 + contactlen;
168 } else {
169 len += (i->second.ASM.s.valid ? 22 : 0) + (i->second.SSM.s.valid ? 22 : 0);
170 }
171
172 if (!write_tlv_start(buff, maxlen, ptr, i->first.family() == AF_INET6 ? T_SOURCE_INFO : T_SOURCE_INFO_IPv4, len))
173 break;
174
175 if (i->first.family() == AF_INET6) {
176 const sockaddr_in6 *addr = i->first.v6();
177
178 memcpy(buff + ptr, &addr->sin6_addr, sizeof(in6_addr));
179 memcpy(buff + ptr + 16, &addr->sin6_port, sizeof(uint16_t));
180
181 ptr += 18;
182 } else {
183 const sockaddr_in *addr = i->first.v4();
184
185 memcpy(buff + ptr, &addr->sin_addr, sizeof(in_addr));
186 memcpy(buff + ptr + 4, &addr->sin_port, sizeof(uint16_t));
187
188 ptr += 6;
189 }
190
191 if (type == MAP_REPORT) {
192 write_tlv_string(buff, maxlen, ptr, T_BEAC_NAME, i->second.name.c_str());
193 write_tlv_string(buff, maxlen, ptr, T_ADMIN_CONTACT, i->second.adminContact.c_str());
194 } else {
195 uint32_t age = (now - i->second.creation) / 1000;
196
197 if (i->second.ASM.s.valid)
198 write_tlv_stats(buff, maxlen, ptr, T_ASM_STATS, age, i->second.sttl, i->second.ASM);
199 if (i->second.SSM.s.valid)
200 write_tlv_stats(buff, maxlen, ptr, T_SSM_STATS, age, i->second.sttl, i->second.SSM);
201 }
202 }
203 }
204
205 return ptr;
206 }
207
build_probe(uint8_t * buff,int maxlen,uint32_t sn,uint64_t ts)208 int build_probe(uint8_t *buff, int maxlen, uint32_t sn, uint64_t ts) {
209 if (maxlen < (int)(4 + 4 + 4))
210 return -1;
211
212 // 0-2 magic
213 *((uint16_t *)buff) = htons(0xbeac);
214
215 // 3 version
216 buff[2] = PROTO_VER;
217
218 // 4 packet type
219 buff[3] = 0; // Probe
220
221 *((uint32_t *)(buff + 4)) = htonl(sn);
222 *((uint32_t *)(buff + 8)) = htonl((uint32_t)ts);
223
224 return 4 + 4 + 4;
225 }
226
tlv_begin(uint8_t * hd,int & len)227 static inline uint8_t *tlv_begin(uint8_t *hd, int &len) {
228 if (len < 2 || hd[1] > len)
229 return 0;
230 return hd;
231 }
232
tlv_next(uint8_t * hd,int & len)233 static inline uint8_t *tlv_next(uint8_t *hd, int &len) {
234 len -= hd[1] + 2;
235 return tlv_begin(hd + hd[1] + 2, len);
236 }
237
read_tlv_stats(uint8_t * tlv,beaconExternalStats & extb,Stats & st)238 static bool read_tlv_stats(uint8_t *tlv, beaconExternalStats &extb, Stats &st) {
239 if (tlv[1] != 20)
240 return false;
241
242 uint32_t tmp;
243
244 memcpy(&tmp, tlv + 2, sizeof(tmp));
245 st.timestamp = ntohl(tmp);
246 memcpy(&tmp, tlv + 6, sizeof(tmp));
247 extb.age = ntohl(tmp);
248
249 // st.timestamp = ntohl(*(uint32_t *)(tlv + 2));
250 // extb.age = ntohl(*(uint32_t *)(tlv + 6));
251
252 st.rttl = tlv[10];
253
254 union {
255 uint32_t u;
256 float f;
257 } tu;
258
259 memcpy(&tmp, tlv + 11, sizeof(tmp));
260 tu.u = ntohl(tmp);
261 st.avgdelay = tu.f;
262
263 memcpy(&tmp, tlv + 15, sizeof(tmp));
264 tu.u = ntohl(tmp);
265 st.avgjitter = tu.f;
266
267 st.avgloss = tlv[19] / 255.;
268 st.avgdup = tlv[20] == 0xff ? 1e10 : tlv[20] / 25.5;
269 st.avgooo = tlv[21] / 255.;
270
271 st.valid = true;
272
273 return true;
274 }
275
check_string(char * hd,int len,string & result)276 static inline bool check_string(char *hd, int len, string &result) {
277 for (int i = 0; i < len; i++) {
278 if (!isprint(hd[i]))
279 return false;
280 }
281 result = string(hd, len);
282 return true;
283 }
284
handle_nmsg(const address & from,uint64_t recvdts,int ttl,uint8_t * buff,int len,bool ssm)285 void handle_nmsg(const address &from, uint64_t recvdts, int ttl, uint8_t *buff, int len, bool ssm) {
286 if (len < 4)
287 return;
288
289 if (ntohs(*((uint16_t *)buff)) != 0xbeac)
290 return;
291
292 if (buff[2] != PROTO_VER)
293 return;
294
295 uint64_t now = get_timestamp();
296
297 if (buff[3] == 0) {
298 if (len == 12) {
299 uint32_t seq = ntohl(*((uint32_t *)(buff + 4)));
300 uint32_t ts = ntohl(*((uint32_t *)(buff + 8)));
301 getSource(from, 0, now, recvdts, true).update(ttl, seq, ts, now, recvdts, ssm);
302 }
303 return;
304 } else if (buff[3] == 1) {
305 if (len < 5)
306 return;
307
308 beaconSource &src = getSource(from, 0, now, recvdts, true);
309
310 src.sttl = buff[4];
311
312 len -= 5;
313
314 for (uint8_t *hd = tlv_begin(buff + 5, len); hd; hd = tlv_next(hd, len)) {
315 if (verbose > 4) {
316 char tmp[64];
317 from.print(tmp, sizeof(tmp));
318 fprintf(stderr, "Parsing TLV (%i, %i) for %s [len is now %i]\n", (int)hd[0], (int)hd[1], tmp, len);
319 }
320
321 if (hd[0] == T_BEAC_NAME) {
322 string name;
323 if (check_string((char *)hd + 2, hd[1], name))
324 src.setName(name);
325 } else if (hd[0] == T_ADMIN_CONTACT) {
326 check_string((char *)hd + 2, hd[1], src.adminContact);
327 } else if (hd[0] == T_SOURCE_INFO || hd[0] == T_SOURCE_INFO_IPv4) {
328 int blen = hd[0] == T_SOURCE_INFO ? 18 : 6;
329
330 if (hd[1] < blen)
331 continue;
332
333 address addr;
334
335 if (hd[0] == T_SOURCE_INFO) {
336 sockaddr_in6 *a6 = (sockaddr_in6 *)&addr;
337
338 a6->sin6_family = AF_INET6;
339
340 memcpy(&a6->sin6_addr, hd + 2, sizeof(in6_addr));
341 memcpy(&a6->sin6_port, hd + 18, sizeof(uint16_t));
342 } else {
343 sockaddr_in *a4 = (sockaddr_in *)&addr;
344
345 a4->sin_family = AF_INET;
346
347 memcpy(&a4->sin_addr, hd + 2, sizeof(in_addr));
348 memcpy(&a4->sin_port, hd + 6, sizeof(uint16_t));
349 }
350
351 beaconExternalStats &stats = src.getExternal(addr, now, recvdts);
352
353 int plen = hd[1] - blen;
354 for (uint8_t *pd = tlv_begin(hd + 2 + blen, plen); pd; pd = tlv_next(pd, plen)) {
355 if (pd[0] == T_BEAC_NAME) {
356 if (check_string((char *)pd + 2, pd[1], stats.name)) {
357 stats.identified = !stats.name.empty();
358 }
359 } else if (pd[0] == T_ADMIN_CONTACT) {
360 check_string((char *)pd + 2, pd[1], stats.contact);
361 } else if (pd[0] == T_ASM_STATS || pd[0] == T_SSM_STATS) {
362 Stats *st = (pd[0] == T_ASM_STATS ? &stats.ASM : &stats.SSM);
363
364 if (!read_tlv_stats(pd, stats, *st))
365 break;
366 st->lastupdate = now;
367 }
368 }
369
370 // trigger local SSM join
371 if (!addr.is_equal(beaconUnicastAddr)) {
372 beaconSource &t = getSource(addr, stats.identified ? stats.name.c_str() : 0, now, recvdts, false);
373 if (t.adminContact.empty())
374 t.adminContact = stats.contact;
375 }
376 } else if (hd[0] == T_WEBSITE_GENERIC || hd[0] == T_WEBSITE_LG || hd[0] == T_WEBSITE_MATRIX) {
377 string url;
378 if (check_string((char *)hd + 2, hd[1], url)) {
379 src.webSites[hd[0]] = url;
380 }
381 } else if (hd[0] == T_CC) {
382 if (hd[1] == 2) {
383 src.CC = string((char *)hd + 2, 2);
384 }
385 } else if (hd[0] == T_SOURCE_FLAGS) {
386 if (hd[1] == 4) {
387 uint32_t v;
388 memcpy(&v, hd + 2, 4);
389 src.Flags = ntohl(v);
390 }
391 } else if (hd[0] == T_LEAVE) {
392 removeSource(from, false);
393 break;
394 }
395 }
396 }
397 }
398
399