zebra, lib: fix the ZEBRA_INTERFACE_VRF_UPDATE zapi message

[mirror_frr.git] / lib / stream.c

/*
 * Packet interface
 * Copyright (C) 1999 Kunihiro Ishiguro
 *
 * This file is part of GNU Zebra.
 *
 * GNU Zebra is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2, or (at your option) any
 * later version.
 *
 * GNU Zebra is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; see the file COPYING; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <zebra.h>
#include <stddef.h>
#include <pthread.h>

#include "stream.h"
#include "memory.h"
#include "network.h"
#include "prefix.h"
#include "log.h"
#include "lib_errors.h"

DEFINE_MTYPE_STATIC(LIB, STREAM, "Stream")
DEFINE_MTYPE_STATIC(LIB, STREAM_FIFO, "Stream FIFO")

/* Tests whether a position is valid */
#define GETP_VALID(S, G) ((G) <= (S)->endp)
#define PUT_AT_VALID(S,G) GETP_VALID(S,G)
#define ENDP_VALID(S, E) ((E) <= (S)->size)

/* asserting sanity checks. Following must be true before
 * stream functions are called:
 *
 * Following must always be true of stream elements
 * before and after calls to stream functions:
 *
 * getp <= endp <= size
 *
 * Note that after a stream function is called following may be true:
 * if (getp == endp) then stream is no longer readable
 * if (endp == size) then stream is no longer writeable
 *
 * It is valid to put to anywhere within the size of the stream, but only
 * using stream_put..._at() functions.
 */
#define STREAM_WARN_OFFSETS(S)                                                 \
	flog_warn(EC_LIB_STREAM,                                               \
		  "&(struct stream): %p, size: %lu, getp: %lu, endp: %lu\n",   \
		  (void *)(S), (unsigned long)(S)->size,                       \
		  (unsigned long)(S)->getp, (unsigned long)(S)->endp)

#define STREAM_VERIFY_SANE(S)                                                  \
	do {                                                                   \
		if (!(GETP_VALID(S, (S)->getp) && ENDP_VALID(S, (S)->endp)))   \
			STREAM_WARN_OFFSETS(S);                                \
		assert(GETP_VALID(S, (S)->getp));                              \
		assert(ENDP_VALID(S, (S)->endp));                              \
	} while (0)

#define STREAM_BOUND_WARN(S, WHAT)                                             \
	do {                                                                   \
		flog_warn(EC_LIB_STREAM, "%s: Attempt to %s out of bounds",    \
			  __func__, (WHAT));                                   \
		STREAM_WARN_OFFSETS(S);                                        \
		assert(0);                                                     \
	} while (0)

#define STREAM_BOUND_WARN2(S, WHAT)                                            \
	do {                                                                   \
		flog_warn(EC_LIB_STREAM, "%s: Attempt to %s out of bounds",    \
			  __func__, (WHAT));                                   \
		STREAM_WARN_OFFSETS(S);                                        \
	} while (0)

/* XXX: Deprecated macro: do not use */
#define CHECK_SIZE(S, Z)                                                       \
	do {                                                                   \
		if (((S)->endp + (Z)) > (S)->size) {                           \
			flog_warn(                                             \
				EC_LIB_STREAM,                                 \
				"CHECK_SIZE: truncating requested size %lu\n", \
				(unsigned long)(Z));                           \
			STREAM_WARN_OFFSETS(S);                                \
			(Z) = (S)->size - (S)->endp;                           \
		}                                                              \
	} while (0);

/* Make stream buffer. */
struct stream *stream_new(size_t size)
{
	struct stream *s;

	assert(size > 0);

	s = XMALLOC(MTYPE_STREAM, sizeof(struct stream) + size);

	s->getp = s->endp = 0;
	s->next = NULL;
	s->size = size;
	return s;
}

/* Free it now. */
void stream_free(struct stream *s)
{
	if (!s)
		return;

	XFREE(MTYPE_STREAM, s);
}

