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
  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)
  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 */
 105         for (i = 0; i < size; i++)
 106                 ba_free(pool, i);
 107
 108         return 0;
 109 }
 110
 111 static int
 112 ba_alloc_helper(struct bitalloc *pool,
 113                 int              offset,
 114                 int              words,
 115                 unsigned int     size,
 116                 int              index,
 117                 int             *clear)
 118 {
 119         bitalloc_word_t *storage = &pool->storage[offset];
 120         int       loc = ba_ffs(storage[index]);
 121         int       r;
 122
 123         if (loc == 0)
 124                 return -1;
 125
 126         loc--;
 127
 128         if (pool->size > size) {
 129                 r = ba_alloc_helper(pool,
 130                                     offset + words + 1,
 131                                     storage[words],
 132                                     size * 32,
 133                                     index * 32 + loc,
 134                                     clear);
 135         } else {
 136                 r = index * 32 + loc;
 137                 *clear = 1;
 138                 pool->free_count--;
 139         }
 140
 141         if (*clear) {
 142                 storage[index] &= ~(1 << loc);
 143                 *clear = (storage[index] == 0);
 144         }
 145
 146         return r;
 147 }
 148
 149 int
 150 ba_alloc(struct bitalloc *pool)
 151 {
 152         int clear = 0;
 153
 154         return ba_alloc_helper(pool, 0, 1, 32, 0, &clear);
 155 }
 156
 157 /**
 158  * Help function to alloc entry from highest available index
 159  *
 160  * Searching the pool from highest index for the empty entry.
 161  *
 162  * [in] pool
 163  *   Pointer to the resource pool
 164  *
 165  * [in] offset
 166  *   Offset of the storage in the pool
 167  *
 168  * [in] words
 169  *   Number of words in this level
 170  *
 171  * [in] size
 172  *   Number of entries in this level
 173  *
 174  * [in] index
 175  *   Index of words that has the entry
 176  *
 177  * [in] clear
 178  *   Indicate if a bit needs to be clear due to the entry is allocated
 179  *
 180  * Returns:
 181  *     0 - Success
 182  *    -1 - Failure
 183  */
 184 static int
 185 ba_alloc_reverse_helper(struct bitalloc *pool,
 186                         int offset,
 187                         int words,
 188                         unsigned int size,
 189                         int index,
 190                         int *clear)
 191 {
 192         bitalloc_word_t *storage = &pool->storage[offset];
 193         int loc = ba_fls(storage[index]);
 194         int r;
 195
 196         if (loc == 0)
 197                 return -1;
 198
 199         loc--;
 200
 201         if (pool->size > size) {
 202                 r = ba_alloc_reverse_helper(pool,
 203                                             offset + words + 1,
 204                                             storage[words],
 205                                             size * 32,
 206                                             index * 32 + loc,
 207                                             clear);
 208         } else {
 209                 r = index * 32 + loc;
 210                 *clear = 1;
 211                 pool->free_count--;
 212         }
 213
 214         if (*clear) {
 215                 storage[index] &= ~(1 << loc);
 216                 *clear = (storage[index] == 0);
 217         }
 218
 219         return r;
 220 }
 221
 222 int
 223 ba_alloc_reverse(struct bitalloc *pool)
 224 {
 225         int clear = 0;
 226
 227         return ba_alloc_reverse_helper(pool, 0, 1, 32, 0, &clear);
 228 }
 229
 230 static int
 231 ba_alloc_index_helper(struct bitalloc *pool,
 232                       int              offset,
 233                       int              words,
 234                       unsigned int     size,
 235                       int             *index,
 236                       int             *clear)
 237 {
 238         bitalloc_word_t *storage = &pool->storage[offset];
 239         int       loc;
 240         int       r;
 241
 242         if (pool->size > size)
 243                 r = ba_alloc_index_helper(pool,
 244                                           offset + words + 1,
 245                                           storage[words],
 246                                           size * 32,
 247                                           index,
 248                                           clear);
 249         else
 250                 r = 1; /* Check if already allocated */
 251
 252         loc = (*index % 32);
 253         *index = *index / 32;
 254
 255         if (r == 1) {
 256                 r = (storage[*index] & (1 << loc)) ? 0 : -1;
 257                 if (r == 0) {
 258                         *clear = 1;
 259                         pool->free_count--;
 260                 }
 261         }
 262
 263         if (*clear) {
 264                 storage[*index] &= ~(1 << loc);
 265                 *clear = (storage[*index] == 0);
 266         }
 267
 268         return r;
 269 }
 270
 271 int
 272 ba_alloc_index(struct bitalloc *pool, int index)
 273 {
 274         int clear = 0;
 275         int index_copy = index;
 276
 277         if (index < 0 || index >= (int)pool->size)
 278                 return -1;
 279
 280         if (ba_alloc_index_helper(pool, 0, 1, 32, &index_copy, &clear) >= 0)
 281                 return index;
 282         else
 283                 return -1;
 284 }
 285
 286 static int
 287 ba_inuse_helper(struct bitalloc *pool,
 288                 int              offset,
 289                 int              words,
 290                 unsigned int     size,
 291                 int             *index)
 292 {
 293         bitalloc_word_t *storage = &pool->storage[offset];
 294         int       loc;
 295         int       r;
 296
 297         if (pool->size > size)
 298                 r = ba_inuse_helper(pool,
 299                                     offset + words + 1,
 300                                     storage[words],
 301                                     size * 32,
 302                                     index);
 303         else
 304                 r = 1; /* Check if in use */
 305
 306         loc = (*index % 32);
 307         *index = *index / 32;
 308
 309         if (r == 1)
 310                 r = (storage[*index] & (1 << loc)) ? -1 : 0;
 311
 312         return r;
 313 }
 314
 315 int
 316 ba_inuse(struct bitalloc *pool, int index)
 317 {
 318         if (index < 0 || index >= (int)pool->size)
 319                 return -1;
 320
 321         return ba_inuse_helper(pool, 0, 1, 32, &index) == 0;
 322 }
 323
 324 static int
 325 ba_free_helper(struct bitalloc *pool,
 326                int              offset,
 327                int              words,
 328                unsigned int     size,
 329                int             *index)
 330 {
 331         bitalloc_word_t *storage = &pool->storage[offset];
 332         int       loc;
 333         int       r;
 334
 335         if (pool->size > size)
 336                 r = ba_free_helper(pool,
 337                                    offset + words + 1,
 338                                    storage[words],
 339                                    size * 32,
 340                                    index);
 341         else
 342                 r = 1; /* Check if already free */
 343
 344         loc = (*index % 32);
 345         *index = *index / 32;
 346
 347         if (r == 1) {
 348                 r = (storage[*index] & (1 << loc)) ? -1 : 0;
 349                 if (r == 0)
 350                         pool->free_count++;
 351         }
 352
 353         if (r == 0)
 354                 storage[*index] |= (1 << loc);
 355
 356         return r;
 357 }
 358
 359 int
 360 ba_free(struct bitalloc *pool, int index)
 361 {
 362         if (index < 0 || index >= (int)pool->size)
 363                 return -1;
 364
 365         return ba_free_helper(pool, 0, 1, 32, &index);
 366 }
 367
 368 int
 369 ba_inuse_free(struct bitalloc *pool, int index)
 370 {
 371         if (index < 0 || index >= (int)pool->size)
 372                 return -1;
 373
 374         return ba_free_helper(pool, 0, 1, 32, &index) + 1;
 375 }
 376
 377 int
 378 ba_free_count(struct bitalloc *pool)
 379 {
 380         return (int)pool->free_count;
 381 }
 382
 383 int ba_inuse_count(struct bitalloc *pool)
 384 {
 385         return (int)(pool->size) - (int)(pool->free_count);
 386 }
 387
 388 static int
 389 ba_find_next_helper(struct bitalloc *pool,
 390                     int              offset,
 391                     int              words,
 392                     unsigned int     size,
 393                     int             *index,
 394                     int              free)
 395 {
 396         bitalloc_word_t *storage = &pool->storage[offset];
 397         int       loc, r, bottom = 0;
 398
 399         if (pool->size > size)
 400                 r = ba_find_next_helper(pool,
 401                                         offset + words + 1,
 402                                         storage[words],
 403                                         size * 32,
 404                                         index,
 405                                         free);
 406         else
 407                 bottom = 1; /* Bottom of tree */
 408
 409         loc = (*index % 32);
 410         *index = *index / 32;
 411
 412         if (bottom) {
 413                 int bit_index = *index * 32;
 414
 415                 loc = ba_ffs(~storage[*index] & ((bitalloc_word_t)-1 << loc));
 416                 if (loc > 0) {
 417                         loc--;
 418                         r = (bit_index + loc);
 419                         if (r >= (int)pool->size)
 420                                 r = -1;
 421                 } else {
 422                         /* Loop over array at bottom of tree */
 423                         r = -1;
 424                         bit_index += 32;
 425                         *index = *index + 1;
 426                         while ((int)pool->size > bit_index) {
 427                                 loc = ba_ffs(~storage[*index]);
 428
 429                                 if (loc > 0) {
 430                                         loc--;
 431                                         r = (bit_index + loc);
 432                                         if (r >= (int)pool->size)
 433                                                 r = -1;
 434                                         break;
 435                                 }
 436                                 bit_index += 32;
 437                                 *index = *index + 1;
 438                         }
 439                 }
 440         }
 441
 442         if (r >= 0 && (free)) {
 443                 if (bottom)
 444                         pool->free_count++;
 445                 storage[*index] |= (1 << loc);
 446         }
 447
 448         return r;
 449 }
 450
 451 int
 452 ba_find_next_inuse(struct bitalloc *pool, int index)
 453 {
 454         if (index < 0 ||
 455             index >= (int)pool->size ||
 456             pool->free_count == pool->size)
 457                 return -1;
 458
 459         return ba_find_next_helper(pool, 0, 1, 32, &index, 0);
 460 }
 461
 462 int
 463 ba_find_next_inuse_free(struct bitalloc *pool, int index)
 464 {
 465         if (index < 0 ||
 466             index >= (int)pool->size ||
 467             pool->free_count == pool->size)
 468                 return -1;
 469
 470         return ba_find_next_helper(pool, 0, 1, 32, &index, 1);
 471 }