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.

2 files changed, 104 insertions, 0 deletions

diff --git a/include/osmocom/core/bitvec.h b/include/osmocom/core/bitvec.h
index 2415a814..d64d69dc 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 8596d510..f9341b70 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,7 @@
 #include <errno.h>
 #include <stdint.h>
 #include <string.h>
+#include <stdio.h>
 
 #include <osmocom/core/bitvec.h>
 
@@ -336,4 +338,97 @@ 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)
+{
+	struct bitvec *bv = talloc_zero(ctx, struct bitvec);
+	if (!bv)
+		return NULL;
+
+	bv->data = talloc_zero_array(bv, 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;
+}
+
 /*! @} */