struct stream *stream_copy(struct stream *new, struct stream *src)
{
	STREAM_VERIFY_SANE(src);

	assert(new != NULL);
	assert(STREAM_SIZE(new) >= src->endp);

	new->endp = src->endp;
	new->getp = src->getp;

	memcpy(new->data, src->data, src->endp);

	return new;
}

struct stream *stream_dup(struct stream *s)
{
	struct stream *new;

	STREAM_VERIFY_SANE(s);

	if ((new = stream_new(s->endp)) == NULL)
		return NULL;

	return (stream_copy(new, s));
}

struct stream *stream_dupcat(struct stream *s1, struct stream *s2,
			     size_t offset)
{
	struct stream *new;

	STREAM_VERIFY_SANE(s1);
	STREAM_VERIFY_SANE(s2);

	if ((new = stream_new(s1->endp + s2->endp)) == NULL)
		return NULL;

	memcpy(new->data, s1->data, offset);
	memcpy(new->data + offset, s2->data, s2->endp);
	memcpy(new->data + offset + s2->endp, s1->data + offset,
	       (s1->endp - offset));
	new->endp = s1->endp + s2->endp;
	return new;
}

size_t stream_resize_inplace(struct stream **sptr, size_t newsize)
{
	struct stream *orig = *sptr;

	STREAM_VERIFY_SANE(orig);

	orig = XREALLOC(MTYPE_STREAM, orig, sizeof(struct stream) + newsize);

	orig->size = newsize;

	if (orig->endp > orig->size)
		orig->endp = orig->size;
	if (orig->getp > orig->endp)
		orig->getp = orig->endp;

	STREAM_VERIFY_SANE(orig);

	*sptr = orig;
	return orig->size;
}

size_t __attribute__((deprecated))stream_resize_orig(struct stream *s,
						     size_t newsize)
{
	assert("stream_resize: Switch code to use stream_resize_inplace" == NULL);

	return stream_resize_inplace(&s, newsize);
}

size_t stream_get_getp(struct stream *s)
{
	STREAM_VERIFY_SANE(s);
	return s->getp;
}

size_t stream_get_endp(struct stream *s)
{
	STREAM_VERIFY_SANE(s);
	return s->endp;
}

size_t stream_get_size(struct stream *s)
{
	STREAM_VERIFY_SANE(s);
	return s->size;
}

/* Stream structre' stream pointer related functions.  */
void stream_set_getp(struct stream *s, size_t pos)
{
	STREAM_VERIFY_SANE(s);

	if (!GETP_VALID(s, pos)) {
		STREAM_BOUND_WARN(s, "set getp");
		pos = s->endp;
	}

	s->getp = pos;
}

void stream_set_endp(struct stream *s, size_t pos)
{
	STREAM_VERIFY_SANE(s);

	if (!ENDP_VALID(s, pos)) {
		STREAM_BOUND_WARN(s, "set endp");
		return;
	}

	/*
	 * Make sure the current read pointer is not beyond the new endp.
	 */
	if (s->getp > pos) {
		STREAM_BOUND_WARN(s, "set endp");
		return;
	}

	s->endp = pos;
	STREAM_VERIFY_SANE(s);
}

/* Forward pointer. */
void stream_forward_getp(struct stream *s, size_t size)
{
	STREAM_VERIFY_SANE(s);

	if (!GETP_VALID(s, s->getp + size)) {
		STREAM_BOUND_WARN(s, "seek getp");
		return;
	}

	s->getp += size;
}

void stream_forward_endp(struct stream *s, size_t size)
{
	STREAM_VERIFY_SANE(s);

	if (!ENDP_VALID(s, s->endp + size)) {
		STREAM_BOUND_WARN(s, "seek endp");
		return;
	}

	s->endp += size;
}

