sys/dev/misc/evdev/freebsd-bitstring.h

   1 /*-
   2  * Copyright (c) 1989, 1993
   3  *      The Regents of the University of California.  All rights reserved.
   4  *
   5  * This code is derived from software contributed to Berkeley by
   6  * Paul Vixie.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  * 3. Neither the name of the University nor the names of its contributors
  17  *    may be used to endorse or promote products derived from this software
  18  *    without specific prior written permission.
  19  *
  20  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
  21  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  23  * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
  24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  26  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  27  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  29  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  30  * SUCH DAMAGE.
  31  *
  32  * Copyright (c) 2014 Spectra Logic Corporation
  33  * All rights reserved.
  34  *
  35  * Redistribution and use in source and binary forms, with or without
  36  * modification, are permitted provided that the following conditions
  37  * are met:
  38  * 1. Redistributions of source code must retain the above copyright
  39  *    notice, this list of conditions, and the following disclaimer,
  40  *    without modification.
  41  * 2. Redistributions in binary form must reproduce at minimum a disclaimer
  42  *    substantially similar to the "NO WARRANTY" disclaimer below
  43  *    ("Disclaimer") and any redistribution must be conditioned upon
  44  *    including a substantially similar Disclaimer requirement for further
  45  *    binary redistribution.
  46  *
  47  * NO WARRANTY
  48  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  49  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  50  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR
  51  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  52  * HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  53  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  54  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  55  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
  56  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
  57  * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  58  * POSSIBILITY OF SUCH DAMAGES.
  59  *
  60  * $FreeBSD$
  61  */
  62 #ifndef _SYS_FREEBSD_BITSTRING_H_
  63 #define _SYS_FREEBSD_BITSTRING_H_
  64
  65 #include <sys/types.h>
  66
  67 /*
  68  * Begin: Ad hoc definitions from FreeBSD sys/sys/types.h
  69  */
  70
  71 /*
  72  * Population count algorithm using SWAR approach
  73  * - "SIMD Within A Register".
  74  */
  75 static __inline __uint16_t
  76 __bitcount16(__uint16_t _x)
  77 {
  78
  79         _x = (_x & 0x5555) + ((_x & 0xaaaa) >> 1);
  80         _x = (_x & 0x3333) + ((_x & 0xcccc) >> 2);
  81         _x = (_x + (_x >> 4)) & 0x0f0f;
  82         _x = (_x + (_x >> 8)) & 0x00ff;
  83         return (_x);
  84 }
  85
  86 static __inline __uint32_t
  87 __bitcount32(__uint32_t _x)
  88 {
  89
  90         _x = (_x & 0x55555555) + ((_x & 0xaaaaaaaa) >> 1);
  91         _x = (_x & 0x33333333) + ((_x & 0xcccccccc) >> 2);
  92         _x = (_x + (_x >> 4)) & 0x0f0f0f0f;
  93         _x = (_x + (_x >> 8));
  94         _x = (_x + (_x >> 16)) & 0x000000ff;
  95         return (_x);
  96 }
  97
  98 #ifdef __LP64__
  99 static __inline __uint64_t
 100 __bitcount64(__uint64_t _x)
 101 {
 102
 103         _x = (_x & 0x5555555555555555) + ((_x & 0xaaaaaaaaaaaaaaaa) >> 1);
 104         _x = (_x & 0x3333333333333333) + ((_x & 0xcccccccccccccccc) >> 2);
 105         _x = (_x + (_x >> 4)) & 0x0f0f0f0f0f0f0f0f;
 106         _x = (_x + (_x >> 8));
 107         _x = (_x + (_x >> 16));
 108         _x = (_x + (_x >> 32)) & 0x000000ff;
 109         return (_x);
 110 }
 111
 112 #define __bitcountl(x)  __bitcount64((unsigned long)(x))
 113 #else
 114 static __inline __uint64_t
 115 __bitcount64(__uint64_t _x)
 116 {
 117
 118         return (__bitcount32(_x >> 32) + __bitcount32(_x));
 119 }
 120
 121 #define __bitcountl(x)  __bitcount32((unsigned long)(x))
 122 #endif
 123 #define __bitcount(x)   __bitcount32((unsigned int)(x))
 124 /*
 125  * End: Ad hoc definitions from FreeBSD sys/sys/types.h
 126  */
 127
 128 typedef unsigned long bitstr_t;
 129
 130 /*---------------------- Private Implementation Details ----------------------*/
 131 #define _BITSTR_MASK (~0UL)
 132 #define _BITSTR_BITS (sizeof(bitstr_t) * 8)
 133
 134 #ifdef roundup2
 135 #define        _bit_roundup2 roundup2
 136 #else
 137 #define        _bit_roundup2(x, y)        (((x)+((y)-1))&(~((y)-1))) /* if y is powers of two */
 138 #endif
 139
 140 /* bitstr_t in bit string containing the bit. */
 141 static inline int
 142 _bit_idx(int _bit)
 143 {
 144         return (_bit / _BITSTR_BITS);
 145 }
 146
 147 /* bit number within bitstr_t at _bit_idx(_bit). */
 148 static inline int
 149 _bit_offset(int _bit)
 150 {
 151         return (_bit % _BITSTR_BITS);
 152 }
 153
 154 /* Mask for the bit within its long. */
 155 static inline bitstr_t
 156 _bit_mask(int _bit)
 157 {
 158         return (1UL << _bit_offset(_bit));
 159 }
 160
 161 static inline bitstr_t
 162 _bit_make_mask(int _start, int _stop)
 163 {
 164         return ((_BITSTR_MASK << _bit_offset(_start)) &
 165             (_BITSTR_MASK >> (_BITSTR_BITS - _bit_offset(_stop) - 1)));
 166 }
 167
 168 /*----------------------------- Public Interface -----------------------------*/
 169 /* Number of bytes allocated for a bit string of nbits bits */
 170 #define bitstr_size(_nbits) (_bit_roundup2(_nbits, _BITSTR_BITS) / 8)
 171
 172 /* Allocate a bit string initialized with no bits set. */
 173 #ifdef _KERNEL
 174 static inline bitstr_t *
 175 bit_alloc(int _nbits, struct malloc_type *type, int flags)
 176 {
 177         return ((bitstr_t *)kmalloc(bitstr_size(_nbits), type, flags | M_ZERO));
 178 }
 179 #else
 180 static inline bitstr_t *
 181 bit_alloc(int _nbits)
 182 {
 183         return ((bitstr_t *)calloc(bitstr_size(_nbits), 1));
 184 }
 185 #endif
 186
 187 /* Allocate a bit string on the stack */
 188 #define bit_decl(name, nbits) \
 189         ((name)[bitstr_size(nbits) / sizeof(bitstr_t)])
 190
 191 /* Is bit N of bit string set? */
 192 static inline int
 193 bit_test(const bitstr_t *_bitstr, int _bit)
 194 {
 195         return ((_bitstr[_bit_idx(_bit)] & _bit_mask(_bit)) != 0);
 196 }
 197
 198 /* Set bit N of bit string. */
 199 static inline void
 200 bit_set(bitstr_t *_bitstr, int _bit)
 201 {
 202         _bitstr[_bit_idx(_bit)] |= _bit_mask(_bit);
 203 }
 204
 205 /* clear bit N of bit string name */
 206 static inline void
 207 bit_clear(bitstr_t *_bitstr, int _bit)
 208 {
 209         _bitstr[_bit_idx(_bit)] &= ~_bit_mask(_bit);
 210 }
 211
 212 /* Set bits start ... stop inclusive in bit string. */
 213 static inline void
 214 bit_nset(bitstr_t *_bitstr, int _start, int _stop)
 215 {
 216         bitstr_t *_stopbitstr;
 217
 218         _stopbitstr = _bitstr + _bit_idx(_stop);
 219         _bitstr += _bit_idx(_start);
 220
 221         if (_bitstr == _stopbitstr) {
 222                 *_bitstr |= _bit_make_mask(_start, _stop);
 223         } else {
 224                 *_bitstr |= _bit_make_mask(_start, _BITSTR_BITS - 1);
 225                 while (++_bitstr < _stopbitstr)
 226                         *_bitstr = _BITSTR_MASK;
 227                 *_stopbitstr |= _bit_make_mask(0, _stop);
 228         }
 229 }
 230
 231 /* Clear bits start ... stop inclusive in bit string. */
 232 static inline void
 233 bit_nclear(bitstr_t *_bitstr, int _start, int _stop)
 234 {
 235         bitstr_t *_stopbitstr;
 236
 237         _stopbitstr = _bitstr + _bit_idx(_stop);
 238         _bitstr += _bit_idx(_start);
 239
 240         if (_bitstr == _stopbitstr) {
 241                 *_bitstr &= ~_bit_make_mask(_start, _stop);
 242         } else {
 243                 *_bitstr &= ~_bit_make_mask(_start, _BITSTR_BITS - 1);
 244                 while (++_bitstr < _stopbitstr)
 245                         *_bitstr = 0;
 246                 *_stopbitstr &= ~_bit_make_mask(0, _stop);
 247         }
 248 }
 249
 250 /* Find the first bit set in bit string at or after bit start. */
 251 static inline void
 252 bit_ffs_at(bitstr_t *_bitstr, int _start, int _nbits, int *_result)
 253 {
 254         bitstr_t *_curbitstr;
 255         bitstr_t *_stopbitstr;
 256         bitstr_t _test;
 257         int _value, _offset;
 258
 259         if (_nbits > 0) {
 260                 _curbitstr = _bitstr + _bit_idx(_start);
 261                 _stopbitstr = _bitstr + _bit_idx(_nbits - 1);
 262
 263                 _test = *_curbitstr;
 264                 if (_bit_offset(_start) != 0)
 265                         _test &= _bit_make_mask(_start, _BITSTR_BITS - 1);
 266                 while (_test == 0 && _curbitstr < _stopbitstr)
 267                         _test = *(++_curbitstr);
 268
 269                 _offset = ffsl(_test);
 270                 _value = ((_curbitstr - _bitstr) * _BITSTR_BITS) + _offset - 1;
 271                 if (_offset == 0 || _value >= _nbits)
 272                         _value = -1;
 273         } else {
 274                 _value = -1;
 275         }
 276         *_result = _value;
 277 }
 278
 279 /* Find the first bit clear in bit string at or after bit start. */
 280 static inline void
 281 bit_ffc_at(bitstr_t *_bitstr, int _start, int _nbits, int *_result)
 282 {
 283         bitstr_t *_curbitstr;
 284         bitstr_t *_stopbitstr;
 285         bitstr_t _test;
 286         int _value, _offset;
 287
 288         if (_nbits > 0) {
 289                 _curbitstr = _bitstr + _bit_idx(_start);
 290                 _stopbitstr = _bitstr + _bit_idx(_nbits - 1);
 291
 292                 _test = *_curbitstr;
 293                 if (_bit_offset(_start) != 0)
 294                         _test |= _bit_make_mask(0, _start - 1);
 295                 while (_test == _BITSTR_MASK && _curbitstr < _stopbitstr)
 296                         _test = *(++_curbitstr);
 297
 298                 _offset = ffsl(~_test);
 299                 _value = ((_curbitstr - _bitstr) * _BITSTR_BITS) + _offset - 1;
 300                 if (_offset == 0 || _value >= _nbits)
 301                         _value = -1;
 302         } else {
 303                 _value = -1;
 304         }
 305         *_result = _value;
 306 }
 307
 308 /* Find the first bit set in bit string. */
 309 static inline void
 310 bit_ffs(bitstr_t *_bitstr, int _nbits, int *_result)
 311 {
 312         bit_ffs_at(_bitstr, /*start*/0, _nbits, _result);
 313 }
 314
 315 /* Find the first bit clear in bit string. */
 316 static inline void
 317 bit_ffc(bitstr_t *_bitstr, int _nbits, int *_result)
 318 {
 319         bit_ffc_at(_bitstr, /*start*/0, _nbits, _result);
 320 }
 321
 322 /* Count the number of bits set in a bitstr of size _nbits at or after _start */
 323 static inline void
 324 bit_count(bitstr_t *_bitstr, int _start, int _nbits, int *_result)
 325 {
 326         bitstr_t *_curbitstr, mask;
 327         int _value = 0, curbitstr_len;
 328
 329         if (_start >= _nbits)
 330                 goto out;
 331
 332         _curbitstr = _bitstr + _bit_idx(_start);
 333         _nbits -= _BITSTR_BITS * _bit_idx(_start);
 334         _start -= _BITSTR_BITS * _bit_idx(_start);
 335
 336         if (_start > 0) {
 337                 curbitstr_len = (int)_BITSTR_BITS < _nbits ?
 338                                 (int)_BITSTR_BITS : _nbits;
 339                 mask = _bit_make_mask(_start, _bit_offset(curbitstr_len - 1));
 340                 _value += __bitcountl(*_curbitstr & mask);
 341                 _curbitstr++;
 342                 _nbits -= _BITSTR_BITS;
 343         }
 344         while (_nbits >= (int)_BITSTR_BITS) {
 345                 _value += __bitcountl(*_curbitstr);
 346                 _curbitstr++;
 347                 _nbits -= _BITSTR_BITS;
 348         }
 349         if (_nbits > 0) {
 350                 mask = _bit_make_mask(0, _bit_offset(_nbits - 1));
 351                 _value += __bitcountl(*_curbitstr & mask);
 352         }
 353
 354 out:
 355         *_result = _value;
 356 }
 357
 358 #endif  /* _SYS_FREEBSD_BITSTRING_H_ */