drivers/net/bnxt/tf_core/bitalloc.c

   1 /* SPDX-License-Identifier: BSD-3-Clause
   2  * Copyright(c) 2019-2020 Broadcom
   3  * All rights reserved.
   4  */
   5
   6 #include "bitalloc.h"
   7
   8 #define BITALLOC_MAX_LEVELS 6
   9
  10 /* Finds the first bit set plus 1, equivalent to gcc __builtin_ffs */
  11 static int
  12 ba_ffs(bitalloc_word_t v)
  13 {
  14         int c; /* c will be the number of zero bits on the right plus 1 */
  15
  16         v &= -v;
  17         c = v ? 32 : 0;
  18
  19         if (v & 0x0000FFFF)
  20                 c -= 16;
  21         if (v & 0x00FF00FF)
  22                 c -= 8;
  23         if (v & 0x0F0F0F0F)
  24                 c -= 4;
  25         if (v & 0x33333333)
  26                 c -= 2;
  27         if (v & 0x55555555)
  28                 c -= 1;
  29
  30         return c;
  31 }
  32
  33 int
  34 ba_init(struct bitalloc *pool, int size)
  35 {
  36         bitalloc_word_t *mem = (bitalloc_word_t *)pool;
  37         int       i;
  38
  39         /* Initialize */
  40         pool->size = 0;
  41
  42         if (size < 1 || size > BITALLOC_MAX_SIZE)
  43                 return -1;
  44
  45         /* Zero structure */
  46         for (i = 0;
  47              i < (int)(BITALLOC_SIZEOF(size) / sizeof(bitalloc_word_t));
  48              i++)
  49                 mem[i] = 0;
  50
  51         /* Initialize */
  52         pool->size = size;
  53
  54         /* Embed number of words of next level, after each level */
  55         int words[BITALLOC_MAX_LEVELS];
  56         int lev = 0;
  57         int offset = 0;
  58
  59         words[0] = (size + 31) / 32;
  60         while (words[lev] > 1) {
  61                 lev++;
  62                 words[lev] = (words[lev - 1] + 31) / 32;
  63         }
  64
  65         while (lev) {
  66                 offset += words[lev];
  67                 pool->storage[offset++] = words[--lev];
  68         }
  69
  70         /* Free the entire pool */
  71         for (i = 0; i < size; i++)
  72                 ba_free(pool, i);
  73
  74         return 0;
  75 }
  76
  77 static int
  78 ba_alloc_helper(struct bitalloc *pool,
  79                 int              offset,
  80                 int              words,
  81                 unsigned int     size,
  82                 int              index,
  83                 int             *clear)
  84 {
  85         bitalloc_word_t *storage = &pool->storage[offset];
  86         int       loc = ba_ffs(storage[index]);
  87         int       r;
  88
  89         if (loc == 0)
  90                 return -1;
  91
  92         loc--;
  93
  94         if (pool->size > size) {
  95                 r = ba_alloc_helper(pool,
  96                                     offset + words + 1,
  97                                     storage[words],
  98                                     size * 32,
  99                                     index * 32 + loc,
 100                                     clear);
 101         } else {
 102                 r = index * 32 + loc;
 103                 *clear = 1;
 104                 pool->free_count--;
 105         }
 106
 107         if (*clear) {
 108                 storage[index] &= ~(1 << loc);
 109                 *clear = (storage[index] == 0);
 110         }
 111
 112         return r;
 113 }
 114
 115 int
 116 ba_alloc(struct bitalloc *pool)
 117 {
 118         int clear = 0;
 119
 120         return ba_alloc_helper(pool, 0, 1, 32, 0, &clear);
 121 }
 122
 123 static int
 124 ba_alloc_index_helper(struct bitalloc *pool,
 125                       int              offset,
 126                       int              words,
 127                       unsigned int     size,
 128                       int             *index,
 129                       int             *clear)
 130 {
 131         bitalloc_word_t *storage = &pool->storage[offset];
 132         int       loc;
 133         int       r;
 134
 135         if (pool->size > size)
 136                 r = ba_alloc_index_helper(pool,
 137                                           offset + words + 1,
 138                                           storage[words],
 139                                           size * 32,
 140                                           index,
 141                                           clear);
 142         else
 143                 r = 1; /* Check if already allocated */
 144
 145         loc = (*index % 32);
 146         *index = *index / 32;
 147
 148         if (r == 1) {
 149                 r = (storage[*index] & (1 << loc)) ? 0 : -1;
 150                 if (r == 0) {
 151                         *clear = 1;
 152                         pool->free_count--;
 153                 }
 154         }
 155
 156         if (*clear) {
 157                 storage[*index] &= ~(1 << loc);
 158                 *clear = (storage[*index] == 0);
 159         }
 160
 161         return r;
 162 }
 163
 164 int
 165 ba_alloc_index(struct bitalloc *pool, int index)
 166 {
 167         int clear = 0;
 168         int index_copy = index;
 169
 170         if (index < 0 || index >= (int)pool->size)
 171                 return -1;
 172
 173         if (ba_alloc_index_helper(pool, 0, 1, 32, &index_copy, &clear) >= 0)
 174                 return index;
 175         else
 176                 return -1;
 177 }
 178
 179 static int
 180 ba_inuse_helper(struct bitalloc *pool,
 181                 int              offset,
 182                 int              words,
 183                 unsigned int     size,
 184                 int             *index)
 185 {
 186         bitalloc_word_t *storage = &pool->storage[offset];
 187         int       loc;
 188         int       r;
 189
 190         if (pool->size > size)
 191                 r = ba_inuse_helper(pool,
 192                                     offset + words + 1,
 193                                     storage[words],
 194                                     size * 32,
 195                                     index);
 196         else
 197                 r = 1; /* Check if in use */
 198
 199         loc = (*index % 32);
 200         *index = *index / 32;
 201
 202         if (r == 1)
 203                 r = (storage[*index] & (1 << loc)) ? -1 : 0;
 204
 205         return r;
 206 }
 207
 208 int
 209 ba_inuse(struct bitalloc *pool, int index)
 210 {
 211         if (index < 0 || index >= (int)pool->size)
 212                 return -1;
 213
 214         return ba_inuse_helper(pool, 0, 1, 32, &index) == 0;
 215 }
 216
 217 static int
 218 ba_free_helper(struct bitalloc *pool,
 219                int              offset,
 220                int              words,
 221                unsigned int     size,
 222                int             *index)
 223 {
 224         bitalloc_word_t *storage = &pool->storage[offset];
 225         int       loc;
 226         int       r;
 227
 228         if (pool->size > size)
 229                 r = ba_free_helper(pool,
 230                                    offset + words + 1,
 231                                    storage[words],
 232                                    size * 32,
 233                                    index);
 234         else
 235                 r = 1; /* Check if already free */
 236
 237         loc = (*index % 32);
 238         *index = *index / 32;
 239
 240         if (r == 1) {
 241                 r = (storage[*index] & (1 << loc)) ? -1 : 0;
 242                 if (r == 0)
 243                         pool->free_count++;
 244         }
 245
 246         if (r == 0)
 247                 storage[*index] |= (1 << loc);
 248
 249         return r;
 250 }
 251
 252 int
 253 ba_free(struct bitalloc *pool, int index)
 254 {
 255         if (index < 0 || index >= (int)pool->size)
 256                 return -1;
 257
 258         return ba_free_helper(pool, 0, 1, 32, &index);
 259 }
 260
 261 int
 262 ba_inuse_free(struct bitalloc *pool, int index)
 263 {
 264         if (index < 0 || index >= (int)pool->size)
 265                 return -1;
 266
 267         return ba_free_helper(pool, 0, 1, 32, &index) + 1;
 268 }
 269
 270 int
 271 ba_free_count(struct bitalloc *pool)
 272 {
 273         return (int)pool->free_count;
 274 }
 275
 276 int ba_inuse_count(struct bitalloc *pool)
 277 {
 278         return (int)(pool->size) - (int)(pool->free_count);
 279 }
 280
 281 static int
 282 ba_find_next_helper(struct bitalloc *pool,
 283                     int              offset,
 284                     int              words,
 285                     unsigned int     size,
 286                     int             *index,
 287                     int              free)
 288 {
 289         bitalloc_word_t *storage = &pool->storage[offset];
 290         int       loc, r, bottom = 0;
 291
 292         if (pool->size > size)
 293                 r = ba_find_next_helper(pool,
 294                                         offset + words + 1,
 295                                         storage[words],
 296                                         size * 32,
 297                                         index,
 298                                         free);
 299         else
 300                 bottom = 1; /* Bottom of tree */
 301
 302         loc = (*index % 32);
 303         *index = *index / 32;
 304
 305         if (bottom) {
 306                 int bit_index = *index * 32;
 307
 308                 loc = ba_ffs(~storage[*index] & ((bitalloc_word_t)-1 << loc));
 309                 if (loc > 0) {
 310                         loc--;
 311                         r = (bit_index + loc);
 312                         if (r >= (int)pool->size)
 313                                 r = -1;
 314                 } else {
 315                         /* Loop over array at bottom of tree */
 316                         r = -1;
 317                         bit_index += 32;
 318                         *index = *index + 1;
 319                         while ((int)pool->size > bit_index) {
 320                                 loc = ba_ffs(~storage[*index]);
 321
 322                                 if (loc > 0) {
 323                                         loc--;
 324                                         r = (bit_index + loc);
 325                                         if (r >= (int)pool->size)
 326                                                 r = -1;
 327                                         break;
 328                                 }
 329                                 bit_index += 32;
 330                                 *index = *index + 1;
 331                         }
 332                 }
 333         }
 334
 335         if (r >= 0 && (free)) {
 336                 if (bottom)
 337                         pool->free_count++;
 338                 storage[*index] |= (1 << loc);
 339         }
 340
 341         return r;
 342 }
 343
 344 int
 345 ba_find_next_inuse(struct bitalloc *pool, int index)
 346 {
 347         if (index < 0 ||
 348             index >= (int)pool->size ||
 349             pool->free_count == pool->size)
 350                 return -1;
 351
 352         return ba_find_next_helper(pool, 0, 1, 32, &index, 0);
 353 }
 354
 355 int
 356 ba_find_next_inuse_free(struct bitalloc *pool, int index)
 357 {
 358         if (index < 0 ||
 359             index >= (int)pool->size ||
 360             pool->free_count == pool->size)
 361                 return -1;
 362
 363         return ba_find_next_helper(pool, 0, 1, 32, &index, 1);
 364 }