/* Copy from stream to destination. */
bool stream_get2(void *dst, struct stream *s, size_t size)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < size) {
		STREAM_BOUND_WARN2(s, "get");
		return false;
	}

	memcpy(dst, s->data + s->getp, size);
	s->getp += size;

	return true;
}

void stream_get(void *dst, struct stream *s, size_t size)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < size) {
		STREAM_BOUND_WARN(s, "get");
		return;
	}

	memcpy(dst, s->data + s->getp, size);
	s->getp += size;
}

/* Get next character from the stream. */
bool stream_getc2(struct stream *s, uint8_t *byte)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < sizeof(uint8_t)) {
		STREAM_BOUND_WARN2(s, "get char");
		return false;
	}
	*byte = s->data[s->getp++];

	return true;
}

uint8_t stream_getc(struct stream *s)
{
	uint8_t c;

	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < sizeof(uint8_t)) {
		STREAM_BOUND_WARN(s, "get char");
		return 0;
	}
	c = s->data[s->getp++];

	return c;
}

/* Get next character from the stream. */
uint8_t stream_getc_from(struct stream *s, size_t from)
{
	uint8_t c;

	STREAM_VERIFY_SANE(s);

	if (!GETP_VALID(s, from + sizeof(uint8_t))) {
		STREAM_BOUND_WARN(s, "get char");
		return 0;
	}

	c = s->data[from];

	return c;
}

bool stream_getw2(struct stream *s, uint16_t *word)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < sizeof(uint16_t)) {
		STREAM_BOUND_WARN2(s, "get ");
		return false;
	}

	*word = s->data[s->getp++] << 8;
	*word |= s->data[s->getp++];

	return true;
}

/* Get next word from the stream. */
uint16_t stream_getw(struct stream *s)
{
	uint16_t w;

	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < sizeof(uint16_t)) {
		STREAM_BOUND_WARN(s, "get ");
		return 0;
	}

	w = s->data[s->getp++] << 8;
	w |= s->data[s->getp++];

	return w;
}

/* Get next word from the stream. */
uint16_t stream_getw_from(struct stream *s, size_t from)
{
	uint16_t w;

	STREAM_VERIFY_SANE(s);

	if (!GETP_VALID(s, from + sizeof(uint16_t))) {
		STREAM_BOUND_WARN(s, "get ");
		return 0;
	}

	w = s->data[from++] << 8;
	w |= s->data[from];

	return w;
}

/* Get next 3-byte from the stream. */
uint32_t stream_get3_from(struct stream *s, size_t from)
{
	uint32_t l;

	STREAM_VERIFY_SANE(s);

	if (!GETP_VALID(s, from + 3)) {
		STREAM_BOUND_WARN(s, "get 3byte");
		return 0;
	}

	l = s->data[from++] << 16;
	l |= s->data[from++] << 8;
	l |= s->data[from];

	return l;
}

uint32_t stream_get3(struct stream *s)
{
	uint32_t l;

	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < 3) {
		STREAM_BOUND_WARN(s, "get 3byte");
		return 0;
	}

	l = s->data[s->getp++] << 16;
	l |= s->data[s->getp++] << 8;
	l |= s->data[s->getp++];

	return l;
}

/* Get next long word from the stream. */
uint32_t stream_getl_from(struct stream *s, size_t from)
{
	uint32_t l;

	STREAM_VERIFY_SANE(s);

	if (!GETP_VALID(s, from + sizeof(uint32_t))) {
		STREAM_BOUND_WARN(s, "get long");
		return 0;
	}

	l = (unsigned)(s->data[from++]) << 24;
	l |= s->data[from++] << 16;
	l |= s->data[from++] << 8;
	l |= s->data[from];

	return l;
}

/* Copy from stream at specific location to destination. */
void stream_get_from(void *dst, struct stream *s, size_t from, size_t size)
{
	STREAM_VERIFY_SANE(s);

	if (!GETP_VALID(s, from + size)) {
		STREAM_BOUND_WARN(s, "get from");
		return;
	}

	memcpy(dst, s->data + from, size);
}

