lib/stream.c

   1 /*
   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 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
  20  */
  21
  22 #include <zebra.h>
  23 #include <stddef.h>
  24
  25 #include "stream.h"
  26 #include "memory.h"
  27 #include "network.h"
  28 #include "prefix.h"
  29 #include "log.h"
  30
  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
  35 /* Tests whether a position is valid */
  36 #define GETP_VALID(S, G) ((G) <= (S)->endp)
  37 #define PUT_AT_VALID(S,G) GETP_VALID(S,G)
  38 #define ENDP_VALID(S, E) ((E) <= (S)->size)
  39
  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  */
  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)
  75
  76 /* XXX: Deprecated macro: do not use */
  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);
  87
  88 /* Make stream buffer. */
  89 struct stream *stream_new(size_t size)
  90 {
  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;
 107 }
 108
 109 /* Free it now. */
 110 void stream_free(struct stream *s)
 111 {
 112         if (!s)
 113                 return;
 114
 115         XFREE(MTYPE_STREAM_DATA, s->data);
 116         XFREE(MTYPE_STREAM, s);
 117 }
 118
 119 struct stream *stream_copy(struct stream *new, struct stream *src)
 120 {
 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;
 132 }
 133
 134 struct stream *stream_dup(struct stream *s)
 135 {
 136         struct stream *new;
 137
 138         STREAM_VERIFY_SANE(s);
 139
 140         if ((new = stream_new(s->endp)) == NULL)
 141                 return NULL;
 142
 143         return (stream_copy(new, s));
 144 }
 145
 146 struct stream *stream_dupcat(struct stream *s1, struct stream *s2,
 147                              size_t offset)
 148 {
 149         struct stream *new;
 150
 151         STREAM_VERIFY_SANE(s1);
 152         STREAM_VERIFY_SANE(s2);
 153
 154         if ((new = stream_new(s1->endp + s2->endp)) == NULL)
 155                 return NULL;
 156
 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;
 163 }
 164
 165 size_t stream_resize(struct stream *s, size_t newsize)
 166 {
 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;
 186 }
 187
 188 size_t stream_get_getp(struct stream *s)
 189 {
 190         STREAM_VERIFY_SANE(s);
 191         return s->getp;
 192 }
 193
 194 size_t stream_get_endp(struct stream *s)
 195 {
 196         STREAM_VERIFY_SANE(s);
 197         return s->endp;
 198 }
 199
 200 size_t stream_get_size(struct stream *s)
 201 {
 202         STREAM_VERIFY_SANE(s);
 203         return s->size;
 204 }
 205
 206 /* Stream structre' stream pointer related functions.  */
 207 void stream_set_getp(struct stream *s, size_t pos)
 208 {
 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;
 217 }
 218
 219 void stream_set_endp(struct stream *s, size_t pos)
 220 {
 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);
 238 }
 239
 240 /* Forward pointer. */
 241 void stream_forward_getp(struct stream *s, size_t size)
 242 {
 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;
 251 }
 252
 253 void stream_forward_endp(struct stream *s, size_t size)
 254 {
 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;
 263 }
 264
 265 /* Copy from stream to destination. */
 266 void stream_get(void *dst, struct stream *s, size_t size)
 267 {
 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;
 277 }
 278
 279 /* Get next character from the stream. */
 280 u_char stream_getc(struct stream *s)
 281 {
 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;
 293 }
 294
 295 /* Get next character from the stream. */
 296 u_char stream_getc_from(struct stream *s, size_t from)
 297 {
 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;
 310 }
 311
 312 /* Get next word from the stream. */
 313 u_int16_t stream_getw(struct stream *s)
 314 {
 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;
 328 }
 329
 330 /* Get next word from the stream. */
 331 u_int16_t stream_getw_from(struct stream *s, size_t from)
 332 {
 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;
 346 }
 347
 348 /* Get next 3-byte from the stream. */
 349 u_int32_t stream_get3_from(struct stream *s, size_t from)
 350 {
 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;
 365 }
 366
 367 u_int32_t stream_get3(struct stream *s)
 368 {
 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;
 383 }
 384
 385 /* Get next long word from the stream. */
 386 u_int32_t stream_getl_from(struct stream *s, size_t from)
 387 {
 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 = (unsigned)(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;
 403 }
 404
 405 /* Copy from stream at specific location to destination. */
 406 void stream_get_from(void *dst, struct stream *s, size_t from, size_t size)
 407 {
 408         STREAM_VERIFY_SANE(s);
 409
 410         if (!GETP_VALID(s, from + size)) {
 411                 STREAM_BOUND_WARN(s, "get from");
 412                 return;
 413         }
 414
 415         memcpy(dst, s->data + from, size);
 416 }
 417
 418 u_int32_t stream_getl(struct stream *s)
 419 {
 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 = (unsigned)(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;
 435 }
 436
 437 /* Get next quad word from the stream. */
 438 uint64_t stream_getq_from(struct stream *s, size_t from)
 439 {
 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;
 459 }
 460
 461 uint64_t 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                 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;
 482 }
 483
 484 /* Get next long word from the stream. */
 485 u_int32_t stream_get_ipv4(struct stream *s)
 486 {
 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;
 500 }
 501
 502 float stream_getf(struct stream *s)
 503 {
 504         union {
 505                 float r;
 506                 uint32_t d;
 507         } u;
 508         u.d = stream_getl(s);
 509         return u.r;
 510 }
 511
 512 double stream_getd(struct stream *s)
 513 {
 514         union {
 515                 double r;
 516                 uint64_t d;
 517         } u;
 518         u.d = stream_getq(s);
 519         return u.r;
 520 }
 521
 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.
 526  *
 527  * stream_write() is saner
 528  */
 529 void stream_put(struct stream *s, const void *src, size_t size)
 530 {
 531
 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;
 548 }
 549
 550 /* Put character to the stream. */
 551 int stream_putc(struct stream *s, u_char c)
 552 {
 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);
 562 }
 563
 564 /* Put word to the stream. */
 565 int stream_putw(struct stream *s, u_int16_t w)
 566 {
 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;
 578 }
 579
 580 /* Put long word to the stream. */
 581 int stream_put3(struct stream *s, u_int32_t l)
 582 {
 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;
 595 }
 596
 597 /* Put long word to the stream. */
 598 int stream_putl(struct stream *s, u_int32_t l)
 599 {
 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;
 613 }
 614
 615 /* Put quad word to the stream. */
 616 int stream_putq(struct stream *s, uint64_t q)
 617 {
 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;
 635 }
 636
 637 int stream_putf(struct stream *s, float f)
 638 {
 639         union {
 640                 float i;
 641                 uint32_t o;
 642         } u;
 643         u.i = f;
 644         return stream_putl(s, u.o);
 645 }
 646
 647 int stream_putd(struct stream *s, double d)
 648 {
 649         union {
 650                 double i;
 651                 uint64_t o;
 652         } u;
 653         u.i = d;
 654         return stream_putq(s, u.o);
 655 }
 656
 657 int stream_putc_at(struct stream *s, size_t putp, u_char c)
 658 {
 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;
 669 }
 670
 671 int stream_putw_at(struct stream *s, size_t putp, u_int16_t w)
 672 {
 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;
 684 }
 685
 686 int stream_put3_at(struct stream *s, size_t putp, u_int32_t l)
 687 {
 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;
 699 }
 700
 701 int stream_putl_at(struct stream *s, size_t putp, u_int32_t l)
 702 {
 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;
 715 }
 716
 717 int stream_putq_at(struct stream *s, size_t putp, uint64_t q)
 718 {
 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;
 735 }
 736
 737 /* Put long word to the stream. */
 738 int stream_put_ipv4(struct stream *s, u_int32_t l)
 739 {
 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);
 750 }
 751
 752 /* Put long word to the stream. */
 753 int stream_put_in_addr(struct stream *s, struct in_addr *addr)
 754 {
 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);
 766 }
 767
 768 /* Put in_addr at location in the stream. */
 769 int stream_put_in_addr_at(struct stream *s, size_t putp, struct in_addr *addr)
 770 {
 771         STREAM_VERIFY_SANE(s);
 772
 773         if (!PUT_AT_VALID(s, putp + 4)) {
 774                 STREAM_BOUND_WARN(s, "put");
 775                 return 0;
 776         }
 777
 778         memcpy(&s->data[putp], addr, 4);
 779         return 4;
 780 }
 781
 782 /* Put in6_addr at location in the stream. */
 783 int stream_put_in6_addr_at(struct stream *s, size_t putp, struct in6_addr *addr)
 784 {
 785         STREAM_VERIFY_SANE(s);
 786
 787         if (!PUT_AT_VALID(s, putp + 16)) {
 788                 STREAM_BOUND_WARN(s, "put");
 789                 return 0;
 790         }
 791
 792         memcpy(&s->data[putp], addr, 16);
 793         return 16;
 794 }
 795
 796 /* Put prefix by nlri type format. */
 797 int stream_put_prefix_addpath(struct stream *s, struct prefix *p,
 798                               int addpath_encode, u_int32_t addpath_tx_id)
 799 {
 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;
 829 }
 830
 831 int stream_put_prefix(struct stream *s, struct prefix *p)
 832 {
 833         return stream_put_prefix_addpath(s, p, 0, 0);
 834 }
 835
 836 /* Put NLRI with label */
 837 int stream_put_labeled_prefix(struct stream *s, struct prefix *p,
 838                               mpls_label_t *label)
 839 {
 840         size_t psize;
 841         u_char *label_pnt = (u_char *)label;
 842
 843         STREAM_VERIFY_SANE(s);
 844
 845         psize = PSIZE(p->prefixlen);
 846
 847         if (STREAM_WRITEABLE(s) < (psize + 3)) {
 848                 STREAM_BOUND_WARN(s, "put");
 849                 return 0;
 850         }
 851
 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;
 858
 859         return (psize + 3);
 860 }
 861
 862 /* Read size from fd. */
 863 int stream_read(struct stream *s, int fd, size_t size)
 864 {
 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;
 880 }
 881
 882 ssize_t stream_read_try(struct stream *s, int fd, size_t size)
 883 {
 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;
 905 }
 906
 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  */
 910 ssize_t stream_recvfrom(struct stream *s, int fd, size_t size, int flags,
 911                         struct sockaddr *from, socklen_t *fromlen)
 912 {
 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;
 936 }
 937
 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  */
 943 ssize_t stream_recvmsg(struct stream *s, int fd, struct msghdr *msgh, int flags,
 944                        size_t size)
 945 {
 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;
 970 }
 971
 972 /* Write data to buffer. */
 973 size_t stream_write(struct stream *s, const void *ptr, size_t size)
 974 {
 975
 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         }
 984
 985         memcpy(s->data + s->endp, ptr, size);
 986         s->endp += size;
 987
 988         return size;
 989 }
 990
 991 /* Return current read pointer.
 992  * DEPRECATED!
 993  * Use stream_get_pnt_to if you must, but decoding streams properly
 994  * is preferred
 995  */
 996 u_char *stream_pnt(struct stream *s)
 997 {
 998         STREAM_VERIFY_SANE(s);
 999         return s->data + s->getp;
1000 }
1001
1002 /* Check does this stream empty? */
1003 int stream_empty(struct stream *s)
1004 {
1005         STREAM_VERIFY_SANE(s);
1006
1007         return (s->endp == 0);
1008 }
1009
1010 /* Reset stream. */
1011 void stream_reset(struct stream *s)
1012 {
1013         STREAM_VERIFY_SANE(s);
1014
1015         s->getp = s->endp = 0;
1016 }
1017
1018 /* Write stream contens to the file discriptor. */
1019 int stream_flush(struct stream *s, int fd)
1020 {
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;
1028 }
1029
1030 /* Stream first in first out queue. */
1031
1032 struct stream_fifo *stream_fifo_new(void)
1033 {
1034         struct stream_fifo *new;
1035
1036         new = XCALLOC(MTYPE_STREAM_FIFO, sizeof(struct stream_fifo));
1037         return new;
1038 }
1039
1040 /* Add new stream to fifo. */
1041 void stream_fifo_push(struct stream_fifo *fifo, struct stream *s)
1042 {
1043         if (fifo->tail)
1044                 fifo->tail->next = s;
1045         else
1046                 fifo->head = s;
1047
1048         fifo->tail = s;
1049
1050         fifo->count++;
1051 }
1052
1053 /* Delete first stream from fifo. */
1054 struct stream *stream_fifo_pop(struct stream_fifo *fifo)
1055 {
1056         struct stream *s;
1057
1058         s = fifo->head;
1059
1060         if (s) {
1061                 fifo->head = s->next;
1062
1063                 if (fifo->head == NULL)
1064                         fifo->tail = NULL;
1065
1066                 fifo->count--;
1067         }
1068
1069         return s;
1070 }
1071
1072 /* Return first fifo entry. */
1073 struct stream *stream_fifo_head(struct stream_fifo *fifo)
1074 {
1075         return fifo->head;
1076 }
1077
1078 void stream_fifo_clean(struct stream_fifo *fifo)
1079 {
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;
1089 }
1090
1091 void stream_fifo_free(struct stream_fifo *fifo)
1092 {
1093         stream_fifo_clean(fifo);
1094         XFREE(MTYPE_STREAM_FIFO, fifo);
1095 }