drivers/net/bnxt/tf_core/bitalloc.c

   1 /* SPDX-License-Identifier: BSD-3-Clause
   2  * Copyright(c) 2019-2021 Broadcom
   3  * All rights reserved.
   4  */
   5
   6 #include "bitalloc.h"
   7
   8 #define BITALLOC_MAX_LEVELS 6
   9
  10
  11 /* Finds the last bit set plus 1, equivalent to gcc __builtin_fls */
  12 static int
  13 ba_fls(bitalloc_word_t v)
  14 {
  15         int c = 32;
  16
  17         if (!v)
  18                 return 0;
  19
  20         if (!(v & 0xFFFF0000u)) {
  21                 v <<= 16;
  22                 c -= 16;
  23         }
  24         if (!(v & 0xFF000000u)) {
  25                 v <<= 8;
  26                 c -= 8;
  27         }
  28         if (!(v & 0xF0000000u)) {
  29                 v <<= 4;
  30                 c -= 4;
  31         }
  32         if (!(v & 0xC0000000u)) {
  33                 v <<= 2;
  34                 c -= 2;
  35         }
  36         if (!(v & 0x80000000u)) {
  37                 v <<= 1;
  38                 c -= 1;
  39         }
  40
  41         return c;
  42 }
  43
  44 /* Finds the first bit set plus 1, equivalent to gcc __builtin_ffs */
  45 static int
  46 ba_ffs(bitalloc_word_t v)
  47 {
  48         int c; /* c will be the number of zero bits on the right plus 1 */
  49
  50         v &= -v;
  51         c = v ? 32 : 0;
  52
  53         if (v & 0x0000FFFF)
  54                 c -= 16;
  55         if (v & 0x00FF00FF)
  56                 c -= 8;
  57         if (v & 0x0F0F0F0F)
  58                 c -= 4;
  59         if (v & 0x33333333)
  60                 c -= 2;
  61         if (v & 0x55555555)
  62                 c -= 1;
  63
  64         return c;
  65 }
  66
  67 int
  68 ba_init(struct bitalloc *pool, int size, bool free)
  69 {
  70         bitalloc_word_t *mem = (bitalloc_word_t *)pool;
  71         int       i;
  72
  73         /* Initialize */
  74         pool->size = 0;
  75
  76         if (size < 1 || size > BITALLOC_MAX_SIZE)
  77                 return -1;
  78
  79         /* Zero structure */
  80         for (i = 0;
  81              i < (int)(BITALLOC_SIZEOF(size) / sizeof(bitalloc_word_t));
  82              i++)
  83                 mem[i] = 0;
  84
  85         /* Initialize */
  86         pool->size = size;
  87
  88         /* Embed number of words of next level, after each level */
  89         int words[BITALLOC_MAX_LEVELS];
  90         int lev = 0;
  91         int offset = 0;
  92
  93         words[0] = (size + 31) / 32;
  94         while (words[lev] > 1) {
  95                 lev++;
  96                 words[lev] = (words[lev - 1] + 31) / 32;
  97         }
  98
  99         while (lev) {
 100                 offset += words[lev];
 101                 pool->storage[offset++] = words[--lev];
 102         }
 103
 104         /* Free the entire pool if it is required*/
 105         if (free) {
 106                 for (i = 0; i < size; i++)
 107                         ba_free(pool, i);
 108         }
 109
 110         return 0;
 111 }
 112
 113 static int
 114 ba_alloc_helper(struct bitalloc *pool,
 115                 int              offset,
 116                 int              words,
 117                 unsigned int     size,
 118                 int              index,
 119                 int             *clear)
 120 {
 121         bitalloc_word_t *storage = &pool->storage[offset];
 122         int       loc = ba_ffs(storage[index]);
 123         int       r;
 124
 125         if (loc == 0)
 126                 return -1;
 127
 128         loc--;
 129
 130         if (pool->size > size) {
 131                 r = ba_alloc_helper(pool,
 132                                     offset + words + 1,
 133                                     storage[words],
 134                                     size * 32,
 135                                     index * 32 + loc,
 136                                     clear);
 137         } else {
 138                 r = index * 32 + loc;
 139                 *clear = 1;
 140                 pool->free_count--;
 141         }
 142
 143         if (*clear) {
 144                 storage[index] &= ~(1 << loc);
 145                 *clear = (storage[index] == 0);
 146         }
 147
 148         return r;
 149 }
 150
 151 int
 152 ba_alloc(struct bitalloc *pool)
 153 {
 154         int clear = 0;
 155
 156         return ba_alloc_helper(pool, 0, 1, 32, 0, &clear);
 157 }
 158
 159 /**
 160  * Help function to alloc entry from highest available index
 161  *
 162  * Searching the pool from highest index for the empty entry.
 163  *
 164  * [in] pool
 165  *   Pointer to the resource pool
 166  *
 167  * [in] offset
 168  *   Offset of the storage in the pool
 169  *
 170  * [in] words
 171  *   Number of words in this level
 172  *
 173  * [in] size
 174  *   Number of entries in this level
 175  *
 176  * [in] index
 177  *   Index of words that has the entry
 178  *
 179  * [in] clear
 180  *   Indicate if a bit needs to be clear due to the entry is allocated
 181  *
 182  * Returns:
 183  *     0 - Success
 184  *    -1 - Failure
 185  */
 186 static int
 187 ba_alloc_reverse_helper(struct bitalloc *pool,
 188                         int offset,
 189                         int words,
 190                         unsigned int size,
 191                         int index,
 192                         int *clear)
 193 {
 194         bitalloc_word_t *storage = &pool->storage[offset];
 195         int loc = ba_fls(storage[index]);
 196         int r;
 197
 198         if (loc == 0)
 199                 return -1;
 200
 201         loc--;
 202
 203         if (pool->size > size) {
 204                 r = ba_alloc_reverse_helper(pool,
 205                                             offset + words + 1,
 206                                             storage[words],
 207                                             size * 32,
 208                                             index * 32 + loc,
 209                                             clear);
 210         } else {
 211                 r = index * 32 + loc;
 212                 *clear = 1;
 213                 pool->free_count--;
 214         }
 215
 216         if (*clear) {
 217                 storage[index] &= ~(1 << loc);
 218                 *clear = (storage[index] == 0);
 219         }
 220
 221         return r;
 222 }
 223
 224 int
 225 ba_alloc_reverse(struct bitalloc *pool)
 226 {
 227         int clear = 0;
 228
 229         return ba_alloc_reverse_helper(pool, 0, 1, 32, 0, &clear);
 230 }
 231
 232 static int
 233 ba_alloc_index_helper(struct bitalloc *pool,
 234                       int              offset,
 235                       int              words,
 236                       unsigned int     size,
 237                       int             *index,
 238                       int             *clear)
 239 {
 240         bitalloc_word_t *storage = &pool->storage[offset];
 241         int       loc;
 242         int       r;
 243
 244         if (pool->size > size)
 245                 r = ba_alloc_index_helper(pool,
 246                                           offset + words + 1,
 247                                           storage[words],
 248                                           size * 32,
 249                                           index,
 250                                           clear);
 251         else
 252                 r = 1; /* Check if already allocated */
 253
 254         loc = (*index % 32);
 255         *index = *index / 32;
 256
 257         if (r == 1) {
 258                 r = (storage[*index] & (1 << loc)) ? 0 : -1;
 259                 if (r == 0) {
 260                         *clear = 1;
 261                         pool->free_count--;
 262                 }
 263         }
 264
 265         if (*clear) {
 266                 storage[*index] &= ~(1 << loc);
 267                 *clear = (storage[*index] == 0);
 268         }
 269
 270         return r;
 271 }
 272
 273 int
 274 ba_alloc_index(struct bitalloc *pool, int index)
 275 {
 276         int clear = 0;
 277         int index_copy = index;
 278
 279         if (index < 0 || index >= (int)pool->size)
 280                 return -1;
 281
 282         if (ba_alloc_index_helper(pool, 0, 1, 32, &index_copy, &clear) >= 0)
 283                 return index;
 284         else
 285                 return -1;
 286 }
 287
 288 static int
 289 ba_inuse_helper(struct bitalloc *pool,
 290                 int              offset,
 291                 int              words,
 292                 unsigned int     size,
 293                 int             *index)
 294 {
 295         bitalloc_word_t *storage = &pool->storage[offset];
 296         int       loc;
 297         int       r;
 298
 299         if (pool->size > size)
 300                 r = ba_inuse_helper(pool,
 301                                     offset + words + 1,
 302                                     storage[words],
 303                                     size * 32,
 304                                     index);
 305         else
 306                 r = 1; /* Check if in use */
 307
 308         loc = (*index % 32);
 309         *index = *index / 32;
 310
 311         if (r == 1)
 312                 r = (storage[*index] & (1 << loc)) ? -1 : 0;
 313
 314         return r;
 315 }
 316
 317 int
 318 ba_inuse(struct bitalloc *pool, int index)
 319 {
 320         if (index < 0 || index >= (int)pool->size)
 321                 return -1;
 322
 323         return ba_inuse_helper(pool, 0, 1, 32, &index) == 0;
 324 }
 325
 326 static int
 327 ba_free_helper(struct bitalloc *pool,
 328                int              offset,
 329                int              words,
 330                unsigned int     size,
 331                int             *index)
 332 {
 333         bitalloc_word_t *storage = &pool->storage[offset];
 334         int       loc;
 335         int       r;
 336
 337         if (pool->size > size)
 338                 r = ba_free_helper(pool,
 339                                    offset + words + 1,
 340                                    storage[words],
 341                                    size * 32,
 342                                    index);
 343         else
 344                 r = 1; /* Check if already free */
 345
 346         loc = (*index % 32);
 347         *index = *index / 32;
 348
 349         if (r == 1) {
 350                 r = (storage[*index] & (1 << loc)) ? -1 : 0;
 351                 if (r == 0)
 352                         pool->free_count++;
 353         }
 354
 355         if (r == 0)
 356                 storage[*index] |= (1 << loc);
 357
 358         return r;
 359 }
 360
 361 int
 362 ba_free(struct bitalloc *pool, int index)
 363 {
 364         if (index < 0 || index >= (int)pool->size)
 365                 return -1;
 366
 367         return ba_free_helper(pool, 0, 1, 32, &index);
 368 }
 369
 370 int
 371 ba_inuse_free(struct bitalloc *pool, int index)
 372 {
 373         if (index < 0 || index >= (int)pool->size)
 374                 return -1;
 375
 376         return ba_free_helper(pool, 0, 1, 32, &index) + 1;
 377 }
 378
 379 int
 380 ba_free_count(struct bitalloc *pool)
 381 {
 382         return (int)pool->free_count;
 383 }
 384
 385 int ba_inuse_count(struct bitalloc *pool)
 386 {
 387         return (int)(pool->size) - (int)(pool->free_count);
 388 }
 389
 390 static int
 391 ba_find_next_helper(struct bitalloc *pool,
 392                     int              offset,
 393                     int              words,
 394                     unsigned int     size,
 395                     int             *index,
 396                     int              free)
 397 {
 398         bitalloc_word_t *storage = &pool->storage[offset];
 399         int       loc, r, bottom = 0;
 400
 401         if (pool->size > size)
 402                 r = ba_find_next_helper(pool,
 403                                         offset + words + 1,
 404                                         storage[words],
 405                                         size * 32,
 406                                         index,
 407                                         free);
 408         else
 409                 bottom = 1; /* Bottom of tree */
 410
 411         loc = (*index % 32);
 412         *index = *index / 32;
 413
 414         if (bottom) {
 415                 int bit_index = *index * 32;
 416
 417                 loc = ba_ffs(~storage[*index] & ((bitalloc_word_t)-1 << loc));
 418                 if (loc > 0) {
 419                         loc--;
 420                         r = (bit_index + loc);
 421                         if (r >= (int)pool->size)
 422                                 r = -1;
 423                 } else {
 424                         /* Loop over array at bottom of tree */
 425                         r = -1;
 426                         bit_index += 32;
 427                         *index = *index + 1;
 428                         while ((int)pool->size > bit_index) {
 429                                 loc = ba_ffs(~storage[*index]);
 430
 431                                 if (loc > 0) {
 432                                         loc--;
 433                                         r = (bit_index + loc);
 434                                         if (r >= (int)pool->size)
 435                                                 r = -1;
 436                                         break;
 437                                 }
 438                                 bit_index += 32;
 439                                 *index = *index + 1;
 440                         }
 441                 }
 442         }
 443
 444         if (r >= 0 && (free)) {
 445                 if (bottom)
 446                         pool->free_count++;
 447                 storage[*index] |= (1 << loc);
 448         }
 449
 450         return r;
 451 }
 452
 453 int
 454 ba_find_next_inuse(struct bitalloc *pool, int index)
 455 {
 456         if (index < 0 ||
 457             index >= (int)pool->size ||
 458             pool->free_count == pool->size)
 459                 return -1;
 460
 461         return ba_find_next_helper(pool, 0, 1, 32, &index, 0);
 462 }
 463
 464 int
 465 ba_find_next_inuse_free(struct bitalloc *pool, int index)
 466 {
 467         if (index < 0 ||
 468             index >= (int)pool->size ||
 469             pool->free_count == pool->size)
 470                 return -1;
 471
 472         return ba_find_next_helper(pool, 0, 1, 32, &index, 1);
 473 }