>From 4cd99e3fa5cda1655da226d6abd562f902d67fcc Mon Sep 17 00:00:00 2001
From: Max <msuraev@sysmocom.de>
Date: Tue, 26 Jan 2016 18:53:45 +0100
Subject: [PATCH] Expand bitvec interface

Add bit filling, shifting and oter functions necessary for bit
compression implementation.

Sponsored-by: On-Waves ehf
---
 include/osmocom/core/bitvec.h |   9 +++
 src/bitvec.c                  | 132 ++++++++++++++++++++++++++++++++++++++++--
 2 files changed, 136 insertions(+), 5 deletions(-)

diff --git a/include/osmocom/core/bitvec.h b/include/osmocom/core/bitvec.h
index d64d69d..774272f 100644
--- a/include/osmocom/core/bitvec.h
+++ b/include/osmocom/core/bitvec.h
@@ -4,6 +4,7 @@
 
 /* (C) 2009 by Harald Welte <laforge@gnumonks.org>
  * (C) 2012 Ivan Klyuchnikov
+ * (C) 2015 Sysmocom s.f.m.c. GmbH by Max <msuraev@sysmocom.de>
  *
  * All Rights Reserved
  *
@@ -41,6 +42,7 @@
 
 #include <stdint.h>
 #include <talloc.h>
+#include <stdbool.h>
 
 /*! \brief A single GSM bit
  *
@@ -82,5 +84,12 @@ 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);
+int bitvec_fill(struct bitvec *bv, unsigned int num_bits, enum bit_value fill);
+char bit_value_to_char(enum bit_value v);
+void bitvec_to_string_r(const struct bitvec *bv, char *str);
+void bitvec_zero(struct bitvec *bv);
+unsigned bitvec_rl(const struct bitvec *bv, bool b);
+void bitvec_shiftl(struct bitvec *bv, unsigned int n);
+int16_t bitvec_get_int16_msb(const struct bitvec *bv, unsigned int num_bits);
 
 /*! @} */
diff --git a/src/bitvec.c b/src/bitvec.c
index f9341b7..09281d4 100644
--- a/src/bitvec.c
+++ b/src/bitvec.c
@@ -34,7 +34,9 @@
 #include <stdint.h>
 #include <string.h>
 #include <stdio.h>
+#include <stdbool.h>
 
+#include <osmocom/core/bits.h>
 #include <osmocom/core/bitvec.h>
 
 #define BITNUM_FROM_COMP(byte, bit)	((byte*8)+bit)
@@ -222,6 +224,20 @@ int bitvec_set_uint(struct bitvec *bv, unsigned int ui, unsigned int num_bits)
 	return 0;
 }
 
+/*! \brief get multiple bits (num_bits) from beginning of vector (MSB side) */
+int16_t bitvec_get_int16_msb(const struct bitvec *bv, unsigned int num_bits)
+{
+	uint8_t tmp[2];
+	if (num_bits > 15 || bv->cur_bit < num_bits)
+		return -EINVAL;
+
+	if (num_bits < 9)
+		return bv->data[0] >> (8 - num_bits);
+
+	memcpy(tmp, bv->data, 2);
+	return osmo_load16be(tmp) >> (16 - num_bits);
+}
+
 /*! \brief get multiple bits (based on numeric value) from current pos */
 int bitvec_get_uint(struct bitvec *bv, unsigned int num_bits)
 {
@@ -240,15 +256,27 @@ int bitvec_get_uint(struct bitvec *bv, unsigned int num_bits)
 	return ui;
 }
 