bool stream_getl2(struct stream *s, uint32_t *l)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < sizeof(uint32_t)) {
		STREAM_BOUND_WARN2(s, "get long");
		return false;
	}

	*l = (unsigned int)(s->data[s->getp++]) << 24;
	*l |= s->data[s->getp++] << 16;
	*l |= s->data[s->getp++] << 8;
	*l |= s->data[s->getp++];

	return true;
}

uint32_t stream_getl(struct stream *s)
{
	uint32_t l;

	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < sizeof(uint32_t)) {
		STREAM_BOUND_WARN(s, "get long");
		return 0;
	}

	l = (unsigned)(s->data[s->getp++]) << 24;
	l |= s->data[s->getp++] << 16;
	l |= s->data[s->getp++] << 8;
	l |= s->data[s->getp++];

	return l;
}

/* Get next quad word from the stream. */
uint64_t stream_getq_from(struct stream *s, size_t from)
{
	uint64_t q;

	STREAM_VERIFY_SANE(s);

	if (!GETP_VALID(s, from + sizeof(uint64_t))) {
		STREAM_BOUND_WARN(s, "get quad");
		return 0;
	}

	q = ((uint64_t)s->data[from++]) << 56;
	q |= ((uint64_t)s->data[from++]) << 48;
	q |= ((uint64_t)s->data[from++]) << 40;
	q |= ((uint64_t)s->data[from++]) << 32;
	q |= ((uint64_t)s->data[from++]) << 24;
	q |= ((uint64_t)s->data[from++]) << 16;
	q |= ((uint64_t)s->data[from++]) << 8;
	q |= ((uint64_t)s->data[from++]);

	return q;
}

uint64_t stream_getq(struct stream *s)
{
	uint64_t q;

	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < sizeof(uint64_t)) {
		STREAM_BOUND_WARN(s, "get quad");
		return 0;
	}

	q = ((uint64_t)s->data[s->getp++]) << 56;
	q |= ((uint64_t)s->data[s->getp++]) << 48;
	q |= ((uint64_t)s->data[s->getp++]) << 40;
	q |= ((uint64_t)s->data[s->getp++]) << 32;
	q |= ((uint64_t)s->data[s->getp++]) << 24;
	q |= ((uint64_t)s->data[s->getp++]) << 16;
	q |= ((uint64_t)s->data[s->getp++]) << 8;
	q |= ((uint64_t)s->data[s->getp++]);

	return q;
}

/* Get next long word from the stream. */
uint32_t stream_get_ipv4(struct stream *s)
{
	uint32_t l;

	STREAM_VERIFY_SANE(s);

	if (STREAM_READABLE(s) < sizeof(uint32_t)) {
		STREAM_BOUND_WARN(s, "get ipv4");
		return 0;
	}

	memcpy(&l, s->data + s->getp, sizeof(uint32_t));
	s->getp += sizeof(uint32_t);

	return l;
}

float stream_getf(struct stream *s)
{
	union {
		float r;
		uint32_t d;
	} u;
	u.d = stream_getl(s);
	return u.r;
}

double stream_getd(struct stream *s)
{
	union {
		double r;
		uint64_t d;
	} u;
	u.d = stream_getq(s);
	return u.r;
}

/* Copy to source to stream.
 *
 * XXX: This uses CHECK_SIZE and hence has funny semantics -> Size will wrap
 * around. This should be fixed once the stream updates are working.
 *
 * stream_write() is saner
 */
void stream_put(struct stream *s, const void *src, size_t size)
{

	/* XXX: CHECK_SIZE has strange semantics. It should be deprecated */
	CHECK_SIZE(s, size);

	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < size) {
		STREAM_BOUND_WARN(s, "put");
		return;
	}

	if (src)
		memcpy(s->data + s->endp, src, size);
	else
		memset(s->data + s->endp, 0, size);

	s->endp += size;
}

