]>
Commit | Line | Data |
---|---|---|
896014f4 | 1 | /* |
718e3744 | 2 | * Packet interface |
3 | * Copyright (C) 1999 Kunihiro Ishiguro | |
4 | * | |
5 | * This file is part of GNU Zebra. | |
6 | * | |
7 | * GNU Zebra is free software; you can redistribute it and/or modify it | |
8 | * under the terms of the GNU General Public License as published by the | |
9 | * Free Software Foundation; either version 2, or (at your option) any | |
10 | * later version. | |
11 | * | |
12 | * GNU Zebra is distributed in the hope that it will be useful, but | |
13 | * WITHOUT ANY WARRANTY; without even the implied warranty of | |
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | |
15 | * General Public License for more details. | |
16 | * | |
896014f4 DL |
17 | * You should have received a copy of the GNU General Public License along |
18 | * with this program; see the file COPYING; if not, write to the Free Software | |
19 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA | |
718e3744 | 20 | */ |
21 | ||
22 | #include <zebra.h> | |
7a2fbbf0 | 23 | #include <stddef.h> |
718e3744 | 24 | |
25 | #include "stream.h" | |
26 | #include "memory.h" | |
27 | #include "network.h" | |
28 | #include "prefix.h" | |
050c013a | 29 | #include "log.h" |
718e3744 | 30 | |
4a1ab8e4 DL |
31 | DEFINE_MTYPE_STATIC(LIB, STREAM, "Stream") |
32 | DEFINE_MTYPE_STATIC(LIB, STREAM_DATA, "Stream data") | |
33 | DEFINE_MTYPE_STATIC(LIB, STREAM_FIFO, "Stream FIFO") | |
34 | ||
050c013a | 35 | /* Tests whether a position is valid */ |
36 | #define GETP_VALID(S,G) \ | |
37 | ((G) <= (S)->endp) | |
38 | #define PUT_AT_VALID(S,G) GETP_VALID(S,G) | |
39 | #define ENDP_VALID(S,E) \ | |
40 | ((E) <= (S)->size) | |
718e3744 | 41 | |
050c013a | 42 | /* asserting sanity checks. Following must be true before |
43 | * stream functions are called: | |
44 | * | |
45 | * Following must always be true of stream elements | |
46 | * before and after calls to stream functions: | |
47 | * | |
48 | * getp <= endp <= size | |
49 | * | |
50 | * Note that after a stream function is called following may be true: | |
51 | * if (getp == endp) then stream is no longer readable | |
52 | * if (endp == size) then stream is no longer writeable | |
53 | * | |
54 | * It is valid to put to anywhere within the size of the stream, but only | |
55 | * using stream_put..._at() functions. | |
56 | */ | |
57 | #define STREAM_WARN_OFFSETS(S) \ | |
d531050b | 58 | zlog_warn ("&(struct stream): %p, size: %lu, getp: %lu, endp: %lu\n", \ |
6c4f4e6e | 59 | (void *)(S), \ |
050c013a | 60 | (unsigned long) (S)->size, \ |
61 | (unsigned long) (S)->getp, \ | |
62 | (unsigned long) (S)->endp)\ | |
63 | ||
64 | #define STREAM_VERIFY_SANE(S) \ | |
65 | do { \ | |
f0b0df1c | 66 | if ( !(GETP_VALID(S, (S)->getp) && ENDP_VALID(S, (S)->endp)) ) \ |
050c013a | 67 | STREAM_WARN_OFFSETS(S); \ |
68 | assert ( GETP_VALID(S, (S)->getp) ); \ | |
69 | assert ( ENDP_VALID(S, (S)->endp) ); \ | |
70 | } while (0) | |
71 | ||
72 | #define STREAM_BOUND_WARN(S, WHAT) \ | |
73 | do { \ | |
74 | zlog_warn ("%s: Attempt to %s out of bounds", __func__, (WHAT)); \ | |
75 | STREAM_WARN_OFFSETS(S); \ | |
76 | assert (0); \ | |
77 | } while (0) | |
78 | ||
79 | /* XXX: Deprecated macro: do not use */ | |
718e3744 | 80 | #define CHECK_SIZE(S, Z) \ |
050c013a | 81 | do { \ |
82 | if (((S)->endp + (Z)) > (S)->size) \ | |
83 | { \ | |
84 | zlog_warn ("CHECK_SIZE: truncating requested size %lu\n", \ | |
85 | (unsigned long) (Z)); \ | |
86 | STREAM_WARN_OFFSETS(S); \ | |
87 | (Z) = (S)->size - (S)->endp; \ | |
88 | } \ | |
89 | } while (0); | |
718e3744 | 90 | |
91 | /* Make stream buffer. */ | |
92 | struct stream * | |
93 | stream_new (size_t size) | |
94 | { | |
95 | struct stream *s; | |
96 | ||
0e43a2bc | 97 | assert (size > 0); |
98 | ||
4b201d46 | 99 | s = XCALLOC (MTYPE_STREAM, sizeof (struct stream)); |
718e3744 | 100 | |
050c013a | 101 | if (s == NULL) |
102 | return s; | |
103 | ||
4b201d46 | 104 | if ( (s->data = XMALLOC (MTYPE_STREAM_DATA, size)) == NULL) |
105 | { | |
106 | XFREE (MTYPE_STREAM, s); | |
107 | return NULL; | |
108 | } | |
109 | ||
718e3744 | 110 | s->size = size; |
111 | return s; | |
112 | } | |
113 | ||
114 | /* Free it now. */ | |
115 | void | |
116 | stream_free (struct stream *s) | |
117 | { | |
4b201d46 | 118 | if (!s) |
119 | return; | |
120 | ||
121 | XFREE (MTYPE_STREAM_DATA, s->data); | |
718e3744 | 122 | XFREE (MTYPE_STREAM, s); |
123 | } | |
050c013a | 124 | |
125 | struct stream * | |
126 | stream_copy (struct stream *new, struct stream *src) | |
127 | { | |
128 | STREAM_VERIFY_SANE (src); | |
129 | ||
130 | assert (new != NULL); | |
131 | assert (STREAM_SIZE(new) >= src->endp); | |
132 | ||
133 | new->endp = src->endp; | |
134 | new->getp = src->getp; | |
135 | ||
136 | memcpy (new->data, src->data, src->endp); | |
137 | ||
138 | return new; | |
139 | } | |
140 | ||
141 | struct stream * | |
142 | stream_dup (struct stream *s) | |
143 | { | |
144 | struct stream *new; | |
145 | ||
146 | STREAM_VERIFY_SANE (s); | |
147 | ||
148 | if ( (new = stream_new (s->endp)) == NULL) | |
149 | return NULL; | |
150 | ||
151 | return (stream_copy (new, s)); | |
152 | } | |
4b201d46 | 153 | |
8c71e481 PM |
154 | struct stream * |
155 | stream_dupcat (struct stream *s1, struct stream *s2, size_t offset) | |
156 | { | |
157 | struct stream *new; | |
158 | ||
159 | STREAM_VERIFY_SANE (s1); | |
160 | STREAM_VERIFY_SANE (s2); | |
161 | ||
162 | if ( (new = stream_new (s1->endp + s2->endp)) == NULL) | |
163 | return NULL; | |
164 | ||
165 | memcpy (new->data, s1->data, offset); | |
166 | memcpy (new->data + offset, s2->data, s2->endp); | |
167 | memcpy (new->data + offset + s2->endp, s1->data + offset, | |
168 | (s1->endp - offset)); | |
169 | new->endp = s1->endp + s2->endp; | |
170 | return new; | |
171 | } | |
172 | ||
4b201d46 | 173 | size_t |
174 | stream_resize (struct stream *s, size_t newsize) | |
175 | { | |
176 | u_char *newdata; | |
177 | STREAM_VERIFY_SANE (s); | |
178 | ||
179 | newdata = XREALLOC (MTYPE_STREAM_DATA, s->data, newsize); | |
180 | ||
181 | if (newdata == NULL) | |
182 | return s->size; | |
183 | ||
184 | s->data = newdata; | |
185 | s->size = newsize; | |
186 | ||
187 | if (s->endp > s->size) | |
188 | s->endp = s->size; | |
189 | if (s->getp > s->endp) | |
190 | s->getp = s->endp; | |
191 | ||
192 | STREAM_VERIFY_SANE (s); | |
193 | ||
194 | return s->size; | |
195 | } | |
6b0655a2 | 196 | |
f2e6c429 | 197 | size_t |
718e3744 | 198 | stream_get_getp (struct stream *s) |
199 | { | |
050c013a | 200 | STREAM_VERIFY_SANE(s); |
718e3744 | 201 | return s->getp; |
202 | } | |
203 | ||
f2e6c429 | 204 | size_t |
718e3744 | 205 | stream_get_endp (struct stream *s) |
206 | { | |
050c013a | 207 | STREAM_VERIFY_SANE(s); |
718e3744 | 208 | return s->endp; |
209 | } | |
210 | ||
f2e6c429 | 211 | size_t |
718e3744 | 212 | stream_get_size (struct stream *s) |
213 | { | |
050c013a | 214 | STREAM_VERIFY_SANE(s); |
718e3744 | 215 | return s->size; |
216 | } | |
217 | ||
218 | /* Stream structre' stream pointer related functions. */ | |
219 | void | |
f2e6c429 | 220 | stream_set_getp (struct stream *s, size_t pos) |
718e3744 | 221 | { |
050c013a | 222 | STREAM_VERIFY_SANE(s); |
223 | ||
224 | if (!GETP_VALID (s, pos)) | |
225 | { | |
226 | STREAM_BOUND_WARN (s, "set getp"); | |
227 | pos = s->endp; | |
228 | } | |
229 | ||
718e3744 | 230 | s->getp = pos; |
231 | } | |
232 | ||
d531050b SV |
233 | void |
234 | stream_set_endp (struct stream *s, size_t pos) | |
235 | { | |
236 | STREAM_VERIFY_SANE(s); | |
237 | ||
4effc29f | 238 | if (!ENDP_VALID(s, pos)) |
d531050b SV |
239 | { |
240 | STREAM_BOUND_WARN (s, "set endp"); | |
4effc29f AS |
241 | return; |
242 | } | |
243 | ||
244 | /* | |
245 | * Make sure the current read pointer is not beyond the new endp. | |
246 | */ | |
247 | if (s->getp > pos) | |
248 | { | |
249 | STREAM_BOUND_WARN(s, "set endp"); | |
250 | return; | |
d531050b SV |
251 | } |
252 | ||
253 | s->endp = pos; | |
4effc29f | 254 | STREAM_VERIFY_SANE(s); |
d531050b SV |
255 | } |
256 | ||
9985f83c | 257 | /* Forward pointer. */ |
718e3744 | 258 | void |
050c013a | 259 | stream_forward_getp (struct stream *s, size_t size) |
718e3744 | 260 | { |
050c013a | 261 | STREAM_VERIFY_SANE(s); |
262 | ||
263 | if (!GETP_VALID (s, s->getp + size)) | |
264 | { | |
265 | STREAM_BOUND_WARN (s, "seek getp"); | |
266 | return; | |
267 | } | |
268 | ||
9985f83c | 269 | s->getp += size; |
718e3744 | 270 | } |
271 | ||
718e3744 | 272 | void |
050c013a | 273 | stream_forward_endp (struct stream *s, size_t size) |
718e3744 | 274 | { |
050c013a | 275 | STREAM_VERIFY_SANE(s); |
276 | ||
277 | if (!ENDP_VALID (s, s->endp + size)) | |
278 | { | |
279 | STREAM_BOUND_WARN (s, "seek endp"); | |
280 | return; | |
281 | } | |
282 | ||
9985f83c | 283 | s->endp += size; |
718e3744 | 284 | } |
6b0655a2 | 285 | |
718e3744 | 286 | /* Copy from stream to destination. */ |
287 | void | |
288 | stream_get (void *dst, struct stream *s, size_t size) | |
289 | { | |
050c013a | 290 | STREAM_VERIFY_SANE(s); |
291 | ||
292 | if (STREAM_READABLE(s) < size) | |
293 | { | |
294 | STREAM_BOUND_WARN (s, "get"); | |
295 | return; | |
296 | } | |
297 | ||
718e3744 | 298 | memcpy (dst, s->data + s->getp, size); |
299 | s->getp += size; | |
300 | } | |
301 | ||
302 | /* Get next character from the stream. */ | |
303 | u_char | |
304 | stream_getc (struct stream *s) | |
305 | { | |
306 | u_char c; | |
050c013a | 307 | |
308 | STREAM_VERIFY_SANE (s); | |
309 | ||
310 | if (STREAM_READABLE(s) < sizeof (u_char)) | |
311 | { | |
312 | STREAM_BOUND_WARN (s, "get char"); | |
313 | return 0; | |
314 | } | |
315 | c = s->data[s->getp++]; | |
316 | ||
718e3744 | 317 | return c; |
318 | } | |
319 | ||
320 | /* Get next character from the stream. */ | |
321 | u_char | |
f2e6c429 | 322 | stream_getc_from (struct stream *s, size_t from) |
718e3744 | 323 | { |
324 | u_char c; | |
325 | ||
050c013a | 326 | STREAM_VERIFY_SANE(s); |
327 | ||
328 | if (!GETP_VALID (s, from + sizeof (u_char))) | |
329 | { | |
330 | STREAM_BOUND_WARN (s, "get char"); | |
331 | return 0; | |
332 | } | |
333 | ||
718e3744 | 334 | c = s->data[from]; |
050c013a | 335 | |
718e3744 | 336 | return c; |
337 | } | |
338 | ||
339 | /* Get next word from the stream. */ | |
340 | u_int16_t | |
341 | stream_getw (struct stream *s) | |
342 | { | |
343 | u_int16_t w; | |
344 | ||
050c013a | 345 | STREAM_VERIFY_SANE (s); |
346 | ||
347 | if (STREAM_READABLE (s) < sizeof (u_int16_t)) | |
348 | { | |
349 | STREAM_BOUND_WARN (s, "get "); | |
350 | return 0; | |
351 | } | |
352 | ||
718e3744 | 353 | w = s->data[s->getp++] << 8; |
354 | w |= s->data[s->getp++]; | |
050c013a | 355 | |
718e3744 | 356 | return w; |
357 | } | |
358 | ||
359 | /* Get next word from the stream. */ | |
360 | u_int16_t | |
f2e6c429 | 361 | stream_getw_from (struct stream *s, size_t from) |
718e3744 | 362 | { |
363 | u_int16_t w; | |
364 | ||
050c013a | 365 | STREAM_VERIFY_SANE(s); |
366 | ||
367 | if (!GETP_VALID (s, from + sizeof (u_int16_t))) | |
368 | { | |
369 | STREAM_BOUND_WARN (s, "get "); | |
370 | return 0; | |
371 | } | |
372 | ||
718e3744 | 373 | w = s->data[from++] << 8; |
374 | w |= s->data[from]; | |
050c013a | 375 | |
718e3744 | 376 | return w; |
377 | } | |
378 | ||
d6f4a61d DL |
379 | /* Get next 3-byte from the stream. */ |
380 | u_int32_t | |
381 | stream_get3_from (struct stream *s, size_t from) | |
382 | { | |
383 | u_int32_t l; | |
384 | ||
385 | STREAM_VERIFY_SANE(s); | |
386 | ||
387 | if (!GETP_VALID (s, from + 3)) | |
388 | { | |
389 | STREAM_BOUND_WARN (s, "get 3byte"); | |
390 | return 0; | |
391 | } | |
392 | ||
393 | l = s->data[from++] << 16; | |
394 | l |= s->data[from++] << 8; | |
395 | l |= s->data[from]; | |
396 | ||
397 | return l; | |
398 | } | |
399 | ||
400 | u_int32_t | |
401 | stream_get3 (struct stream *s) | |
402 | { | |
403 | u_int32_t l; | |
404 | ||
405 | STREAM_VERIFY_SANE(s); | |
406 | ||
407 | if (STREAM_READABLE (s) < 3) | |
408 | { | |
409 | STREAM_BOUND_WARN (s, "get 3byte"); | |
410 | return 0; | |
411 | } | |
412 | ||
413 | l = s->data[s->getp++] << 16; | |
414 | l |= s->data[s->getp++] << 8; | |
415 | l |= s->data[s->getp++]; | |
416 | ||
417 | return l; | |
418 | } | |
419 | ||
718e3744 | 420 | /* Get next long word from the stream. */ |
050c013a | 421 | u_int32_t |
422 | stream_getl_from (struct stream *s, size_t from) | |
423 | { | |
424 | u_int32_t l; | |
425 | ||
426 | STREAM_VERIFY_SANE(s); | |
427 | ||
428 | if (!GETP_VALID (s, from + sizeof (u_int32_t))) | |
429 | { | |
430 | STREAM_BOUND_WARN (s, "get long"); | |
431 | return 0; | |
432 | } | |
433 | ||
434 | l = s->data[from++] << 24; | |
435 | l |= s->data[from++] << 16; | |
436 | l |= s->data[from++] << 8; | |
437 | l |= s->data[from]; | |
438 | ||
439 | return l; | |
440 | } | |
441 | ||
3f9c7369 DS |
442 | /* Copy from stream at specific location to destination. */ |
443 | void | |
444 | stream_get_from (void *dst, struct stream *s, size_t from, size_t size) | |
445 | { | |
446 | STREAM_VERIFY_SANE(s); | |
447 | ||
448 | if (!GETP_VALID (s, from + size)) | |
449 | { | |
450 | STREAM_BOUND_WARN (s, "get from"); | |
451 | return; | |
452 | } | |
453 | ||
454 | memcpy (dst, s->data + from, size); | |
455 | } | |
456 | ||
718e3744 | 457 | u_int32_t |
458 | stream_getl (struct stream *s) | |
459 | { | |
460 | u_int32_t l; | |
461 | ||
050c013a | 462 | STREAM_VERIFY_SANE(s); |
463 | ||
464 | if (STREAM_READABLE (s) < sizeof (u_int32_t)) | |
465 | { | |
466 | STREAM_BOUND_WARN (s, "get long"); | |
467 | return 0; | |
468 | } | |
469 | ||
718e3744 | 470 | l = s->data[s->getp++] << 24; |
471 | l |= s->data[s->getp++] << 16; | |
472 | l |= s->data[s->getp++] << 8; | |
473 | l |= s->data[s->getp++]; | |
050c013a | 474 | |
718e3744 | 475 | return l; |
476 | } | |
4b201d46 | 477 | |
478 | /* Get next quad word from the stream. */ | |
479 | uint64_t | |
480 | stream_getq_from (struct stream *s, size_t from) | |
481 | { | |
581a02a9 | 482 | uint64_t q; |
4b201d46 | 483 | |
484 | STREAM_VERIFY_SANE(s); | |
485 | ||
486 | if (!GETP_VALID (s, from + sizeof (uint64_t))) | |
487 | { | |
488 | STREAM_BOUND_WARN (s, "get quad"); | |
489 | return 0; | |
490 | } | |
491 | ||
492 | q = ((uint64_t) s->data[from++]) << 56; | |
493 | q |= ((uint64_t) s->data[from++]) << 48; | |
494 | q |= ((uint64_t) s->data[from++]) << 40; | |
495 | q |= ((uint64_t) s->data[from++]) << 32; | |
496 | q |= ((uint64_t) s->data[from++]) << 24; | |
497 | q |= ((uint64_t) s->data[from++]) << 16; | |
498 | q |= ((uint64_t) s->data[from++]) << 8; | |
499 | q |= ((uint64_t) s->data[from++]); | |
500 | ||
501 | return q; | |
502 | } | |
503 | ||
504 | uint64_t | |
505 | stream_getq (struct stream *s) | |
506 | { | |
507 | uint64_t q; | |
508 | ||
509 | STREAM_VERIFY_SANE(s); | |
510 | ||
511 | if (STREAM_READABLE (s) < sizeof (uint64_t)) | |
512 | { | |
513 | STREAM_BOUND_WARN (s, "get quad"); | |
514 | return 0; | |
515 | } | |
516 | ||
517 | q = ((uint64_t) s->data[s->getp++]) << 56; | |
518 | q |= ((uint64_t) s->data[s->getp++]) << 48; | |
519 | q |= ((uint64_t) s->data[s->getp++]) << 40; | |
520 | q |= ((uint64_t) s->data[s->getp++]) << 32; | |
521 | q |= ((uint64_t) s->data[s->getp++]) << 24; | |
522 | q |= ((uint64_t) s->data[s->getp++]) << 16; | |
523 | q |= ((uint64_t) s->data[s->getp++]) << 8; | |
524 | q |= ((uint64_t) s->data[s->getp++]); | |
525 | ||
526 | return q; | |
527 | } | |
528 | ||
718e3744 | 529 | /* Get next long word from the stream. */ |
530 | u_int32_t | |
531 | stream_get_ipv4 (struct stream *s) | |
532 | { | |
533 | u_int32_t l; | |
534 | ||
050c013a | 535 | STREAM_VERIFY_SANE(s); |
536 | ||
537 | if (STREAM_READABLE (s) < sizeof(u_int32_t)) | |
538 | { | |
539 | STREAM_BOUND_WARN (s, "get ipv4"); | |
540 | return 0; | |
541 | } | |
542 | ||
543 | memcpy (&l, s->data + s->getp, sizeof(u_int32_t)); | |
544 | s->getp += sizeof(u_int32_t); | |
718e3744 | 545 | |
546 | return l; | |
547 | } | |
6b0655a2 | 548 | |
16f1b9ee OD |
549 | float |
550 | stream_getf (struct stream *s) | |
551 | { | |
552 | union { | |
553 | float r; | |
554 | uint32_t d; | |
555 | } u; | |
556 | u.d = stream_getl (s); | |
557 | return u.r; | |
558 | } | |
559 | ||
560 | double | |
561 | stream_getd (struct stream *s) | |
562 | { | |
563 | union { | |
564 | double r; | |
565 | uint64_t d; | |
566 | } u; | |
567 | u.d = stream_getq (s); | |
568 | return u.r; | |
569 | } | |
570 | ||
050c013a | 571 | /* Copy to source to stream. |
572 | * | |
573 | * XXX: This uses CHECK_SIZE and hence has funny semantics -> Size will wrap | |
574 | * around. This should be fixed once the stream updates are working. | |
0dab9303 | 575 | * |
576 | * stream_write() is saner | |
050c013a | 577 | */ |
718e3744 | 578 | void |
3d52bb80 | 579 | stream_put (struct stream *s, const void *src, size_t size) |
718e3744 | 580 | { |
581 | ||
050c013a | 582 | /* XXX: CHECK_SIZE has strange semantics. It should be deprecated */ |
718e3744 | 583 | CHECK_SIZE(s, size); |
050c013a | 584 | |
585 | STREAM_VERIFY_SANE(s); | |
586 | ||
587 | if (STREAM_WRITEABLE (s) < size) | |
588 | { | |
589 | STREAM_BOUND_WARN (s, "put"); | |
590 | return; | |
591 | } | |
592 | ||
718e3744 | 593 | if (src) |
9985f83c | 594 | memcpy (s->data + s->endp, src, size); |
718e3744 | 595 | else |
9985f83c | 596 | memset (s->data + s->endp, 0, size); |
718e3744 | 597 | |
9985f83c | 598 | s->endp += size; |
718e3744 | 599 | } |
600 | ||
601 | /* Put character to the stream. */ | |
602 | int | |
603 | stream_putc (struct stream *s, u_char c) | |
604 | { | |
050c013a | 605 | STREAM_VERIFY_SANE(s); |
606 | ||
607 | if (STREAM_WRITEABLE (s) < sizeof(u_char)) | |
608 | { | |
609 | STREAM_BOUND_WARN (s, "put"); | |
610 | return 0; | |
611 | } | |
612 | ||
613 | s->data[s->endp++] = c; | |
614 | return sizeof (u_char); | |
718e3744 | 615 | } |
616 | ||
617 | /* Put word to the stream. */ | |
618 | int | |
619 | stream_putw (struct stream *s, u_int16_t w) | |
620 | { | |
050c013a | 621 | STREAM_VERIFY_SANE (s); |
718e3744 | 622 | |
050c013a | 623 | if (STREAM_WRITEABLE (s) < sizeof (u_int16_t)) |
624 | { | |
625 | STREAM_BOUND_WARN (s, "put"); | |
626 | return 0; | |
627 | } | |
628 | ||
9985f83c | 629 | s->data[s->endp++] = (u_char)(w >> 8); |
630 | s->data[s->endp++] = (u_char) w; | |
718e3744 | 631 | |
718e3744 | 632 | return 2; |
633 | } | |
634 | ||
d6f4a61d DL |
635 | /* Put long word to the stream. */ |
636 | int | |
637 | stream_put3 (struct stream *s, u_int32_t l) | |
638 | { | |
639 | STREAM_VERIFY_SANE (s); | |
640 | ||
641 | if (STREAM_WRITEABLE (s) < 3) | |
642 | { | |
643 | STREAM_BOUND_WARN (s, "put"); | |
644 | return 0; | |
645 | } | |
646 | ||
647 | s->data[s->endp++] = (u_char)(l >> 16); | |
648 | s->data[s->endp++] = (u_char)(l >> 8); | |
649 | s->data[s->endp++] = (u_char)l; | |
650 | ||
651 | return 3; | |
652 | } | |
653 | ||
718e3744 | 654 | /* Put long word to the stream. */ |
655 | int | |
656 | stream_putl (struct stream *s, u_int32_t l) | |
657 | { | |
050c013a | 658 | STREAM_VERIFY_SANE (s); |
718e3744 | 659 | |
050c013a | 660 | if (STREAM_WRITEABLE (s) < sizeof (u_int32_t)) |
661 | { | |
662 | STREAM_BOUND_WARN (s, "put"); | |
663 | return 0; | |
664 | } | |
665 | ||
9985f83c | 666 | s->data[s->endp++] = (u_char)(l >> 24); |
667 | s->data[s->endp++] = (u_char)(l >> 16); | |
668 | s->data[s->endp++] = (u_char)(l >> 8); | |
669 | s->data[s->endp++] = (u_char)l; | |
718e3744 | 670 | |
718e3744 | 671 | return 4; |
672 | } | |
673 | ||
4b201d46 | 674 | /* Put quad word to the stream. */ |
675 | int | |
676 | stream_putq (struct stream *s, uint64_t q) | |
677 | { | |
678 | STREAM_VERIFY_SANE (s); | |
679 | ||
680 | if (STREAM_WRITEABLE (s) < sizeof (uint64_t)) | |
681 | { | |
682 | STREAM_BOUND_WARN (s, "put quad"); | |
683 | return 0; | |
684 | } | |
685 | ||
686 | s->data[s->endp++] = (u_char)(q >> 56); | |
687 | s->data[s->endp++] = (u_char)(q >> 48); | |
688 | s->data[s->endp++] = (u_char)(q >> 40); | |
689 | s->data[s->endp++] = (u_char)(q >> 32); | |
690 | s->data[s->endp++] = (u_char)(q >> 24); | |
691 | s->data[s->endp++] = (u_char)(q >> 16); | |
692 | s->data[s->endp++] = (u_char)(q >> 8); | |
693 | s->data[s->endp++] = (u_char)q; | |
694 | ||
695 | return 8; | |
696 | } | |
697 | ||
16f1b9ee OD |
698 | int |
699 | stream_putf (struct stream *s, float f) | |
700 | { | |
701 | union { | |
702 | float i; | |
703 | uint32_t o; | |
704 | } u; | |
705 | u.i = f; | |
706 | return stream_putl (s, u.o); | |
707 | } | |
708 | ||
709 | int | |
710 | stream_putd (struct stream *s, double d) | |
711 | { | |
712 | union { | |
713 | double i; | |
714 | uint64_t o; | |
715 | } u; | |
716 | u.i = d; | |
717 | return stream_putq (s, u.o); | |
718 | } | |
719 | ||
718e3744 | 720 | int |
f2e6c429 | 721 | stream_putc_at (struct stream *s, size_t putp, u_char c) |
718e3744 | 722 | { |
050c013a | 723 | STREAM_VERIFY_SANE(s); |
724 | ||
725 | if (!PUT_AT_VALID (s, putp + sizeof (u_char))) | |
726 | { | |
727 | STREAM_BOUND_WARN (s, "put"); | |
728 | return 0; | |
729 | } | |
730 | ||
718e3744 | 731 | s->data[putp] = c; |
050c013a | 732 | |
718e3744 | 733 | return 1; |
734 | } | |
735 | ||
736 | int | |
f2e6c429 | 737 | stream_putw_at (struct stream *s, size_t putp, u_int16_t w) |
718e3744 | 738 | { |
050c013a | 739 | STREAM_VERIFY_SANE(s); |
740 | ||
741 | if (!PUT_AT_VALID (s, putp + sizeof (u_int16_t))) | |
742 | { | |
743 | STREAM_BOUND_WARN (s, "put"); | |
744 | return 0; | |
745 | } | |
746 | ||
718e3744 | 747 | s->data[putp] = (u_char)(w >> 8); |
748 | s->data[putp + 1] = (u_char) w; | |
050c013a | 749 | |
718e3744 | 750 | return 2; |
751 | } | |
752 | ||
d6f4a61d DL |
753 | int |
754 | stream_put3_at (struct stream *s, size_t putp, u_int32_t l) | |
755 | { | |
756 | STREAM_VERIFY_SANE(s); | |
757 | ||
758 | if (!PUT_AT_VALID (s, putp + 3)) | |
759 | { | |
760 | STREAM_BOUND_WARN (s, "put"); | |
761 | return 0; | |
762 | } | |
763 | s->data[putp] = (u_char)(l >> 16); | |
764 | s->data[putp + 1] = (u_char)(l >> 8); | |
765 | s->data[putp + 2] = (u_char)l; | |
766 | ||
767 | return 3; | |
768 | } | |
769 | ||
718e3744 | 770 | int |
f2e6c429 | 771 | stream_putl_at (struct stream *s, size_t putp, u_int32_t l) |
718e3744 | 772 | { |
050c013a | 773 | STREAM_VERIFY_SANE(s); |
774 | ||
775 | if (!PUT_AT_VALID (s, putp + sizeof (u_int32_t))) | |
776 | { | |
777 | STREAM_BOUND_WARN (s, "put"); | |
778 | return 0; | |
779 | } | |
718e3744 | 780 | s->data[putp] = (u_char)(l >> 24); |
781 | s->data[putp + 1] = (u_char)(l >> 16); | |
782 | s->data[putp + 2] = (u_char)(l >> 8); | |
783 | s->data[putp + 3] = (u_char)l; | |
050c013a | 784 | |
718e3744 | 785 | return 4; |
786 | } | |
787 | ||
4b201d46 | 788 | int |
789 | stream_putq_at (struct stream *s, size_t putp, uint64_t q) | |
790 | { | |
791 | STREAM_VERIFY_SANE(s); | |
792 | ||
793 | if (!PUT_AT_VALID (s, putp + sizeof (uint64_t))) | |
794 | { | |
795 | STREAM_BOUND_WARN (s, "put"); | |
796 | return 0; | |
797 | } | |
798 | s->data[putp] = (u_char)(q >> 56); | |
799 | s->data[putp + 1] = (u_char)(q >> 48); | |
800 | s->data[putp + 2] = (u_char)(q >> 40); | |
801 | s->data[putp + 3] = (u_char)(q >> 32); | |
802 | s->data[putp + 4] = (u_char)(q >> 24); | |
803 | s->data[putp + 5] = (u_char)(q >> 16); | |
804 | s->data[putp + 6] = (u_char)(q >> 8); | |
805 | s->data[putp + 7] = (u_char)q; | |
806 | ||
807 | return 8; | |
808 | } | |
809 | ||
718e3744 | 810 | /* Put long word to the stream. */ |
811 | int | |
812 | stream_put_ipv4 (struct stream *s, u_int32_t l) | |
813 | { | |
050c013a | 814 | STREAM_VERIFY_SANE(s); |
815 | ||
816 | if (STREAM_WRITEABLE (s) < sizeof (u_int32_t)) | |
817 | { | |
818 | STREAM_BOUND_WARN (s, "put"); | |
819 | return 0; | |
820 | } | |
821 | memcpy (s->data + s->endp, &l, sizeof (u_int32_t)); | |
822 | s->endp += sizeof (u_int32_t); | |
718e3744 | 823 | |
050c013a | 824 | return sizeof (u_int32_t); |
718e3744 | 825 | } |
826 | ||
827 | /* Put long word to the stream. */ | |
828 | int | |
829 | stream_put_in_addr (struct stream *s, struct in_addr *addr) | |
830 | { | |
050c013a | 831 | STREAM_VERIFY_SANE(s); |
832 | ||
833 | if (STREAM_WRITEABLE (s) < sizeof (u_int32_t)) | |
834 | { | |
835 | STREAM_BOUND_WARN (s, "put"); | |
836 | return 0; | |
837 | } | |
718e3744 | 838 | |
050c013a | 839 | memcpy (s->data + s->endp, addr, sizeof (u_int32_t)); |
840 | s->endp += sizeof (u_int32_t); | |
718e3744 | 841 | |
050c013a | 842 | return sizeof (u_int32_t); |
718e3744 | 843 | } |
844 | ||
3f9c7369 DS |
845 | /* Put in_addr at location in the stream. */ |
846 | int | |
847 | stream_put_in_addr_at (struct stream *s, size_t putp, struct in_addr *addr) | |
848 | { | |
849 | STREAM_VERIFY_SANE(s); | |
850 | ||
851 | if (!PUT_AT_VALID (s, putp + 4)) | |
852 | { | |
853 | STREAM_BOUND_WARN (s, "put"); | |
854 | return 0; | |
855 | } | |
856 | ||
857 | memcpy (&s->data[putp], addr, 4); | |
858 | return 4; | |
859 | } | |
860 | ||
861 | /* Put in6_addr at location in the stream. */ | |
862 | int | |
863 | stream_put_in6_addr_at (struct stream *s, size_t putp, struct in6_addr *addr) | |
864 | { | |
865 | STREAM_VERIFY_SANE(s); | |
866 | ||
867 | if (!PUT_AT_VALID (s, putp + 16)) | |
868 | { | |
869 | STREAM_BOUND_WARN (s, "put"); | |
870 | return 0; | |
871 | } | |
872 | ||
873 | memcpy (&s->data[putp], addr, 16); | |
874 | return 16; | |
875 | } | |
876 | ||
718e3744 | 877 | /* Put prefix by nlri type format. */ |
878 | int | |
adbac85e DW |
879 | stream_put_prefix_addpath (struct stream *s, struct prefix *p, |
880 | int addpath_encode, u_int32_t addpath_tx_id) | |
718e3744 | 881 | { |
050c013a | 882 | size_t psize; |
adbac85e | 883 | size_t psize_with_addpath; |
050c013a | 884 | |
885 | STREAM_VERIFY_SANE(s); | |
886 | ||
718e3744 | 887 | psize = PSIZE (p->prefixlen); |
adbac85e DW |
888 | |
889 | if (addpath_encode) | |
890 | psize_with_addpath = psize + 4; | |
891 | else | |
892 | psize_with_addpath = psize; | |
050c013a | 893 | |
adbac85e | 894 | if (STREAM_WRITEABLE (s) < (psize_with_addpath + sizeof (u_char))) |
050c013a | 895 | { |
896 | STREAM_BOUND_WARN (s, "put"); | |
897 | return 0; | |
898 | } | |
899 | ||
adbac85e DW |
900 | if (addpath_encode) |
901 | { | |
902 | s->data[s->endp++] = (u_char)(addpath_tx_id >> 24); | |
903 | s->data[s->endp++] = (u_char)(addpath_tx_id >> 16); | |
904 | s->data[s->endp++] = (u_char)(addpath_tx_id >> 8); | |
905 | s->data[s->endp++] = (u_char)addpath_tx_id; | |
906 | } | |
907 | ||
af514777 | 908 | s->data[s->endp++] = p->prefixlen; |
9985f83c | 909 | memcpy (s->data + s->endp, &p->u.prefix, psize); |
910 | s->endp += psize; | |
718e3744 | 911 | |
718e3744 | 912 | return psize; |
913 | } | |
6b0655a2 | 914 | |
adbac85e DW |
915 | int |
916 | stream_put_prefix (struct stream *s, struct prefix *p) | |
917 | { | |
918 | return stream_put_prefix_addpath (s, p, 0, 0); | |
919 | } | |
920 | ||
cd1964ff DS |
921 | /* Put NLRI with label */ |
922 | int | |
923 | stream_put_labeled_prefix (struct stream *s, struct prefix *p, u_char *label) | |
924 | { | |
925 | size_t psize; | |
926 | ||
927 | STREAM_VERIFY_SANE(s); | |
928 | ||
929 | psize = PSIZE (p->prefixlen); | |
930 | ||
931 | if (STREAM_WRITEABLE (s) < (psize + 3)) | |
932 | { | |
933 | STREAM_BOUND_WARN (s, "put"); | |
934 | return 0; | |
935 | } | |
936 | ||
937 | stream_putc (s, (p->prefixlen + 24)); | |
938 | stream_putc(s, label[0]); | |
939 | stream_putc(s, label[1]); | |
940 | stream_putc(s, label[2]); | |
941 | memcpy (s->data + s->endp, &p->u.prefix, psize); | |
942 | s->endp += psize; | |
943 | ||
944 | return (psize + 3); | |
945 | } | |
adbac85e | 946 | |
718e3744 | 947 | /* Read size from fd. */ |
948 | int | |
949 | stream_read (struct stream *s, int fd, size_t size) | |
950 | { | |
951 | int nbytes; | |
952 | ||
050c013a | 953 | STREAM_VERIFY_SANE(s); |
954 | ||
955 | if (STREAM_WRITEABLE (s) < size) | |
956 | { | |
957 | STREAM_BOUND_WARN (s, "put"); | |
958 | return 0; | |
959 | } | |
960 | ||
9985f83c | 961 | nbytes = readn (fd, s->data + s->endp, size); |
718e3744 | 962 | |
963 | if (nbytes > 0) | |
9985f83c | 964 | s->endp += nbytes; |
965 | ||
718e3744 | 966 | return nbytes; |
967 | } | |
968 | ||
262feb1a | 969 | ssize_t |
970 | stream_read_try(struct stream *s, int fd, size_t size) | |
971 | { | |
972 | ssize_t nbytes; | |
973 | ||
974 | STREAM_VERIFY_SANE(s); | |
975 | ||
976 | if (STREAM_WRITEABLE(s) < size) | |
977 | { | |
978 | STREAM_BOUND_WARN (s, "put"); | |
979 | /* Fatal (not transient) error, since retrying will not help | |
980 | (stream is too small to contain the desired data). */ | |
981 | return -1; | |
982 | } | |
983 | ||
984 | if ((nbytes = read(fd, s->data + s->endp, size)) >= 0) | |
985 | { | |
986 | s->endp += nbytes; | |
987 | return nbytes; | |
988 | } | |
989 | /* Error: was it transient (return -2) or fatal (return -1)? */ | |
81fb3240 | 990 | if (ERRNO_IO_RETRY(errno)) |
991 | return -2; | |
992 | zlog_warn("%s: read failed on fd %d: %s", __func__, fd, safe_strerror(errno)); | |
993 | return -1; | |
262feb1a | 994 | } |
995 | ||
0dab9303 | 996 | /* Read up to size bytes into the stream from the fd, using recvmsgfrom |
997 | * whose arguments match the remaining arguments to this function | |
998 | */ | |
999 | ssize_t | |
1000 | stream_recvfrom (struct stream *s, int fd, size_t size, int flags, | |
1001 | struct sockaddr *from, socklen_t *fromlen) | |
1002 | { | |
1003 | ssize_t nbytes; | |
1004 | ||
1005 | STREAM_VERIFY_SANE(s); | |
1006 | ||
1007 | if (STREAM_WRITEABLE(s) < size) | |
1008 | { | |
1009 | STREAM_BOUND_WARN (s, "put"); | |
1010 | /* Fatal (not transient) error, since retrying will not help | |
1011 | (stream is too small to contain the desired data). */ | |
1012 | return -1; | |
1013 | } | |
1014 | ||
1015 | if ((nbytes = recvfrom (fd, s->data + s->endp, size, | |
1016 | flags, from, fromlen)) >= 0) | |
1017 | { | |
1018 | s->endp += nbytes; | |
1019 | return nbytes; | |
1020 | } | |
1021 | /* Error: was it transient (return -2) or fatal (return -1)? */ | |
1022 | if (ERRNO_IO_RETRY(errno)) | |
1023 | return -2; | |
1024 | zlog_warn("%s: read failed on fd %d: %s", __func__, fd, safe_strerror(errno)); | |
1025 | return -1; | |
1026 | } | |
1027 | ||
050c013a | 1028 | /* Read up to smaller of size or SIZE_REMAIN() bytes to the stream, starting |
1029 | * from endp. | |
1030 | * First iovec will be used to receive the data. | |
1031 | * Stream need not be empty. | |
1032 | */ | |
0dab9303 | 1033 | ssize_t |
050c013a | 1034 | stream_recvmsg (struct stream *s, int fd, struct msghdr *msgh, int flags, |
1035 | size_t size) | |
1036 | { | |
1037 | int nbytes; | |
1038 | struct iovec *iov; | |
1039 | ||
1040 | STREAM_VERIFY_SANE(s); | |
1041 | assert (msgh->msg_iovlen > 0); | |
1042 | ||
1043 | if (STREAM_WRITEABLE (s) < size) | |
1044 | { | |
1045 | STREAM_BOUND_WARN (s, "put"); | |
262feb1a | 1046 | /* This is a logic error in the calling code: the stream is too small |
1047 | to hold the desired data! */ | |
1048 | return -1; | |
050c013a | 1049 | } |
1050 | ||
1051 | iov = &(msgh->msg_iov[0]); | |
1052 | iov->iov_base = (s->data + s->endp); | |
1053 | iov->iov_len = size; | |
1054 | ||
1055 | nbytes = recvmsg (fd, msgh, flags); | |
1056 | ||
1057 | if (nbytes > 0) | |
1058 | s->endp += nbytes; | |
1059 | ||
1060 | return nbytes; | |
1061 | } | |
1062 | ||
718e3744 | 1063 | /* Write data to buffer. */ |
0dab9303 | 1064 | size_t |
3d52bb80 | 1065 | stream_write (struct stream *s, const void *ptr, size_t size) |
718e3744 | 1066 | { |
1067 | ||
1068 | CHECK_SIZE(s, size); | |
1069 | ||
050c013a | 1070 | STREAM_VERIFY_SANE(s); |
1071 | ||
1072 | if (STREAM_WRITEABLE (s) < size) | |
1073 | { | |
1074 | STREAM_BOUND_WARN (s, "put"); | |
1075 | return 0; | |
1076 | } | |
1077 | ||
9985f83c | 1078 | memcpy (s->data + s->endp, ptr, size); |
1079 | s->endp += size; | |
1080 | ||
718e3744 | 1081 | return size; |
1082 | } | |
1083 | ||
050c013a | 1084 | /* Return current read pointer. |
1085 | * DEPRECATED! | |
1086 | * Use stream_get_pnt_to if you must, but decoding streams properly | |
1087 | * is preferred | |
1088 | */ | |
718e3744 | 1089 | u_char * |
1090 | stream_pnt (struct stream *s) | |
1091 | { | |
050c013a | 1092 | STREAM_VERIFY_SANE(s); |
718e3744 | 1093 | return s->data + s->getp; |
1094 | } | |
1095 | ||
1096 | /* Check does this stream empty? */ | |
1097 | int | |
1098 | stream_empty (struct stream *s) | |
1099 | { | |
050c013a | 1100 | STREAM_VERIFY_SANE(s); |
1101 | ||
1102 | return (s->endp == 0); | |
718e3744 | 1103 | } |
1104 | ||
1105 | /* Reset stream. */ | |
1106 | void | |
1107 | stream_reset (struct stream *s) | |
1108 | { | |
050c013a | 1109 | STREAM_VERIFY_SANE (s); |
1110 | ||
1111 | s->getp = s->endp = 0; | |
718e3744 | 1112 | } |
1113 | ||
1114 | /* Write stream contens to the file discriptor. */ | |
1115 | int | |
1116 | stream_flush (struct stream *s, int fd) | |
1117 | { | |
1118 | int nbytes; | |
050c013a | 1119 | |
1120 | STREAM_VERIFY_SANE(s); | |
1121 | ||
718e3744 | 1122 | nbytes = write (fd, s->data + s->getp, s->endp - s->getp); |
050c013a | 1123 | |
718e3744 | 1124 | return nbytes; |
1125 | } | |
6b0655a2 | 1126 | |
718e3744 | 1127 | /* Stream first in first out queue. */ |
1128 | ||
1129 | struct stream_fifo * | |
81fb3240 | 1130 | stream_fifo_new (void) |
718e3744 | 1131 | { |
1132 | struct stream_fifo *new; | |
1133 | ||
1134 | new = XCALLOC (MTYPE_STREAM_FIFO, sizeof (struct stream_fifo)); | |
1135 | return new; | |
1136 | } | |
1137 | ||
1138 | /* Add new stream to fifo. */ | |
1139 | void | |
1140 | stream_fifo_push (struct stream_fifo *fifo, struct stream *s) | |
1141 | { | |
1142 | if (fifo->tail) | |
1143 | fifo->tail->next = s; | |
1144 | else | |
1145 | fifo->head = s; | |
1146 | ||
1147 | fifo->tail = s; | |
1148 | ||
1149 | fifo->count++; | |
1150 | } | |
1151 | ||
1152 | /* Delete first stream from fifo. */ | |
1153 | struct stream * | |
1154 | stream_fifo_pop (struct stream_fifo *fifo) | |
1155 | { | |
1156 | struct stream *s; | |
1157 | ||
1158 | s = fifo->head; | |
1159 | ||
1160 | if (s) | |
1161 | { | |
1162 | fifo->head = s->next; | |
1163 | ||
1164 | if (fifo->head == NULL) | |
1165 | fifo->tail = NULL; | |
718e3744 | 1166 | |
d531050b SV |
1167 | fifo->count--; |
1168 | } | |
718e3744 | 1169 | |
1170 | return s; | |
1171 | } | |
1172 | ||
1173 | /* Return first fifo entry. */ | |
1174 | struct stream * | |
1175 | stream_fifo_head (struct stream_fifo *fifo) | |
1176 | { | |
1177 | return fifo->head; | |
1178 | } | |
1179 | ||
1180 | void | |
1181 | stream_fifo_clean (struct stream_fifo *fifo) | |
1182 | { | |
1183 | struct stream *s; | |
1184 | struct stream *next; | |
1185 | ||
1186 | for (s = fifo->head; s; s = next) | |
1187 | { | |
1188 | next = s->next; | |
1189 | stream_free (s); | |
1190 | } | |
1191 | fifo->head = fifo->tail = NULL; | |
1192 | fifo->count = 0; | |
1193 | } | |
1194 | ||
1195 | void | |
1196 | stream_fifo_free (struct stream_fifo *fifo) | |
1197 | { | |
1198 | stream_fifo_clean (fifo); | |
1199 | XFREE (MTYPE_STREAM_FIFO, fifo); | |
1200 | } |