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