/* Put character to the stream. */
int stream_putc(struct stream *s, uint8_t c)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < sizeof(uint8_t)) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	s->data[s->endp++] = c;
	return sizeof(uint8_t);
}

/* Put word to the stream. */
int stream_putw(struct stream *s, uint16_t w)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < sizeof(uint16_t)) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	s->data[s->endp++] = (uint8_t)(w >> 8);
	s->data[s->endp++] = (uint8_t)w;

	return 2;
}

/* Put long word to the stream. */
int stream_put3(struct stream *s, uint32_t l)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < 3) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	s->data[s->endp++] = (uint8_t)(l >> 16);
	s->data[s->endp++] = (uint8_t)(l >> 8);
	s->data[s->endp++] = (uint8_t)l;

	return 3;
}

/* Put long word to the stream. */
int stream_putl(struct stream *s, uint32_t l)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < sizeof(uint32_t)) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	s->data[s->endp++] = (uint8_t)(l >> 24);
	s->data[s->endp++] = (uint8_t)(l >> 16);
	s->data[s->endp++] = (uint8_t)(l >> 8);
	s->data[s->endp++] = (uint8_t)l;

	return 4;
}

/* Put quad word to the stream. */
int stream_putq(struct stream *s, uint64_t q)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < sizeof(uint64_t)) {
		STREAM_BOUND_WARN(s, "put quad");
		return 0;
	}

	s->data[s->endp++] = (uint8_t)(q >> 56);
	s->data[s->endp++] = (uint8_t)(q >> 48);
	s->data[s->endp++] = (uint8_t)(q >> 40);
	s->data[s->endp++] = (uint8_t)(q >> 32);
	s->data[s->endp++] = (uint8_t)(q >> 24);
	s->data[s->endp++] = (uint8_t)(q >> 16);
	s->data[s->endp++] = (uint8_t)(q >> 8);
	s->data[s->endp++] = (uint8_t)q;

	return 8;
}

int stream_putf(struct stream *s, float f)
{
	union {
		float i;
		uint32_t o;
	} u;
	u.i = f;
	return stream_putl(s, u.o);
}

int stream_putd(struct stream *s, double d)
{
	union {
		double i;
		uint64_t o;
	} u;
	u.i = d;
	return stream_putq(s, u.o);
}

int stream_putc_at(struct stream *s, size_t putp, uint8_t c)
{
	STREAM_VERIFY_SANE(s);

	if (!PUT_AT_VALID(s, putp + sizeof(uint8_t))) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	s->data[putp] = c;

	return 1;
}

int stream_putw_at(struct stream *s, size_t putp, uint16_t w)
{
	STREAM_VERIFY_SANE(s);

	if (!PUT_AT_VALID(s, putp + sizeof(uint16_t))) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	s->data[putp] = (uint8_t)(w >> 8);
	s->data[putp + 1] = (uint8_t)w;

	return 2;
}

int stream_put3_at(struct stream *s, size_t putp, uint32_t l)
{
	STREAM_VERIFY_SANE(s);

	if (!PUT_AT_VALID(s, putp + 3)) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}
	s->data[putp] = (uint8_t)(l >> 16);
	s->data[putp + 1] = (uint8_t)(l >> 8);
	s->data[putp + 2] = (uint8_t)l;

	return 3;
}

int stream_putl_at(struct stream *s, size_t putp, uint32_t l)
{
	STREAM_VERIFY_SANE(s);

	if (!PUT_AT_VALID(s, putp + sizeof(uint32_t))) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}
	s->data[putp] = (uint8_t)(l >> 24);
	s->data[putp + 1] = (uint8_t)(l >> 16);
	s->data[putp + 2] = (uint8_t)(l >> 8);
	s->data[putp + 3] = (uint8_t)l;

	return 4;
}

