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 <err.h>
22 #include <stdio.h>
23 #include <string.h>
24
25 #include "logging.h"
26 #include "msg.h"
27 #include "omping.h"
28 #include "tlv.h"
29 #include "util.h"
30
31 /*
32 * Create answer message from query message. orig_msg is pointer to buffer with query message
33 * with orig_msg_len length (only used bytes, not buffer size). new_msg is pointer to buffer where
34 * to store result message. new_msg_len is size of buffer. ttl is value of TTL option. server_tstamp
35 * is boolean variable and if set, server timestamp option is added to message.
36 *
37 * All options from original messages are copied without changing order. Only exceptions are Server
38 * Info, Multicast Prefix, Session ID, TTL and Server Timestamp, which are not copied.
39 *
40 * Returned value is size of new message or 0 on fail (mostly because msg_len
41 * is smaller then needed). If success, new message is always at least 1 bytes long.
42 */
43 size_t
msg_answer_create(const char * orig_msg,size_t orig_msg_len,char * new_msg,size_t new_msg_len,uint8_t ttl,int server_tstamp)44 msg_answer_create(const char *orig_msg, size_t orig_msg_len, char *new_msg, size_t new_msg_len,
45 uint8_t ttl, int server_tstamp)
46 {
47 struct tlv_iterator tlv_iter;
48 enum tlv_opt_type opt_type;
49 size_t pos;
50
51 pos = 0;
52
53 new_msg[pos++] = (unsigned char)MSG_TYPE_ANSWER;
54
55 memset(&tlv_iter, 0, sizeof(tlv_iter));
56 tlv_iter_init(orig_msg, orig_msg_len, &tlv_iter);
57
58 while (tlv_iter_next(&tlv_iter) != -1) {
59 opt_type = tlv_iter_get_type(&tlv_iter);
60 if (opt_type != TLV_OPT_TYPE_SERVER_INFO &&
61 opt_type != TLV_OPT_TYPE_MCAST_PREFIX &&
62 opt_type != TLV_OPT_TYPE_SES_ID &&
63 opt_type != TLV_OPT_TYPE_TTL &&
64 opt_type != TLV_OPT_TYPE_SERVER_TSTAMP) {
65 tlv_iter_item_copy(&tlv_iter, new_msg, new_msg_len, &pos);
66 }
67 }
68
69 if (tlv_add_ttl(new_msg, new_msg_len, &pos, ttl) == -1)
70 goto small_buf_err;
71
72 if (server_tstamp) {
73 if (tlv_add_server_tstamp(new_msg, new_msg_len, &pos) == -1)
74 goto small_buf_err;
75 }
76
77 return (pos);
78
79 small_buf_err:
80 return (0);
81 }
82
83 /*
84 * Decode message. Decoded message is stored in msg_decoded structure.
85 */
86 void
msg_decode(const char * msg,size_t msg_len,struct msg_decoded * decoded)87 msg_decode(const char *msg, size_t msg_len, struct msg_decoded *decoded)
88 {
89 char debug_str[128];
90 struct tlv_iterator tlv_iter;
91 size_t pos;
92 uint32_t u32, u32_2;
93 uint16_t tlv_len;
94 uint16_t u16;
95 uint8_t u8;
96
97 memset(decoded, 0, sizeof(struct msg_decoded));
98
99 decoded->msg_type = (enum msg_type)msg[0];
100
101 DEBUG2_PRINTF("Message type %c (0x%X)", decoded->msg_type, decoded->msg_type);
102
103 tlv_iter_init(msg, msg_len, &tlv_iter);
104
105 while (tlv_iter_next(&tlv_iter) == 0) {
106 tlv_len = tlv_iter_get_len(&tlv_iter);
107
108 if (logging_get_verbose() >= LOGGING_LEVEL_DEBUG2) {
109 snprintf(debug_str, sizeof(debug_str), "%u:%s:%u:",
110 tlv_iter_get_type(&tlv_iter),
111 tlv_opt_type_to_str(tlv_iter_get_type(&tlv_iter)), tlv_len);
112 }
113
114 switch (tlv_iter_get_type(&tlv_iter)) {
115 case TLV_OPT_TYPE_VERSION:
116 if (tlv_len == 1) {
117 memcpy(&u8, tlv_iter_get_data(&tlv_iter), sizeof(u8));
118
119 decoded->version = u8;
120
121 DEBUG2_PRINTF("%s%u", debug_str, u8);
122 } else {
123 DEBUG2_PRINTF("%slen != 1", debug_str);
124 }
125 break;
126 case TLV_OPT_TYPE_CLIENT_ID:
127 if (tlv_len > 0) {
128 decoded->client_id_len = tlv_len;
129 decoded->client_id = tlv_iter_get_data(&tlv_iter);
130
131 DEBUG2_HEXDUMP(debug_str, decoded->client_id, tlv_len);
132 } else {
133 DEBUG2_PRINTF("%slen <= 0", debug_str);
134 }
135 break;
136 case TLV_OPT_TYPE_SEQ_NUM:
137 if (tlv_len == 4) {
138 decoded->seq_num_isset = 1;
139 memcpy(&u32, tlv_iter_get_data(&tlv_iter), sizeof(u32));
140 u32 = ntohl(u32);
141 decoded->seq_num = u32;
142
143 DEBUG2_PRINTF("%s%u", debug_str, u32);
144 } else {
145 DEBUG2_PRINTF("%slen != 4", debug_str);
146 }
147 break;
148 case TLV_OPT_TYPE_CLIENT_TSTAMP:
149 if (tlv_len == 8) {
150 memcpy(&u32, tlv_iter_get_data(&tlv_iter), sizeof(u32));
151 u32 = ntohl(u32);
152 decoded->client_tstamp.tv_sec = u32;
153
154 memcpy(&u32_2, tlv_iter_get_data(&tlv_iter) + sizeof(u32),
155 sizeof(u32_2));
156 u32_2 = ntohl(u32_2);
157 decoded->client_tstamp.tv_usec = u32_2;
158
159 decoded->client_tstamp_isset = 1;
160
161 DEBUG2_PRINTF("%s(%u,%u)", debug_str, u32, u32_2);
162 } else {
163 DEBUG2_PRINTF("%slen != 8", debug_str);
164 }
165 break;
166 case TLV_OPT_TYPE_MCAST_GRP:
167 if (tlv_len > 2) {
168 memcpy(&u16, tlv_iter_get_data(&tlv_iter), sizeof(u16));
169 u16 = ntohs(u16);
170
171 if ((u16 == AF_IANA_IP && tlv_len == 6) ||
172 (u16 == AF_IANA_IP6 && tlv_len == 18)) {
173 decoded->mcast_grp_len = tlv_len;
174 decoded->mcast_grp = tlv_iter_get_data(&tlv_iter);
175
176 DEBUG2_PRINTF("%sAF %u", debug_str, u16);
177 } else {
178 DEBUG2_PRINTF("%sbad AF %u or len", debug_str, u16);
179 }
180 } else {
181 DEBUG2_PRINTF("%slen <= 2", debug_str);
182 }
183 break;
184 case TLV_OPT_TYPE_OPT_REQUEST:
185 if (tlv_len > 1 && (tlv_len % 2 == 0)) {
186 for (pos = 0; pos < (uint16_t)(tlv_len / 2); pos++) {
187 memcpy(&u16, tlv_iter_get_data(&tlv_iter) + pos * 2,
188 sizeof(u16));
189
190 u16 = ntohs(u16);
191
192 switch (u16) {
193 case TLV_OPT_TYPE_SERVER_INFO:
194 decoded->request_opt_server_info = 1;
195
196 DEBUG2_PRINTF("%s%zu opt %u", debug_str, pos, u16);
197 break;
198 case TLV_OPT_TYPE_SERVER_TSTAMP:
199 decoded->request_opt_server_tstamp = 1;
200
201 DEBUG2_PRINTF("%s%zu opt %u", debug_str, pos, u16);
202 break;
203 default:
204 DEBUG2_PRINTF("%s%zu unknown opt %u", debug_str,
205 pos, u16);
206 break;
207 }
208 }
209 } else {
210 DEBUG2_PRINTF("%slen <= 1 || (tlv_len %%2 != 0)", debug_str);
211 }
212 break;
213 case TLV_OPT_TYPE_SERVER_INFO:
214 if (tlv_len > 0) {
215 decoded->server_info = tlv_iter_get_data(&tlv_iter);
216 decoded->server_info_len = tlv_len;
217
218 DEBUG2_HEXDUMP(debug_str, decoded->server_info, tlv_len);
219 } else {
220 DEBUG2_PRINTF("%slen <= 0", debug_str);
221 }
222 break;
223 case TLV_OPT_TYPE_TTL:
224 if (tlv_len == 1) {
225 memcpy(&u8, tlv_iter_get_data(&tlv_iter), sizeof(u8));
226
227 decoded->ttl = u8;
228
229 DEBUG2_PRINTF("%s%u", debug_str, u8);
230 } else {
231 DEBUG2_PRINTF("%slen != 1", debug_str);
232 }
233 break;
234 case TLV_OPT_TYPE_MCAST_PREFIX:
235 if (tlv_len > 2) {
236 memcpy(&u16, tlv_iter_get_data(&tlv_iter), sizeof(u16));
237 u16 = ntohs(u16);
238
239 if (u16 == AF_IANA_IP || u16 == AF_IANA_IP6) {
240 decoded->mcast_prefix_isset = 1;
241
242 DEBUG2_PRINTF("%sAF %u", debug_str, u16);
243 } else {
244 DEBUG2_PRINTF("%sbad AF %u", debug_str, u16);
245 }
246 } else {
247 DEBUG2_PRINTF("%slen <= 2", debug_str);
248 }
249 break;
250 case TLV_OPT_TYPE_SES_ID:
251 if (tlv_len > 0) {
252 decoded->ses_id_len = tlv_len;
253 decoded->ses_id = tlv_iter_get_data(&tlv_iter);
254
255 DEBUG2_HEXDUMP(debug_str, decoded->ses_id, tlv_len);
256 } else {
257 DEBUG2_PRINTF("%slen <= 0", debug_str);
258 }
259 break;
260 case TLV_OPT_TYPE_SERVER_TSTAMP:
261 if (tlv_len == 8) {
262 memcpy(&u32, tlv_iter_get_data(&tlv_iter), sizeof(u32));
263 u32 = ntohl(u32);
264 decoded->server_tstamp.tv_sec = u32;
265
266 memcpy(&u32_2, tlv_iter_get_data(&tlv_iter) + sizeof(u32),
267 sizeof(u32_2));
268 u32_2 = ntohl(u32_2);
269 decoded->server_tstamp.tv_usec = u32_2;
270
271 decoded->server_tstamp_isset = 1;
272
273 DEBUG2_PRINTF("%s(%u,%u)", debug_str, u32, u32_2);
274 } else {
275 DEBUG2_PRINTF("%slen != 8", debug_str);
276 }
277 break;
278 default:
279 DEBUG2_PRINTF("%s", debug_str);
280 break;
281 }
282 }
283 }
284
285 /*
286 * Create init message. msg is pointer to buffer where to store result message. msg_len is size of
287 * buffer. mcast_addr is required multicast address. client_id is client ID to store in message with
288 * length client_id_len.
289 *
290 * Returned value is size of new message or 0 on fail (mostly because msg_len
291 * is smaller then needed). If success, new message is always at least 1 bytes long.
292 */
293 size_t
msg_init_create(char * msg,size_t msg_len,int req_si,const struct sockaddr_storage * mcast_addr,const char * client_id,size_t client_id_len)294 msg_init_create(char *msg, size_t msg_len, int req_si, const struct sockaddr_storage *mcast_addr,
295 const char *client_id, size_t client_id_len)
296 {
297 size_t pos;
298 uint16_t u16;
299
300 pos = 0;
301
302 if (client_id == NULL) {
303 return (0);
304 }
305
306 msg[pos++] = (unsigned char)MSG_TYPE_INIT;
307
308 if (tlv_add_version(msg, msg_len, &pos) == -1)
309 goto small_buf_err;
310
311 if (tlv_add(msg, msg_len, &pos, TLV_OPT_TYPE_CLIENT_ID, client_id_len, client_id) == -1)
312 goto small_buf_err;
313
314
315 if (req_si) {
316 u16 = TLV_OPT_TYPE_SERVER_INFO;
317
318 if (tlv_add_opt_request(msg, msg_len, &pos, &u16, 1) == -1)
319 goto small_buf_err;
320 }
321
322
323 if (tlv_add_mcast_prefix(msg, msg_len, &pos, mcast_addr) == -1)
324 goto small_buf_err;
325
326 return (pos);
327
328 small_buf_err:
329 return (0);
330 }
331
332 /*
333 * Create query message. msg is pointer to buffer where to store result message. msg_len is size
334 * of buffer. mcast_addr is required multicast group address. server_tstamp is boolean to decide if
335 * to include Option request option with server time stamp. client_id is Client ID with length
336 * client_id_len. session_id with session_id_len is similar, but for Session ID.
337 *
338 * Returned value is size of new message or 0 on fail (mostly because msg_len
339 * is smaller then needed). If success, new message is always at least 1 bytes long.
340 */
341 size_t
msg_query_create(char * msg,size_t msg_len,const struct sockaddr_storage * mcast_addr,uint32_t seq_num,int server_tstamp,const char * client_id,size_t client_id_len,const char * session_id,size_t session_id_len)342 msg_query_create(char *msg, size_t msg_len, const struct sockaddr_storage *mcast_addr,
343 uint32_t seq_num, int server_tstamp, const char *client_id, size_t client_id_len,
344 const char *session_id, size_t session_id_len)
345 {
346 size_t pos;
347 uint16_t u16;
348
349 pos = 0;
350
351 msg[pos++] = (unsigned char)MSG_TYPE_QUERY;
352
353 if (tlv_add_version(msg, msg_len, &pos) == -1)
354 goto small_buf_err;
355
356 if (client_id) {
357 if (tlv_add(msg, msg_len, &pos, TLV_OPT_TYPE_CLIENT_ID, client_id_len,
358 client_id) == -1) {
359 goto small_buf_err;
360 }
361 }
362
363 if (tlv_add_seq_num(msg, msg_len, &pos, seq_num) == -1)
364 goto small_buf_err;
365
366 if (tlv_add_client_tstamp(msg, msg_len, &pos) == -1)
367 goto small_buf_err;
368
369 if (tlv_add_mcast_grp(msg, msg_len, &pos, mcast_addr) == -1)
370 goto small_buf_err;
371
372 if (server_tstamp) {
373 u16 = TLV_OPT_TYPE_SERVER_TSTAMP;
374
375 if (tlv_add_opt_request(msg, msg_len, &pos, &u16, 1) == -1)
376 goto small_buf_err;
377 }
378
379 if (tlv_add(msg, msg_len, &pos, TLV_OPT_TYPE_SES_ID, session_id_len, session_id) == -1)
380 goto small_buf_err;
381
382 return (pos);
383
384 small_buf_err:
385 return (0);
386 }
387
388 /*
389 * Create response message. msg is pointer to buffer where to store result message. msg_len is size
390 * of buffer. msg_decoded is decoded init message used for some informations (like client id, ...).
391 * mcast_grp and mcast_prefix are booleans used for decision, if to add Multicast Group and/or
392 * Multicast Prefix options. mcast_addr is value for mcast_grp and/or mcast_prefix. If none of this
393 * options is/are required, mcasr_addr can be set to NULL. ulticast address. session_id is
394 * session ID of client.
395 *
396 * Returned value is size of new message or 0 on fail (mostly because msg_len
397 * is smaller then needed). If success, new message is always at least 1 bytes long.
398 */
399 size_t
msg_response_create(char * msg,size_t msg_len,const struct msg_decoded * msg_decoded,int mcast_grp,int mcast_prefix,const struct sockaddr_storage * mcast_addr,const char * session_id,size_t session_id_len)400 msg_response_create(char *msg, size_t msg_len, const struct msg_decoded *msg_decoded,
401 int mcast_grp, int mcast_prefix, const struct sockaddr_storage *mcast_addr,
402 const char *session_id, size_t session_id_len)
403 {
404 size_t pos;
405
406 pos = 0;
407
408 msg[pos++] = (unsigned char)MSG_TYPE_RESPONSE;
409 if (tlv_add_version(msg, msg_len, &pos) == -1)
410 goto small_buf_err;
411
412 if (msg_decoded->client_id) {
413 if (tlv_add(msg, msg_len, &pos, TLV_OPT_TYPE_CLIENT_ID, msg_decoded->client_id_len,
414 msg_decoded->client_id) == -1)
415 goto small_buf_err;
416 }
417
418 if (msg_decoded->seq_num_isset) {
419 if (tlv_add_seq_num(msg, msg_len, &pos, msg_decoded->seq_num) == -1)
420 goto small_buf_err;
421 }
422
423 if (mcast_grp) {
424 if (tlv_add_mcast_grp(msg, msg_len, &pos, mcast_addr) == -1)
425 goto small_buf_err;
426 }
427
428 if (msg_decoded->request_opt_server_info) {
429 if (tlv_add_server_info(msg, msg_len, &pos, PROGRAM_SERVER_INFO) == -1)
430 goto small_buf_err;
431 }
432
433 if (mcast_prefix) {
434 if (tlv_add_mcast_prefix(msg, msg_len, &pos, mcast_addr) == -1)
435 goto small_buf_err;
436 }
437
438 if (session_id) {
439 if (tlv_add(msg, msg_len, &pos, TLV_OPT_TYPE_SES_ID, session_id_len,
440 session_id) == -1) {
441 goto small_buf_err;
442 }
443 }
444
445 return (pos);
446
447 small_buf_err:
448 return (0);
449 }
450
451 /*
452 * Update Server Timestamp option in message to current time stamp. msg is pointer to buffer with
453 * message and msg_len is length of message (without unused space).
454 * Function returns 0 on success, otherwise -1.
455 */
456 int
msg_update_server_tstamp(char * msg,size_t msg_len)457 msg_update_server_tstamp(char *msg, size_t msg_len)
458 {
459 struct tlv_iterator tlv_iter;
460 size_t pos;
461
462 memset(&tlv_iter, 0, sizeof(tlv_iter));
463 tlv_iter_init(msg, msg_len, &tlv_iter);
464
465 while (tlv_iter_next(&tlv_iter) != -1) {
466 if (tlv_iter_get_type(&tlv_iter) == TLV_OPT_TYPE_SERVER_TSTAMP) {
467 pos = tlv_iter.pos;
468
469 if (tlv_add_server_tstamp(msg, msg_len, &pos) == -1)
470 goto add_tstamp_err;
471 }
472 }
473
474 return (0);
475
476 add_tstamp_err:
477 return (-1);
478 }
479