From d6f4a61dde127c6154f7933b0a4003740b6a0768 Mon Sep 17 00:00:00 2001
From: David Lamparter <equinox@opensourcerouting.org>
Date: Thu, 28 Jul 2016 17:23:47 +0200
Subject: [PATCH] lib: add 3-byte stream functions

This will be used for BGP MPLS labels.

Signed-off-by: David Lamparter <equinox@opensourcerouting.org>
---
 lib/stream.c | 77 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 lib/stream.h |  4 +++
 2 files changed, 81 insertions(+)

diff --git a/lib/stream.c b/lib/stream.c
index 4c237563a5..fe77e2a2ad 100644
--- a/lib/stream.c
+++ b/lib/stream.c
@@ -373,6 +373,47 @@ stream_getw_from (struct stream *s, size_t from)
   return w;
 }
 
+/* Get next 3-byte from the stream. */
+u_int32_t
+stream_get3_from (struct stream *s, size_t from)
+{
+  u_int32_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;
+}
+
+u_int32_t
+stream_get3 (struct stream *s)
+{
+  u_int32_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. */
 u_int32_t
 stream_getl_from (struct stream *s, size_t from)
@@ -566,6 +607,25 @@ stream_putw (struct stream *s, u_int16_t w)
   return 2;
 }
 
+/* Put long word to the stream. */
+int
+stream_put3 (struct stream *s, u_int32_t l)
+{
+  STREAM_VERIFY_SANE (s);
+
+  if (STREAM_WRITEABLE (s) < 3)
+    {
+      STREAM_BOUND_WARN (s, "put");
+      return 0;
+    }
+  
+  s->data[s->endp++] = (u_char)(l >> 16);
+  s->data[s->endp++] = (u_char)(l >>  8);
+  s->data[s->endp++] = (u_char)l;
+
+  return 3;
+}
+
 /* Put long word to the stream. */
 int
 stream_putl (struct stream *s, u_int32_t l)
@@ -643,6 +703,23 @@ stream_putw_at (struct stream *s, size_t putp, u_int16_t w)
   return 2;
 }
 
+int
+stream_put3_at (struct stream *s, size_t putp, u_int32_t l)
+{
+  STREAM_VERIFY_SANE(s);
+  
+  if (!PUT_AT_VALID (s, putp + 3))
+    {
+      STREAM_BOUND_WARN (s, "put");
+      return 0;
+    }
+  s->data[putp] = (u_char)(l >> 16);
+  s->data[putp + 1] = (u_char)(l >>  8);
+  s->data[putp + 2] = (u_char)l;
+  
+  return 3;
+}
+
 int
 stream_putl_at (struct stream *s, size_t putp, u_int32_t l)
 {
diff --git a/lib/stream.h b/lib/stream.h
index 738034438c..4e9b21df5f 100644
--- a/lib/stream.h
+++ b/lib/stream.h
@@ -167,6 +167,8 @@ extern int stream_putc (struct stream *, u_char);
 extern int stream_putc_at (struct stream *, size_t, u_char);
 extern int stream_putw (struct stream *, u_int16_t);
 extern int stream_putw_at (struct stream *, size_t, u_int16_t);
+extern int stream_put3 (struct stream *, u_int32_t);
+extern int stream_put3_at (struct stream *, size_t, u_int32_t);
 extern int stream_putl (struct stream *, u_int32_t);
 extern int stream_putl_at (struct stream *, size_t, u_int32_t);
 extern int stream_putq (struct stream *, uint64_t);
@@ -186,6 +188,8 @@ extern u_char stream_getc (struct stream *);
 extern u_char stream_getc_from (struct stream *, size_t);
 extern u_int16_t stream_getw (struct stream *);
 extern u_int16_t stream_getw_from (struct stream *, size_t);
+extern u_int32_t stream_get3 (struct stream *);
+extern u_int32_t stream_get3_from (struct stream *, size_t);
 extern u_int32_t stream_getl (struct stream *);
 extern u_int32_t stream_getl_from (struct stream *, size_t);
 extern uint64_t stream_getq (struct stream *);