int stream_putq_at(struct stream *s, size_t putp, uint64_t q)
{
	STREAM_VERIFY_SANE(s);

	if (!PUT_AT_VALID(s, putp + sizeof(uint64_t))) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}
	s->data[putp] = (uint8_t)(q >> 56);
	s->data[putp + 1] = (uint8_t)(q >> 48);
	s->data[putp + 2] = (uint8_t)(q >> 40);
	s->data[putp + 3] = (uint8_t)(q >> 32);
	s->data[putp + 4] = (uint8_t)(q >> 24);
	s->data[putp + 5] = (uint8_t)(q >> 16);
	s->data[putp + 6] = (uint8_t)(q >> 8);
	s->data[putp + 7] = (uint8_t)q;

	return 8;
}

/* Put long word to the stream. */
int stream_put_ipv4(struct stream *s, uint32_t l)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < sizeof(uint32_t)) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}
	memcpy(s->data + s->endp, &l, sizeof(uint32_t));
	s->endp += sizeof(uint32_t);

	return sizeof(uint32_t);
}

/* Put long word to the stream. */
int stream_put_in_addr(struct stream *s, struct in_addr *addr)
{
	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < sizeof(uint32_t)) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	memcpy(s->data + s->endp, addr, sizeof(uint32_t));
	s->endp += sizeof(uint32_t);

	return sizeof(uint32_t);
}

/* Put in_addr at location in the stream. */
int stream_put_in_addr_at(struct stream *s, size_t putp, struct in_addr *addr)
{
	STREAM_VERIFY_SANE(s);

	if (!PUT_AT_VALID(s, putp + 4)) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	memcpy(&s->data[putp], addr, 4);
	return 4;
}

/* Put in6_addr at location in the stream. */
int stream_put_in6_addr_at(struct stream *s, size_t putp, struct in6_addr *addr)
{
	STREAM_VERIFY_SANE(s);

	if (!PUT_AT_VALID(s, putp + 16)) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	memcpy(&s->data[putp], addr, 16);
	return 16;
}

/* Put prefix by nlri type format. */
int stream_put_prefix_addpath(struct stream *s, struct prefix *p,
			      int addpath_encode, uint32_t addpath_tx_id)
{
	size_t psize;
	size_t psize_with_addpath;

	STREAM_VERIFY_SANE(s);

	psize = PSIZE(p->prefixlen);

	if (addpath_encode)
		psize_with_addpath = psize + 4;
	else
		psize_with_addpath = psize;

	if (STREAM_WRITEABLE(s) < (psize_with_addpath + sizeof(uint8_t))) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	if (addpath_encode) {
		s->data[s->endp++] = (uint8_t)(addpath_tx_id >> 24);
		s->data[s->endp++] = (uint8_t)(addpath_tx_id >> 16);
		s->data[s->endp++] = (uint8_t)(addpath_tx_id >> 8);
		s->data[s->endp++] = (uint8_t)addpath_tx_id;
	}

	s->data[s->endp++] = p->prefixlen;
	memcpy(s->data + s->endp, &p->u.prefix, psize);
	s->endp += psize;

	return psize;
}

int stream_put_prefix(struct stream *s, struct prefix *p)
{
	return stream_put_prefix_addpath(s, p, 0, 0);
}

/* Put NLRI with label */
int stream_put_labeled_prefix(struct stream *s, struct prefix *p,
			      mpls_label_t *label)
{
	size_t psize;
	uint8_t *label_pnt = (uint8_t *)label;

	STREAM_VERIFY_SANE(s);

	psize = PSIZE(p->prefixlen);

	if (STREAM_WRITEABLE(s) < (psize + 3)) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	stream_putc(s, (p->prefixlen + 24));
	stream_putc(s, label_pnt[0]);
	stream_putc(s, label_pnt[1]);
	stream_putc(s, label_pnt[2]);
	memcpy(s->data + s->endp, &p->u.prefix, psize);
	s->endp += psize;

	return (psize + 3);
}

