1 files changed, 211 insertions, 0 deletions
diff --git a/gr-digital/lib/header_buffer.cc b/gr-digital/lib/header_buffer.cc
new file mode 100644
index 0000000000..04233da8f2
--- /dev/null
+++ b/gr-digital/lib/header_buffer.cc
@@ -0,0 +1,211 @@
+/* -*- c++ -*- */
+/* Copyright 2015 Free Software Foundation, Inc.
+ *
+ * This file is part of GNU Radio
+ *
+ * GNU Radio 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 3, or (at your option)
+ * any later version.
+ *
+ * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <string.h>
+#include <algorithm>
+#include <stdexcept>
+#include <volk/volk.h>
+#include <gnuradio/digital/header_buffer.h>
+
+namespace gr {
+  namespace digital {
+
+    header_buffer::header_buffer(uint8_t *bytes_out)
+    {
+      d_offset = 0;
+      d_buffer = bytes_out;
+    }
+
+    header_buffer::~header_buffer()
+    {
+    }
+
+    void
+    header_buffer::clear()
+    {
+      if(d_buffer) // TX mode
+        d_offset = 0;
+      else // RX mode
+        d_input.clear();
+    }
+
+    size_t
+    header_buffer::length() const
+    {
+      if(d_buffer) // TX mode
+        return d_offset;
+      else // RX mode
+        return d_input.size();
+    }
+
+    const uint8_t*
+    header_buffer::header() const
+    {
+      return d_buffer;
+    }
+
+    void
+    header_buffer::add_field8(uint8_t data, int len, bool bs)
+    {
+      int nbytes = len/8;
+      if(d_buffer) {
+        memcpy(&d_buffer[d_offset], &data, nbytes);
+        d_offset += nbytes;
+      }
+    }
+
+    void
+    header_buffer::add_field16(uint16_t data, int len, bool bs)
+    {
+      int nbytes = len/8;
+      if(d_buffer) {
+        uint16_t x = data;
+        if(!bs) {
+          volk_16u_byteswap(&x, 1);
+          x = x >> (16-len);
+        }
+        memcpy(&d_buffer[d_offset], &x, nbytes);
+        d_offset += nbytes;
+      }
+    }
+
+    void
+    header_buffer::add_field32(uint32_t data, int len, bool bs)
+    {
+      int nbytes = len/8;
+      if(d_buffer) {
+        uint32_t x = data;
+        if(!bs) {
+          volk_32u_byteswap(&x, 1);
+          x = x >> (32-len);
+        }
+        memcpy(&d_buffer[d_offset], &x, nbytes);
+        d_offset += nbytes;
+      }
+    }
+
+    void
+    header_buffer::add_field64(uint64_t data, int len, bool bs)
+    {
+      int nbytes = len/8;
+      if(d_buffer) {
+        uint64_t x = data;
+        if(!bs) {
+          volk_64u_byteswap(&x, 1);
+          x = x >> (64-len);
+        }
+        memcpy(&d_buffer[d_offset], &x, nbytes);
+        d_offset += nbytes;
+      }
+    }
+
+    void
+    header_buffer::insert_bit(int bit)
+    {
+      d_input.push_back(bit);
+    }
+
+    uint8_t
+    header_buffer::extract_field8(int pos, int len, bool bs)
+    {
+      if(len > 8) {
+        throw std::runtime_error("header_buffer::extract_field for "
+                                 "uint8_t length must be <= 8");
+      }
+
+      uint8_t field = 0x00;
+      std::vector<bool>::iterator itr;
+      for(itr = d_input.begin()+pos; itr != d_input.begin()+pos+len; itr++) {
+        field = (field << 1) | ((*itr) & 0x1);
+      }
+
+      return field;
+    }
+
+    uint16_t
+    header_buffer::extract_field16(int pos, int len, bool bs)
+    {
+      if(len > 16) {
+        throw std::runtime_error("header_buffer::extract_field for "
+                                 "uint16_t length must be <= 16");
+      }
+
+      uint16_t field = 0x0000;
+      std::vector<bool>::iterator itr;
+      for(itr = d_input.begin()+pos; itr != d_input.begin()+pos+len; itr++) {
+        field = (field << 1) | ((*itr) & 0x1);
+      }
+
+      if(bs) {
+        volk_16u_byteswap(&field, 1);
+      }
+
+      return field;
+    }
+
+    uint32_t
+    header_buffer::extract_field32(int pos, int len, bool bs)
+    {
+      if(len > 32) {
+        throw std::runtime_error("header_buffer::extract_field for "
+                                 "uint32_t length must be <= 32");
+      }
+
+      uint32_t field = 0x00000000;
+      std::vector<bool>::iterator itr;
+      for(itr = d_input.begin()+pos; itr != d_input.begin()+pos+len; itr++) {
+        field = (field << 1) | ((*itr) & 0x1);
+      }
+
+      if(bs) {
+        volk_32u_byteswap(&field, 1);
+      }
+
+      return field;
+    }
+
+    uint64_t
+    header_buffer::extract_field64(int pos, int len, bool bs)
+    {
+      if(len > 64) {
+        throw std::runtime_error("header_buffer::extract_field for "
+                                 "uint64_t length must be <= 64");
+      }
+
+      uint64_t field = 0x0000000000000000;
+      std::vector<bool>::iterator itr;
+      for(itr = d_input.begin()+pos; itr != d_input.begin()+pos+len; itr++) {
+        field = (field << 1) | ((*itr) & 0x1);
+      }
+
+      if(bs) {
+        volk_64u_byteswap(&field, 1);
+      }
+
+      return field;
+    }
+
+  } /* namespace digital */
+} /* namespace gr */