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