/* Read size from fd. */
int stream_read(struct stream *s, int fd, size_t size)
{
	int nbytes;

	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < size) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	nbytes = readn(fd, s->data + s->endp, size);

	if (nbytes > 0)
		s->endp += nbytes;

	return nbytes;
}

ssize_t stream_read_try(struct stream *s, int fd, size_t size)
{
	ssize_t nbytes;

	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < size) {
		STREAM_BOUND_WARN(s, "put");
		/* Fatal (not transient) error, since retrying will not help
		   (stream is too small to contain the desired data). */
		return -1;
	}

	if ((nbytes = read(fd, s->data + s->endp, size)) >= 0) {
		s->endp += nbytes;
		return nbytes;
	}
	/* Error: was it transient (return -2) or fatal (return -1)? */
	if (ERRNO_IO_RETRY(errno))
		return -2;
	flog_err(EC_LIB_SOCKET, "%s: read failed on fd %d: %s", __func__, fd,
		 safe_strerror(errno));
	return -1;
}

/* Read up to size bytes into the stream from the fd, using recvmsgfrom
 * whose arguments match the remaining arguments to this function
 */
ssize_t stream_recvfrom(struct stream *s, int fd, size_t size, int flags,
			struct sockaddr *from, socklen_t *fromlen)
{
	ssize_t nbytes;

	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < size) {
		STREAM_BOUND_WARN(s, "put");
		/* Fatal (not transient) error, since retrying will not help
		   (stream is too small to contain the desired data). */
		return -1;
	}

	if ((nbytes = recvfrom(fd, s->data + s->endp, size, flags, from,
			       fromlen))
	    >= 0) {
		s->endp += nbytes;
		return nbytes;
	}
	/* Error: was it transient (return -2) or fatal (return -1)? */
	if (ERRNO_IO_RETRY(errno))
		return -2;
	flog_err(EC_LIB_SOCKET, "%s: read failed on fd %d: %s", __func__, fd,
		 safe_strerror(errno));
	return -1;
}

/* Read up to smaller of size or SIZE_REMAIN() bytes to the stream, starting
 * from endp.
 * First iovec will be used to receive the data.
 * Stream need not be empty.
 */
ssize_t stream_recvmsg(struct stream *s, int fd, struct msghdr *msgh, int flags,
		       size_t size)
{
	int nbytes;
	struct iovec *iov;

	STREAM_VERIFY_SANE(s);
	assert(msgh->msg_iovlen > 0);

	if (STREAM_WRITEABLE(s) < size) {
		STREAM_BOUND_WARN(s, "put");
		/* This is a logic error in the calling code: the stream is too
		   small
		   to hold the desired data! */
		return -1;
	}

	iov = &(msgh->msg_iov[0]);
	iov->iov_base = (s->data + s->endp);
	iov->iov_len = size;

	nbytes = recvmsg(fd, msgh, flags);

	if (nbytes > 0)
		s->endp += nbytes;

	return nbytes;
}

/* Write data to buffer. */
size_t stream_write(struct stream *s, const void *ptr, size_t size)
{

	CHECK_SIZE(s, size);

	STREAM_VERIFY_SANE(s);

	if (STREAM_WRITEABLE(s) < size) {
		STREAM_BOUND_WARN(s, "put");
		return 0;
	}

	memcpy(s->data + s->endp, ptr, size);
	s->endp += size;

	return size;
}

/* Return current read pointer.
 * DEPRECATED!
 * Use stream_get_pnt_to if you must, but decoding streams properly
 * is preferred
 */
uint8_t *stream_pnt(struct stream *s)
{
	STREAM_VERIFY_SANE(s);
	return s->data + s->getp;
}

/* Check does this stream empty? */
int stream_empty(struct stream *s)
{
	STREAM_VERIFY_SANE(s);

	return (s->endp == 0);
}