+/*! \brief fill num_bits with \fill starting from the current position
+ * returns 0 on success, negative otherwise (out of vector boundary)
+ */
+int bitvec_fill(struct bitvec *bv, unsigned int num_bits, enum bit_value fill)
+{
+	unsigned i, stop = bv->cur_bit + num_bits;
+	for (i = bv->cur_bit; i < stop; i++)
+		if (bitvec_set_bit(bv, fill) < 0)
+			return -EINVAL;
+
+	return 0;
+}
+
 /*! \brief pad all remaining bits up to num_bits */
 int bitvec_spare_padding(struct bitvec *bv, unsigned int up_to_bit)
 {
-	unsigned int i;
-
-	for (i = bv->cur_bit; i <= up_to_bit; i++)
-		bitvec_set_bit(bv, L);
+	int n = up_to_bit - bv->cur_bit + 1;
+	if (n < 1)
+		return 0;
 
-	return 0;
+	return bitvec_fill(bv, n, L);
 }
 
 /*! \brief find first bit set in bit vector */
@@ -431,4 +459,98 @@ int bitvec_write_field(struct bitvec *bv, unsigned int write_index, uint64_t val
 	return 0;
 }
 
+/*! \brief convert enum to corresponding character */
+char bit_value_to_char(enum bit_value v)
+{
+	switch (v) {
+	case ZERO: return '0';
+	case ONE: return '1';
+	case L: return 'L';
+	case H: return 'H';
+	}
+	/* make compiler happy - "avoid control reaches end of non-void function" warning: */
+	return '?';
+}
+
+/*! \brief prints bit vector to provided string
+ * It's caller's responcibility to ensure that we won't shoot him in the foot.
+ */
+void bitvec_to_string_r(const struct bitvec *bv, char *str)
+{
+	unsigned i, pos = 0;
+	char *cur = str;
+	for (i = 0; i < bv->cur_bit; i++) {
+		if (0 == i % 8)
+			*cur++ = ' ';
+		*cur++ = bit_value_to_char(bitvec_get_bit_pos(bv, i));
+		pos++;
+	}
+	*cur = 0;
+}
+
+/* we assume that x have at least 1 non-b bit */
+static inline unsigned _leading_bits(uint8_t x, bool b)
+{
+	if (b) {
+		if (x < 0x80) return 0;
+		if (x < 0xC0) return 1;
+		if (x < 0xE0) return 2;
+		if (x < 0xF0) return 3;
+		if (x < 0xF8) return 4;
+		if (x < 0xFC) return 5;
+		if (x < 0xFE) return 6;
+	} else {
+		if (x > 0x7F) return 0;
+		if (x > 0x3F) return 1;
+		if (x > 0x1F) return 2;
+		if (x > 0xF) return 3;
+		if (x > 7) return 4;
+		if (x > 3) return 5;
+		if (x > 1) return 6;
+	}
+	return 7;
+}
+/*! \brief force bit vector to all 0 and current bit to the beginnig of the vector */
+void bitvec_zero(struct bitvec *bv)
+{
+	bv->cur_bit = 0;
+	memset(bv->data, 0, bv->data_len);
+}
+
+/*! \brief Return number (bits) of uninterrupted run of \b in \bv starting from the MSB */
+unsigned bitvec_rl(const struct bitvec *bv, bool b)
+{
+	unsigned i;
+	for (i = 0; i < (bv->cur_bit % 8 ? bv->cur_bit / 8 + 1 : bv->cur_bit / 8); i++) {
+		if ( (b ? 0xFF : 0) != bv->data[i])
+			return i * 8 + _leading_bits(bv->data[i], b);
+	}
+
+	return bv->cur_bit;
+}
+
+/*! \brief Shifts bitvec to the left, n MSB bits lost */
+void bitvec_shiftl(struct bitvec *bv, unsigned n)
+{
+	if (0 == n)
+		return;
+	if (n >= bv->cur_bit) {
+		bitvec_zero(bv);
+		return;
+	}
+
+	memmove(bv->data, bv->data + n / 8, bv->data_len - n / 8);
+
+	uint8_t tmp[2];
+	unsigned i;
+	for (i = 0; i < bv->data_len - 2; i++) {
+		uint16_t t = osmo_load16be(bv->data + i);
+		osmo_store16be(t << (n % 8), &tmp);
+		bv->data[i] = tmp[0];
+	}
+
+	bv->data[bv->data_len - 1] <<= (n % 8);
+	bv->cur_bit -= n;
+}
+
 /*! @} */
-- 
2.5.0

