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 <osmocom/core/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(const 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 /* check if the bit is L or H for a given position inside a bitvec */
  81 enum bit_value bitvec_get_bit_pos_high(const struct bitvec *bv,
  82                                         unsigned int bitnr)
  83 {
  84         unsigned int bytenum = bytenum_from_bitnum(bitnr);
  85         unsigned int bitnum = 7 - (bitnr % 8);
  86         uint8_t bitval;
  87
  88         if (bytenum >= bv->data_len)
  89                 return -EINVAL;
  90
  91         bitval = bitval2mask(H, bitnum);
  92
  93         if ((bv->data[bytenum] & (1 << bitnum)) == bitval)
  94                 return H;
  95
  96         return L;
  97 }
  98
  99 /* get the Nth set bit inside the bit vector */
 100 unsigned int bitvec_get_nth_set_bit(const struct bitvec *bv, unsigned int n)
 101 {
 102         unsigned int i, k = 0;
 103
 104         for (i = 0; i < bv->data_len*8; i++) {
 105                 if (bitvec_get_bit_pos(bv, i) == ONE) {
 106                         k++;
 107                         if (k == n)
 108                                 return i;
 109                 }
 110         }
 111
 112         return 0;
 113 }
 114
 115 /* set the bit at a given position inside a bitvec */
 116 int bitvec_set_bit_pos(struct bitvec *bv, unsigned int bitnr,
 117                         enum bit_value bit)
 118 {
 119         unsigned int bytenum = bytenum_from_bitnum(bitnr);
 120         unsigned int bitnum = 7 - (bitnr % 8);
 121         uint8_t bitval;
 122
 123         if (bytenum >= bv->data_len)
 124                 return -EINVAL;
 125
 126         /* first clear the bit */
 127         bitval = bitval2mask(ONE, bitnum);
 128         bv->data[bytenum] &= ~bitval;
 129
 130         /* then set it to desired value */
 131         bitval = bitval2mask(bit, bitnum);
 132         bv->data[bytenum] |= bitval;
 133
 134         return 0;
 135 }
 136
 137 /* set the next bit inside a bitvec */
 138 int bitvec_set_bit(struct bitvec *bv, enum bit_value bit)
 139 {
 140         int rc;
 141
 142         rc = bitvec_set_bit_pos(bv, bv->cur_bit, bit);
 143         if (!rc)
 144                 bv->cur_bit++;
 145
 146         return rc;
 147 }
 148
 149 /* get the next bit (low/high) inside a bitvec */
 150 int bitvec_get_bit_high(struct bitvec *bv)
 151 {
 152         int rc;
 153
 154         rc = bitvec_get_bit_pos_high(bv, bv->cur_bit);
 155         if (rc >= 0)
 156                 bv->cur_bit++;
 157
 158         return rc;
 159 }
 160
 161 /* set multiple bits (based on array of bitvals) at current pos */
 162 int bitvec_set_bits(struct bitvec *bv, enum bit_value *bits, int count)
 163 {
 164         int i, rc;
 165
 166         for (i = 0; i < count; i++) {
 167                 rc = bitvec_set_bit(bv, bits[i]);
 168                 if (rc)
 169                         return rc;
 170         }
 171
 172         return 0;
 173 }
 174
 175 /* set multiple bits (based on numeric value) at current pos */
 176 int bitvec_set_uint(struct bitvec *bv, unsigned int ui, int num_bits)
 177 {
 178         int i, rc;
 179
 180         for (i = 0; i < num_bits; i++) {
 181                 int bit = 0;
 182                 if (ui & (1 << (num_bits - i - 1)))
 183                         bit = 1;
 184                 rc = bitvec_set_bit(bv, bit);
 185                 if (rc)
 186                         return rc;
 187         }
 188
 189         return 0;
 190 }
 191
 192 /* get multiple bits (based on numeric value) from current pos */
 193 int bitvec_get_uint(struct bitvec *bv, int num_bits)
 194 {
 195         int i;
 196         unsigned int ui = 0;
 197
 198         for (i = 0; i < num_bits; i++) {
 199                 int bit = bitvec_get_bit_pos(bv, bv->cur_bit);
 200                 if (bit < 0)
 201                         return bit;
 202                 if (bit)
 203                         ui |= (1 << (num_bits - i - 1));
 204                 bv->cur_bit++;
 205         }
 206
 207         return ui;
 208 }
 209
 210 /* pad all remaining bits up to num_bits */
 211 int bitvec_spare_padding(struct bitvec *bv, unsigned int up_to_bit)
 212 {
 213         unsigned int i;
 214
 215         for (i = bv->cur_bit; i <= up_to_bit; i++)
 216                 bitvec_set_bit(bv, L);
 217
 218         return 0;
 219 }
 220
 221 /* find first bit set in bit vector */
 222 int bitvec_find_bit_pos(const struct bitvec *bv, unsigned int n,
 223                         enum bit_value val)
 224 {
 225         unsigned int i;
 226
 227         for (i = n; i < bv->data_len*8; i++) {
 228                 if (bitvec_get_bit_pos(bv, i) == val)
 229                         return i;
 230         }
 231
 232         return -1;
 233 }