src/bitvec.c

   1 /* bit vector utility routines */
   2
   3 /* (C) 2009 by Harald Welte <laforge@gnumonks.org>
   4  *
   5  * All Rights Reserved
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License along
  18  * with this program; if not, write to the Free Software Foundation, Inc.,
  19  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  20  *
  21  */
  22
  23
  24 #include <errno.h>
  25 #include <stdint.h>
  26
  27 #include <osmocore/bitvec.h>
  28
  29 #define BITNUM_FROM_COMP(byte, bit)     ((byte*8)+bit)
  30
  31 static inline unsigned int bytenum_from_bitnum(unsigned int bitnum)
  32 {
  33         unsigned int bytenum = bitnum / 8;
  34
  35         return bytenum;
  36 }
  37
  38 /* convert ZERO/ONE/L/H to a bitmask at given pos in a byte */
  39 static uint8_t bitval2mask(enum bit_value bit, uint8_t bitnum)
  40 {
  41         int bitval;
  42
  43         switch (bit) {
  44         case ZERO:
  45                 bitval = (0 << bitnum);
  46                 break;
  47         case ONE:
  48                 bitval = (1 << bitnum);
  49                 break;
  50         case L:
  51                 bitval = ((0x2b ^ (0 << bitnum)) & (1 << bitnum));
  52                 break;
  53         case H:
  54                 bitval = ((0x2b ^ (1 << bitnum)) & (1 << bitnum));
  55                 break;
  56         default:
  57                 return 0;
  58         }
  59         return bitval;
  60 }
  61
  62 /* check if the bit is 0 or 1 for a given position inside a bitvec */
  63 enum bit_value bitvec_get_bit_pos(struct bitvec *bv, unsigned int bitnr)
  64 {
  65         unsigned int bytenum = bytenum_from_bitnum(bitnr);
  66         unsigned int bitnum = 7 - (bitnr % 8);
  67         uint8_t bitval;
  68
  69         if (bytenum >= bv->data_len)
  70                 return -EINVAL;
  71
  72         bitval = bitval2mask(ONE, bitnum);
  73
  74         if (bv->data[bytenum] & bitval)
  75                 return ONE;
  76
  77         return ZERO;
  78 }
  79
  80 /* get the Nth set bit inside the bit vector */
  81 unsigned int bitvec_get_nth_set_bit(struct bitvec *bv, unsigned int n)
  82 {
  83         unsigned int i, k = 0;
  84
  85         for (i = 0; i < bv->data_len*8; i++) {
  86                 if (bitvec_get_bit_pos(bv, i) == ONE) {
  87                         k++;
  88                         if (k == n)
  89                                 return i;
  90                 }
  91         }
  92
  93         return 0;
  94 }
  95
  96 /* set the bit at a given position inside a bitvec */
  97 int bitvec_set_bit_pos(struct bitvec *bv, unsigned int bitnr,
  98                         enum bit_value bit)
  99 {
 100         unsigned int bytenum = bytenum_from_bitnum(bitnr);
 101         unsigned int bitnum = 7 - (bitnr % 8);
 102         uint8_t bitval;
 103
 104         if (bytenum >= bv->data_len)
 105                 return -EINVAL;
 106
 107         /* first clear the bit */
 108         bitval = bitval2mask(ONE, bitnum);
 109         bv->data[bytenum] &= ~bitval;
 110
 111         /* then set it to desired value */
 112         bitval = bitval2mask(bit, bitnum);
 113         bv->data[bytenum] |= bitval;
 114
 115         return 0;
 116 }
 117
 118 /* set the next bit inside a bitvec */
 119 int bitvec_set_bit(struct bitvec *bv, enum bit_value bit)
 120 {
 121         int rc;
 122
 123         rc = bitvec_set_bit_pos(bv, bv->cur_bit, bit);
 124         if (!rc)
 125                 bv->cur_bit++;
 126
 127         return rc;
 128 }
 129
 130 /* set multiple bits (based on array of bitvals) at current pos */
 131 int bitvec_set_bits(struct bitvec *bv, enum bit_value *bits, int count)
 132 {
 133         int i, rc;
 134
 135         for (i = 0; i < count; i++) {
 136                 rc = bitvec_set_bit(bv, bits[i]);
 137                 if (rc)
 138                         return rc;
 139         }
 140
 141         return 0;
 142 }
 143
 144 /* set multiple bits (based on numeric value) at current pos */
 145 int bitvec_set_uint(struct bitvec *bv, unsigned int ui, int num_bits)
 146 {
 147         int i, rc;
 148
 149         for (i = 0; i < num_bits; i++) {
 150                 int bit = 0;
 151                 if (ui & (1 << (num_bits - i - 1)))
 152                         bit = 1;
 153                 rc = bitvec_set_bit(bv, bit);
 154                 if (rc)
 155                         return rc;
 156         }
 157
 158         return 0;
 159 }
 160
 161 /* pad all remaining bits up to num_bits */
 162 int bitvec_spare_padding(struct bitvec *bv, unsigned int up_to_bit)
 163 {
 164         unsigned int i;
 165
 166         for (i = bv->cur_bit; i <= up_to_bit; i++)
 167                 bitvec_set_bit(bv, L);
 168
 169         return 0;
 170 }