[3/5] Add bitvector functions from osmo-pcu

From bf86d3a8449721968d3016793ee34420fc128492 Mon Sep 17 00:00:00 2001
From: Max <msuraev@sysmocom.de>
Date: Mon, 25 Jan 2016 17:24:18 +0100
Subject: [PATCH] Add bitvec-related functions from Osmo-PCU

Allocation, pack/unpack, field access and helper routines used
extensively by Osmo-PCU. Whenever memory allocation happens, alocator
context is passed explicitly by caller.
---
 include/osmocom/core/bitvec.h |  9 ++++
 src/bitvec.c                  | 99 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 108 insertions(+)

diff --git a/include/osmocom/core/bitvec.h b/include/osmocom/core/bitvec.h
index 2415a81..d64d69d 100644
--- a/include/osmocom/core/bitvec.h
+++ b/include/osmocom/core/bitvec.h
@@ -3,6 +3,7 @@ 
 /* bit vector utility routines */
 
 /* (C) 2009 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2012 Ivan Klyuchnikov
  *
  * All Rights Reserved
  *
@@ -39,6 +40,7 @@ 
  */
 
 #include <stdint.h>
+#include <talloc.h>
 
 /*! \brief A single GSM bit
  *
@@ -73,5 +75,12 @@  int bitvec_find_bit_pos(const struct bitvec *bv, unsigned int n, enum bit_value
 int bitvec_spare_padding(struct bitvec *bv, unsigned int up_to_bit);
 int bitvec_get_bytes(struct bitvec *bv, uint8_t *bytes, unsigned int count);
 int bitvec_set_bytes(struct bitvec *bv, const uint8_t *bytes, unsigned int count);
+struct bitvec *bitvec_alloc(unsigned int size, TALLOC_CTX *bvctx);
+void bitvec_free(struct bitvec *bv);
+int bitvec_unhex(struct bitvec *bv, const char *src);
+unsigned int bitvec_pack(const struct bitvec *bv, uint8_t *buffer);
+unsigned int bitvec_unpack(struct bitvec *bv, const uint8_t *buffer);
+uint64_t bitvec_read_field(struct bitvec *bv, unsigned int read_index, unsigned int len);
+int bitvec_write_field(struct bitvec *bv, unsigned int write_index, uint64_t val, unsigned int len);
 
 /*! @} */
diff --git a/src/bitvec.c b/src/bitvec.c
index 8596d51..96f8ea6 100644
--- a/src/bitvec.c
+++ b/src/bitvec.c
@@ -1,6 +1,7 @@ 
 /* bit vector utility routines */
 
 /* (C) 2009 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2012 Ivan Klyuchnikov
  * (C) 2015 by Sysmocom s.f.m.c. GmbH
  *
  * All Rights Reserved
@@ -32,6 +33,8 @@ 
 #include <errno.h>
 #include <stdint.h>
 #include <string.h>
+#include <stdio.h>
+#include <talloc.h>
 
 #include <osmocom/core/bitvec.h>
 
@@ -336,4 +339,100 @@  int bitvec_set_bytes(struct bitvec *bv, const uint8_t *bytes, unsigned int count
 	bv->cur_bit += count * 8;
 	return 0;
 }
+
+struct bitvec *bitvec_alloc(unsigned int size, TALLOC_CTX *ctx)
+{
+	TALLOC_CTX *bvctx = talloc_zero(ctx, struct bitvec);
+	if (!bvctx)
+		return NULL;
+
+	struct bitvec *bv = bvctx;
+
+	bv->data = talloc_zero_array(bvctx, uint8_t, size);
+	if (!(bv->data)) {
+		talloc_free(bv);
+		return NULL;
+	}
+
+	bv->data_len = size;
+	bv->cur_bit = 0;
+	return bv;
+}
+
+void bitvec_free(struct bitvec *bv)
+{
+	talloc_free(bv->data);
+	talloc_free(bv);
+}
+
+unsigned int bitvec_pack(const struct bitvec *bv, uint8_t *buffer)
+{
+	unsigned int i = 0;
+	for (i = 0; i < bv->data_len; i++)
+	{
+		buffer[i] = bv->data[i];
+	}
+	return i;
+}
+
+unsigned int bitvec_unpack(struct bitvec *bv, const uint8_t *buffer)
+{
+	unsigned int i = 0;
+	for (i = 0; i < bv->data_len; i++) {
+		bv->data[i] = buffer[i];
+	}
+	return i;
+}
+
+
+int bitvec_unhex(struct bitvec *bv, const char *src)
+{
+	unsigned val;
+	unsigned write_index = 0;
+	unsigned digits = bv->data_len*2;
+	for (unsigned i=0; i<digits; i++) {
+		if (sscanf(src+i, "%1x", &val) < 1) {
+			return 1;
+		}
+		bitvec_write_field(bv, write_index,val, 4);
+	}
+	return 0;
+}
+
+uint64_t bitvec_read_field(struct bitvec *bv, unsigned int read_index, unsigned int len)
+{
+	unsigned int i;
+	uint64_t ui = 0;
+	bv->cur_bit = read_index;
+
+	for (i = 0; i < len; i++) {
+		int bit = bitvec_get_bit_pos((const struct bitvec *)bv, bv->cur_bit);
+		if (bit < 0)
+			return bit;
+		if (bit)
+			ui |= ((uint64_t)1 << (len - i - 1));
+		bv->cur_bit++;
+	}
+	read_index += len;
+	return ui;
+}
+
+
+int bitvec_write_field(struct bitvec *bv, unsigned int write_index, uint64_t val, unsigned int len)
+{
+	unsigned int i;
+	int rc;
+	bv->cur_bit = write_index;
+	for (i = 0; i < len; i++) {
+		int bit = 0;
+		if (val & ((uint64_t)1 << (len - i - 1)))
+			bit = 1;
+		rc = bitvec_set_bit(bv, bit);
+		if (rc)
+			return rc;
+	}
+	write_index += len;
+	return 0;
+}
+
 /*! @} */
-- 
2.5.0