/* Reset stream. */
void stream_reset(struct stream *s)
{
	STREAM_VERIFY_SANE(s);

	s->getp = s->endp = 0;
}

/* Write stream contens to the file discriptor. */
int stream_flush(struct stream *s, int fd)
{
	int nbytes;

	STREAM_VERIFY_SANE(s);

	nbytes = write(fd, s->data + s->getp, s->endp - s->getp);

	return nbytes;
}

/* Stream first in first out queue. */

struct stream_fifo *stream_fifo_new(void)
{
	struct stream_fifo *new;

	new = XCALLOC(MTYPE_STREAM_FIFO, sizeof(struct stream_fifo));
	pthread_mutex_init(&new->mtx, NULL);
	return new;
}

/* Add new stream to fifo. */
void stream_fifo_push(struct stream_fifo *fifo, struct stream *s)
{
#if defined DEV_BUILD
	size_t max, curmax;
#endif

	if (fifo->tail)
		fifo->tail->next = s;
	else
		fifo->head = s;

	fifo->tail = s;
	fifo->tail->next = NULL;
#if !defined DEV_BUILD
	atomic_fetch_add_explicit(&fifo->count, 1, memory_order_release);
#else
	max = atomic_fetch_add_explicit(&fifo->count, 1, memory_order_release);
	curmax = atomic_load_explicit(&fifo->max_count, memory_order_relaxed);
	if (max > curmax)
		atomic_store_explicit(&fifo->max_count, max,
				      memory_order_relaxed);
#endif
}

void stream_fifo_push_safe(struct stream_fifo *fifo, struct stream *s)
{
	pthread_mutex_lock(&fifo->mtx);
	{
		stream_fifo_push(fifo, s);
	}
	pthread_mutex_unlock(&fifo->mtx);
}

/* Delete first stream from fifo. */
struct stream *stream_fifo_pop(struct stream_fifo *fifo)
{
	struct stream *s;

	s = fifo->head;

	if (s) {
		fifo->head = s->next;

		if (fifo->head == NULL)
			fifo->tail = NULL;

		atomic_fetch_sub_explicit(&fifo->count, 1,
					  memory_order_release);

		/* ensure stream is scrubbed of references to this fifo */
		s->next = NULL;
	}

	return s;
}

struct stream *stream_fifo_pop_safe(struct stream_fifo *fifo)
{
	struct stream *ret;

	pthread_mutex_lock(&fifo->mtx);
	{
		ret = stream_fifo_pop(fifo);
	}
	pthread_mutex_unlock(&fifo->mtx);

	return ret;
}

struct stream *stream_fifo_head(struct stream_fifo *fifo)
{
	return fifo->head;
}

struct stream *stream_fifo_head_safe(struct stream_fifo *fifo)
{
	struct stream *ret;

	pthread_mutex_lock(&fifo->mtx);
	{
		ret = stream_fifo_head(fifo);
	}
	pthread_mutex_unlock(&fifo->mtx);

	return ret;
}

void stream_fifo_clean(struct stream_fifo *fifo)
{
	struct stream *s;
	struct stream *next;

	for (s = fifo->head; s; s = next) {
		next = s->next;
		stream_free(s);
	}
	fifo->head = fifo->tail = NULL;
	atomic_store_explicit(&fifo->count, 0, memory_order_release);
}

void stream_fifo_clean_safe(struct stream_fifo *fifo)
{
	pthread_mutex_lock(&fifo->mtx);
	{
		stream_fifo_clean(fifo);
	}
	pthread_mutex_unlock(&fifo->mtx);
}

size_t stream_fifo_count_safe(struct stream_fifo *fifo)
{
	return atomic_load_explicit(&fifo->count, memory_order_acquire);
}

void stream_fifo_free(struct stream_fifo *fifo)
{
	stream_fifo_clean(fifo);
	pthread_mutex_destroy(&fifo->mtx);
	XFREE(MTYPE_STREAM_FIFO, fifo);
}