1 /*
2 * Copyright (c) 2010-2011, Red Hat, Inc.
3 *
4 * Permission to use, copy, modify, and/or distribute this software for any
5 * purpose with or without fee is hereby granted, provided that the above
6 * copyright notice and this permission notice appear in all copies.
7 *
8 * THE SOFTWARE IS PROVIDED "AS IS" AND RED HAT, INC. DISCLAIMS ALL WARRANTIES
9 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES
10 * OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL RED HAT, INC. BE LIABLE
11 * FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
12 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
13 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
14 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
15 */
16
17 /*
18 * Author: Jan Friesse <jfriesse@redhat.com>
19 */
20
21 #include <arpa/inet.h>
22
23 #include <netinet/in.h>
24
25 #ifdef __sun
26 #include <alloca.h>
27 #endif /* __sun */
28
29 #include <err.h>
30 #define __STDC_FORMAT_MACROS
31 #include <inttypes.h>
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <string.h>
35
36 #include "addrfunc.h"
37 #include "logging.h"
38 #include "omping.h"
39 #include "tlv.h"
40 #include "util.h"
41
42 static int tlv_add_actual_ts(char *msg, size_t msg_len, size_t *pos, enum tlv_opt_type opt);
43
44 static int tlv_add_sas(char *msg, size_t msg_len, size_t *pos, enum tlv_opt_type opt,
45 const struct sockaddr_storage *sas, int store_prefix_len);
46
47 static int tlv_add_ts(char *msg, size_t msg_len, size_t *pos, enum tlv_opt_type opt,
48 struct timeval *tv);
49
50 static int tlv_add_u8(char *msg, size_t msg_len, size_t *pos, enum tlv_opt_type opt,
51 uint8_t val);
52
53 /*
54 * Add option opt_type with length opt_len and value to message msg with msg_len length to position
55 * pos. Position is automatically adjusted to new position, so subsequent calls of function add
56 * new option to correct position. Function returns 0 on success, otherwise -1.
57 */
58 int
tlv_add(char * msg,size_t msg_len,size_t * pos,enum tlv_opt_type opt_type,uint16_t opt_len,const void * value)59 tlv_add(char *msg, size_t msg_len, size_t *pos, enum tlv_opt_type opt_type, uint16_t opt_len,
60 const void *value)
61 {
62 uint16_t nlen;
63 uint16_t nopt_type;
64
65 DEBUG2_PRINTF("Add option %"PRIu16" with len %"PRIu16" pos %zu", opt_type, opt_len, *pos);
66
67 if (*pos + sizeof(nopt_type) + sizeof(nlen) + opt_len > msg_len) {
68 DEBUG2_PRINTF("Can't store option. msg_len too small.");
69 return (-1);
70 }
71
72 nopt_type = ntohs((uint16_t)opt_type);
73 memcpy(msg + *pos, &nopt_type, sizeof(nopt_type));
74 *pos += sizeof(nopt_type);
75
76 nlen = htons(opt_len);
77
78 memcpy(msg + *pos, &nlen, sizeof(nlen));
79 *pos += sizeof(nlen);
80
81 memcpy(msg + *pos, value, opt_len);
82
83 *pos += opt_len;
84
85 return (0);
86 }
87
88 /*
89 * Add TLV with actual time stamp
90 */
91 static int
tlv_add_actual_ts(char * msg,size_t msg_len,size_t * pos,enum tlv_opt_type opt)92 tlv_add_actual_ts(char *msg, size_t msg_len, size_t *pos, enum tlv_opt_type opt)
93 {
94 struct timeval tv;
95
96 tv = util_get_time();
97
98 return (tlv_add_ts(msg, msg_len, pos, opt, &tv));
99 }
100
101 /*
102 * Add TLV with actual client time stamp
103 */
104 int
tlv_add_client_tstamp(char * msg,size_t msg_len,size_t * pos)105 tlv_add_client_tstamp(char *msg, size_t msg_len, size_t *pos)
106 {
107 return (tlv_add_actual_ts(msg, msg_len, pos, TLV_OPT_TYPE_CLIENT_TSTAMP));
108 }
109
110 /*
111 * Add TLV with mcast group
112 */
113 int
tlv_add_mcast_grp(char * msg,size_t msg_len,size_t * pos,const struct sockaddr_storage * sas)114 tlv_add_mcast_grp(char *msg, size_t msg_len, size_t *pos, const struct sockaddr_storage *sas)
115 {
116 return (tlv_add_sas(msg, msg_len, pos, TLV_OPT_TYPE_MCAST_GRP, sas, 0));
117 }
118
119 /*
120 * Add TLV with mcast prefix
121 */
122 int
tlv_add_mcast_prefix(char * msg,size_t msg_len,size_t * pos,const struct sockaddr_storage * sas)123 tlv_add_mcast_prefix(char *msg, size_t msg_len, size_t *pos, const struct sockaddr_storage *sas)
124 {
125 return (tlv_add_sas(msg, msg_len, pos, TLV_OPT_TYPE_MCAST_PREFIX, sas, 1));
126 }
127
128 /*
129 * Add TLV with option request option. Options are passed in opts array with opts_len length.
130 */
131 int
tlv_add_opt_request(char * msg,size_t msg_len,size_t * pos,uint16_t * opts,size_t opts_len)132 tlv_add_opt_request(char *msg, size_t msg_len, size_t *pos, uint16_t *opts, size_t opts_len)
133 {
134 char *value;
135 size_t val_len;
136 unsigned int i;
137 uint16_t opt;
138
139 if (opts_len == 0)
140 return (-1);
141
142 val_len = opts_len * sizeof(uint16_t);
143
144 value = (char *)alloca(val_len);
145
146 for (i = 0; i < opts_len; i++) {
147 opt = htons(opts[i]);
148
149 memcpy(value + i * sizeof(opt), &opt, sizeof(opt));
150 }
151
152 return (tlv_add(msg, msg_len, pos, TLV_OPT_TYPE_OPT_REQUEST, val_len, value));
153 }
154
155 /*
156 * Add TLV with sockaddr_storage ip address. If store_prefix_len is set, prefix length of address
157 * (always full prefix) is also stored.
158 */
159 static int
tlv_add_sas(char * msg,size_t msg_len,size_t * pos,enum tlv_opt_type opt,const struct sockaddr_storage * sas,int store_prefix_len)160 tlv_add_sas(char *msg, size_t msg_len, size_t *pos, enum tlv_opt_type opt,
161 const struct sockaddr_storage *sas, int store_prefix_len)
162 {
163 char *value;
164 void *addr_pointer;
165 size_t addr_len;
166 size_t opt_len;
167 uint16_t af;
168 uint8_t pref_len_val;
169
170 switch (sas->ss_family) {
171 case AF_INET:
172 af = AF_IANA_IP;
173 addr_len = sizeof(struct in_addr);
174 addr_pointer = &((struct sockaddr_in *)sas)->sin_addr;
175 break;
176 case AF_INET6:
177 af = AF_IANA_IP6;
178 addr_len = sizeof(struct in6_addr);
179 addr_pointer = &((struct sockaddr_in6 *)sas)->sin6_addr;
180 break;
181 default:
182 DEBUG_PRINTF("Unknown sas family %d", sas->ss_family);
183 errx(1, "Unknown sas family %d", sas->ss_family);
184 }
185
186 pref_len_val = addr_len * 8;
187
188 opt_len = sizeof(af) + addr_len;
189
190 if (store_prefix_len)
191 opt_len += sizeof(pref_len_val);
192
193 value = (char *)alloca(opt_len);
194
195 af = htons(af);
196
197 memcpy(value, &af, sizeof(af));
198 if (store_prefix_len)
199 memcpy(value + sizeof(af), &pref_len_val, sizeof(pref_len_val));
200
201 memcpy(value + sizeof(af) + (store_prefix_len ? sizeof(pref_len_val) : 0), addr_pointer,
202 addr_len);
203
204 return (tlv_add(msg, msg_len, pos, opt, opt_len, value));
205
206 }
207
208 /*
209 * Add sequence number TLV.
210 */
211 int
tlv_add_seq_num(char * msg,size_t msg_len,size_t * pos,uint32_t seq)212 tlv_add_seq_num(char *msg, size_t msg_len, size_t *pos, uint32_t seq)
213 {
214 uint32_t nseq;
215
216 nseq = htonl(seq);
217 return (tlv_add(msg, msg_len, pos, TLV_OPT_TYPE_SEQ_NUM, sizeof(nseq), &nseq));
218 }
219
220 /*
221 * Add TLV with server info
222 */
223 int
tlv_add_server_info(char * msg,size_t msg_len,size_t * pos,const char * server_info)224 tlv_add_server_info(char *msg, size_t msg_len, size_t *pos, const char *server_info)
225 {
226 if (strlen(server_info) == 0)
227 return (-1);
228
229 return (tlv_add(msg, msg_len, pos, TLV_OPT_TYPE_SERVER_INFO, strlen(server_info),
230 server_info));
231 }
232
233 /*
234 * Add TLV with actual server timestamp
235 */
236 int
tlv_add_server_tstamp(char * msg,size_t msg_len,size_t * pos)237 tlv_add_server_tstamp(char *msg, size_t msg_len, size_t *pos)
238 {
239 return (tlv_add_actual_ts(msg, msg_len, pos, TLV_OPT_TYPE_SERVER_TSTAMP));
240 }
241
242 /*
243 * Add timestamp
244 */
245 static int
tlv_add_ts(char * msg,size_t msg_len,size_t * pos,enum tlv_opt_type opt,struct timeval * tv)246 tlv_add_ts(char *msg, size_t msg_len, size_t *pos, enum tlv_opt_type opt, struct timeval *tv)
247 {
248 char value[8];
249 uint32_t u32;
250
251 u32 = tv->tv_sec;
252 u32 = htonl(u32);
253 memcpy(value, &u32, sizeof(u32));
254
255 u32 = tv->tv_usec;
256 u32 = htonl(u32);
257 memcpy(value + sizeof(u32), &u32, sizeof(u32));
258
259 return (tlv_add(msg, msg_len, pos, opt, sizeof(value), value));
260 }
261
262 /*
263 * Add server's TTL TLV
264 */
265 int
tlv_add_ttl(char * msg,size_t msg_len,size_t * pos,uint8_t ttl)266 tlv_add_ttl(char *msg, size_t msg_len, size_t *pos, uint8_t ttl)
267 {
268 return (tlv_add_u8(msg, msg_len, pos, TLV_OPT_TYPE_TTL, ttl));
269 }
270
271 /*
272 * Add uint8_t type as option opt.
273 */
274 static int
tlv_add_u8(char * msg,size_t msg_len,size_t * pos,enum tlv_opt_type opt,uint8_t val)275 tlv_add_u8(char *msg, size_t msg_len, size_t *pos, enum tlv_opt_type opt, uint8_t val)
276 {
277 return (tlv_add(msg, msg_len, pos, opt, sizeof(val), &val));
278 }
279
280 /*
281 * Add TLV with protocol version.
282 */
283 int
tlv_add_version(char * msg,size_t msg_len,size_t * pos)284 tlv_add_version(char *msg, size_t msg_len, size_t *pos)
285 {
286 uint8_t ver;
287
288 ver = PROTOCOL_VERSION;
289
290 return (tlv_add_u8(msg, msg_len, pos, TLV_OPT_TYPE_VERSION, ver));
291 }
292
293 /*
294 * Return pointer to tlv data
295 */
296 const char *
tlv_iter_get_data(const struct tlv_iterator * tlv_iter)297 tlv_iter_get_data(const struct tlv_iterator *tlv_iter)
298 {
299 return (tlv_iter->msg + tlv_iter->pos + 2 * sizeof(uint16_t));
300 }
301
302 /*
303 * Get length of item currently pointed by iterator
304 */
305 uint16_t
tlv_iter_get_len(const struct tlv_iterator * tlv_iter)306 tlv_iter_get_len(const struct tlv_iterator *tlv_iter)
307 {
308 uint16_t len;
309
310 memcpy(&len, tlv_iter->msg + tlv_iter->pos + sizeof(uint16_t), sizeof(len));
311 len = ntohs(len);
312
313 return (len);
314 }
315
316 /*
317 * Get type of item currently pointed by iterator
318 */
319 enum tlv_opt_type
tlv_iter_get_type(const struct tlv_iterator * tlv_iter)320 tlv_iter_get_type(const struct tlv_iterator *tlv_iter)
321 {
322 uint16_t res;
323
324 memcpy(&res, tlv_iter->msg + tlv_iter->pos, sizeof(res));
325 res = ntohs(res);
326
327 return ((enum tlv_opt_type)res);
328 }
329
330 /*
331 * Initialize iterator
332 */
333 void
tlv_iter_init(const char * msg,size_t msg_len,struct tlv_iterator * tlv_iter)334 tlv_iter_init(const char *msg, size_t msg_len, struct tlv_iterator *tlv_iter)
335 {
336
337 tlv_iter->msg = msg;
338 tlv_iter->msg_len = msg_len;
339 tlv_iter->pos = 0;
340 }
341
342 /*
343 * Copy item from message pointed with iterator tlv_iter to new message new_msg with new_msg_len
344 * length to position pos. Return 0 on success, and -1 on failure.
345 */
346 int
tlv_iter_item_copy(const struct tlv_iterator * tlv_iter,char * new_msg,size_t new_msg_len,size_t * pos)347 tlv_iter_item_copy(const struct tlv_iterator *tlv_iter, char *new_msg, size_t new_msg_len,
348 size_t *pos)
349 {
350 size_t item_size;
351
352 DEBUG2_PRINTF("Copy option %"PRIu16" with len %"PRIu16" pos %zu",
353 tlv_iter_get_type(tlv_iter), tlv_iter_get_len(tlv_iter), *pos);
354
355 item_size = tlv_iter_get_len(tlv_iter) + 2 * sizeof(uint16_t);
356
357 if (*pos + item_size > new_msg_len) {
358 DEBUG2_PRINTF("Can't copy option. new_msg_len too small.");
359
360 return (-1);
361 }
362
363 memcpy(new_msg + *pos, tlv_iter->msg + tlv_iter->pos, item_size);
364
365 *pos += item_size;
366
367 return (0);
368 }
369
370 /*
371 * Move iterator to the next item. Returns 0 when move was successful, or -1 if end of the message
372 * was reached.
373 */
374 int
tlv_iter_next(struct tlv_iterator * tlv_iter)375 tlv_iter_next(struct tlv_iterator *tlv_iter)
376 {
377 uint16_t nlen;
378
379 if (tlv_iter->pos == 0) {
380 tlv_iter->pos = 1;
381 return (0);
382 }
383
384 nlen = tlv_iter_get_len(tlv_iter);
385
386 if (tlv_iter->pos + sizeof(uint16_t) + sizeof(nlen) + nlen >= tlv_iter->msg_len) {
387 return (-1);
388 }
389
390 tlv_iter->pos += sizeof(uint16_t) + sizeof(nlen) + nlen;
391
392 return (0);
393 }
394
395 /*
396 * Compare msg item pointed by iterator of MCAST_PREFIX type with sockaddr address
397 */
398 int
tlv_iter_pref_eq(const struct tlv_iterator * tlv_iter,const struct sockaddr_storage * sas)399 tlv_iter_pref_eq(const struct tlv_iterator *tlv_iter, const struct sockaddr_storage *sas)
400 {
401 uint16_t tlv_len;
402 uint16_t u16;
403 uint8_t pref_len;
404 uint8_t min_len;
405
406 if (tlv_iter_get_type(tlv_iter) != TLV_OPT_TYPE_MCAST_PREFIX) {
407 return (0);
408 }
409
410 tlv_len = tlv_iter_get_len(tlv_iter);
411
412 if (tlv_len <= 2) {
413 return (0);
414 }
415
416 memcpy(&u16, tlv_iter_get_data(tlv_iter), sizeof(u16));
417 u16 = ntohs(u16);
418
419 if (u16 != AF_IANA_IP && u16 != AF_IANA_IP6) {
420 return (0);
421 }
422
423 memcpy(&pref_len, tlv_iter_get_data(tlv_iter) + 2, sizeof(pref_len));
424
425 min_len = pref_len / 8;
426 if (pref_len % 8 != 0)
427 min_len++;
428
429 if (tlv_len - 3 < min_len) {
430 return (0);
431 }
432
433 return (tlv_pref_eq(sas, u16, pref_len, tlv_iter_get_data(tlv_iter) + 3));
434 }
435
436 /*
437 * Compare sockaddr_storage address sas with mcast_grp received in message with length
438 * mcast_grp_len. Return 0 if addresses mismatch, otherwise not 0.
439 */
440 int
tlv_mcast_grp_eq(const struct sockaddr_storage * sas,const char * mcast_grp,size_t mcast_grp_len)441 tlv_mcast_grp_eq(const struct sockaddr_storage *sas, const char *mcast_grp, size_t mcast_grp_len)
442 {
443 uint16_t u16;
444
445 memcpy(&u16, mcast_grp, sizeof(u16));
446 u16 = ntohs(u16);
447
448 if (!((u16 == AF_IANA_IP && mcast_grp_len == 6) ||
449 (u16 == AF_IANA_IP6 && mcast_grp_len == 18))) {
450 return (0);
451 }
452
453 if (u16 == AF_IANA_IP && sas->ss_family != AF_INET) {
454 return (0);
455 }
456
457 if (u16 == AF_IANA_IP6 && sas->ss_family != AF_INET6) {
458 return (0);
459 }
460
461 return (tlv_pref_eq(sas, u16, (mcast_grp_len - 2) * 8, mcast_grp + 2));
462 }
463
464 /*
465 * Return static string with opt name
466 */
467 const char *
tlv_opt_type_to_str(enum tlv_opt_type opt)468 tlv_opt_type_to_str(enum tlv_opt_type opt)
469 {
470 const char *res;
471
472 switch (opt) {
473 case TLV_OPT_TYPE_VERSION: res = "Version"; break;
474 case TLV_OPT_TYPE_CLIENT_ID: res = "Client ID"; break;
475 case TLV_OPT_TYPE_SEQ_NUM: res = "Sequence Number"; break;
476 case TLV_OPT_TYPE_CLIENT_TSTAMP: res = "Client Timestamp"; break;
477 case TLV_OPT_TYPE_MCAST_GRP: res = "Multicast Group"; break;
478 case TLV_OPT_TYPE_OPT_REQUEST: res = "Option Request Option"; break;
479 case TLV_OPT_TYPE_SERVER_INFO: res = "Server Information"; break;
480 case TLV_OPT_TYPE_TTL: res = "TTL"; break;
481 case TLV_OPT_TYPE_MCAST_PREFIX: res = "Multicast Prefix"; break;
482 case TLV_OPT_TYPE_SES_ID: res = "Session ID"; break;
483 case TLV_OPT_TYPE_SERVER_TSTAMP: res = "Server Timestamp"; break;
484 default: res = "Unknown"; break;
485 }
486
487 return (res);
488 }
489
490 /*
491 * Compare prefix address with sockaddr_storage address. iana_af is IANA address family, prefix is
492 * prefix length and addr is pointer to bytes of prefixed address. Only needed number of bytes is
493 * compared.
494 */
495 int
tlv_pref_eq(const struct sockaddr_storage * sas,uint16_t iana_af,uint8_t prefix,const char * addr)496 tlv_pref_eq(const struct sockaddr_storage *sas, uint16_t iana_af, uint8_t prefix, const char *addr)
497 {
498 char sas_addr[32];
499 size_t sas_addr_len;
500 uint16_t sas_iana_af;
501 unsigned char cb1, cb2;
502 uint8_t plen_max, plen_min;
503
504 memset(sas_addr, 0, sizeof(sas_addr));
505
506 switch (sas->ss_family) {
507 case AF_INET:
508 sas_iana_af = AF_IANA_IP;
509
510 plen_min = 4;
511 plen_max = 32;
512
513 sas_addr_len = sizeof(struct in_addr);
514 memcpy(sas_addr, &((struct sockaddr_in *)sas)->sin_addr, sas_addr_len);
515 break;
516 case AF_INET6:
517 sas_iana_af = AF_IANA_IP6;
518
519 plen_min = 8;
520 plen_max = 128;
521
522 sas_addr_len = sizeof(struct in6_addr);
523 memcpy(sas_addr, &((struct sockaddr_in6 *)sas)->sin6_addr, sas_addr_len);
524 break;
525 default:
526 DEBUG_PRINTF("Unknown ss family %d", sas->ss_family);
527 errx(1, "Unknown ss family %d", sas->ss_family);
528 }
529
530 if (iana_af != sas_iana_af) {
531 return (0);
532 }
533
534 if (prefix == 0) {
535 /*
536 * Wildcard
537 */
538 return (1);
539 }
540
541 if (prefix < plen_min || prefix > plen_max) {
542 return (0);
543 }
544
545 /*
546 * Full bytes comparation
547 */
548 if (memcmp(sas_addr, addr, prefix / 8) != 0) {
549 return (0);
550 }
551
552
553 /*
554 * Rest bit comparation
555 */
556 if (prefix % 8 != 0 && prefix / 8 < sizeof(sas_addr_len)) {
557 cb1 = (unsigned char)(sas_addr[prefix / 8] & (0xff << (8 - (prefix % 8))));
558 cb2 = (unsigned char)(addr[prefix / 8] & (0xff << (8 - (prefix % 8))));
559 if (cb1 != cb2) {
560 return (0);
561 }
562 }
563
564 return (1);
565 }
566