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

From: Max &lt;msuraev(a)sysmocom.de&gt; Sponsored-by: On-Waves ehf --- include/osmocom/core/bitvec.h | 8 ++++ src/bitvec.c | 92 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 100 insertions(+) diff --git a/include/osmocom/core/bitvec.h b/include/osmocom/core/bitvec.h index e03b774..ef65f9e 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 &lt;laforge(a)gnumonks.org&gt; + * (C) 2012 Ivan Klyuchnikov * (C) 2015 by Sysmocom s.f.m.c. GmbH * * All Rights Reserved @@ -74,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); +void bitvec_free(struct bitvec *bv); +int bitvec_unhex(struct bitvec *bv, const char* src); +unsigned int bitvec_pack(struct bitvec *bv, uint8_t *buffer); +unsigned int bitvec_unpack(struct bitvec *bv, 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..68b8d98 100644 --- a/src/bitvec.c +++ b/src/bitvec.c @@ -1,6 +1,7 @@ /* bit vector utility routines */ /* (C) 2009 by Harald Welte &lt;laforge(a)gnumonks.org&gt; + * (C) 2012 Ivan Klyuchnikov * (C) 2015 by Sysmocom s.f.m.c. GmbH * * All Rights Reserved @@ -32,11 +33,15 @@ #include <errno.h> #include <stdint.h> #include <string.h> +#include <stdio.h> +#include <talloc.h> #include <osmocom/core/bitvec.h> #define BITNUM_FROM_COMP(byte, bit) ((byte*8)+bit) +void *bv_tall_ctx; + static inline unsigned int bytenum_from_bitnum(unsigned int bitnum) { unsigned int bytenum = bitnum / 8; @@ -336,4 +341,91 @@ 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 size) +{ + struct bitvec *bv = talloc_zero(bv_tall_ctx, struct bitvec); + bv->data_len = size; + bv->cur_bit = 0; + bv->data = talloc_zero_array(bv_tall_ctx, uint8_t, size); + return bv; +} + +void bitvec_free(struct bitvec *bv) +{ + talloc_free(bv->data); + talloc_free(bv); +} + +unsigned int bitvec_pack(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, 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