sys/net/route/nhop_utils.c

   1 /*-
   2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   3  *
   4  * Copyright (c) 2020 Alexander V. Chernikov
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions
   8  * are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  *    notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  *    notice, this list of conditions and the following disclaimer in the
  13  *    documentation and/or other materials provided with the distribution.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  18  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  19  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  20  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  21  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  22  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  23  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  24  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  25  * SUCH DAMAGE.
  26  */
  27
  28 #include <sys/cdefs.h>
  29 __FBSDID("$FreeBSD$");
  30 #include "opt_inet.h"
  31 #include "opt_route.h"
  32 #include "opt_mpath.h"
  33
  34 #include <sys/param.h>
  35 #include <sys/systm.h>
  36 #include <sys/lock.h>
  37 #include <sys/malloc.h>
  38 #include <sys/mbuf.h>
  39 #include <sys/kernel.h>
  40
  41 #include <net/route/nhop_utils.h>
  42
  43 #define BLOCK_ITEMS     (8 * sizeof(u_long))    /* Number of items for ffsl() */
  44
  45 #define _BLOCKS_TO_SZ(_blocks)          ((size_t)(_blocks) * sizeof(u_long))
  46 #define _BLOCKS_TO_ITEMS(_blocks)       ((uint32_t)(_blocks) * BLOCK_ITEMS)
  47 #define _ITEMS_TO_BLOCKS(_items)        ((_items) / BLOCK_ITEMS)
  48
  49 static void _bitmask_init_idx(void *index, uint32_t items);
  50
  51 void
  52 bitmask_init(struct bitmask_head *bh, void *idx, uint32_t num_items)
  53 {
  54
  55         if (idx != NULL)
  56                 _bitmask_init_idx(idx, num_items);
  57
  58         memset(bh, 0, sizeof(struct bitmask_head));
  59         bh->blocks = _ITEMS_TO_BLOCKS(num_items);
  60         bh->idx = (u_long *)idx;
  61 }
  62
  63 uint32_t
  64 bitmask_get_resize_items(const struct bitmask_head *bh)
  65 {
  66         if ((bh->items_count * 2 > _BLOCKS_TO_ITEMS(bh->blocks)) && bh->items_count < 65536)
  67                 return (_BLOCKS_TO_ITEMS(bh->blocks) * 2);
  68
  69         return (0);
  70 }
  71
  72 int
  73 bitmask_should_resize(const struct bitmask_head *bh)
  74 {
  75
  76         return (bitmask_get_resize_items(bh) != 0);
  77 }
  78
  79 #if 0
  80 uint32_t
  81 _bitmask_get_blocks(uint32_t items)
  82 {
  83
  84         return (items / BLOCK_ITEMS);
  85 }
  86 #endif
  87
  88 size_t
  89 bitmask_get_size(uint32_t items)
  90 {
  91 #if _KERNEL
  92         KASSERT((items % BLOCK_ITEMS) == 0,
  93            ("bitmask size needs to power of 2 and greater or equal to %zu",
  94             BLOCK_ITEMS));
  95 #else
  96         assert((items % BLOCK_ITEMS) == 0);
  97 #endif
  98
  99         return (items / 8);
 100 }
 101
 102 static void
 103 _bitmask_init_idx(void *_idx, uint32_t items)
 104 {
 105         size_t size = bitmask_get_size(items);
 106         u_long *idx = (u_long *)_idx;
 107
 108         /* Mark all as free */
 109         memset(idx, 0xFF, size);
 110         *idx &= ~(u_long)1; /* Always skip index 0 */
 111 }
 112
 113 /*
 114  * _try_merge api to allow shrinking?
 115  */
 116 int
 117 bitmask_copy(const struct bitmask_head *bi, void *new_idx, uint32_t new_items)
 118 {
 119         uint32_t new_blocks = _BLOCKS_TO_ITEMS(new_items);
 120
 121         _bitmask_init_idx(new_idx, new_items);
 122
 123         if (bi->blocks < new_blocks) {
 124                 /* extend current blocks */
 125                 if (bi->blocks > 0)
 126                         memcpy(new_idx, bi->idx, _BLOCKS_TO_SZ(bi->blocks));
 127                 return (0);
 128         } else {
 129                 /* XXX: ensure all other blocks are non-zero */
 130                 for (int i = new_blocks; i < bi->blocks; i++) {
 131                 }
 132
 133                 return (1);
 134         }
 135 }
 136
 137 void
 138 bitmask_swap(struct bitmask_head *bh, void *new_idx, uint32_t new_items, void **pidx)
 139 {
 140         void *old_ptr;
 141
 142         old_ptr = bh->idx;
 143
 144         bh->idx = (u_long *)new_idx;
 145         bh->blocks = _ITEMS_TO_BLOCKS(new_items);
 146
 147         if (pidx != NULL)
 148                 *pidx = old_ptr;
 149 }
 150
 151 /*
 152  * Allocate new index in given instance and stores in in @pidx.
 153  * Returns 0 on success.
 154  */
 155 int
 156 bitmask_alloc_idx(struct bitmask_head *bi, uint16_t *pidx)
 157 {
 158         u_long *mask;
 159         int i, off, v;
 160
 161         off = bi->free_off;
 162         mask = &bi->idx[off];
 163
 164         for (i = off; i < bi->blocks; i++, mask++) {
 165                 if ((v = ffsl(*mask)) == 0)
 166                         continue;
 167
 168                 /* Mark as busy */
 169                 *mask &= ~ ((u_long)1 << (v - 1));
 170
 171                 bi->free_off = i;
 172
 173                 v = BLOCK_ITEMS * i + v - 1;
 174
 175                 *pidx = v;
 176                 bi->items_count++;
 177                 return (0);
 178         }
 179
 180         return (1);
 181 }
 182
 183 /*
 184  * Removes index from given set.
 185  * Returns 0 on success.
 186  */
 187 int
 188 bitmask_free_idx(struct bitmask_head *bi, uint16_t idx)
 189 {
 190         u_long *mask;
 191         int i, v;
 192
 193         if (idx == 0)
 194                 return (1);
 195
 196         i = idx / BLOCK_ITEMS;
 197         v = idx % BLOCK_ITEMS;
 198
 199         if (i >= bi->blocks)
 200                 return (1);
 201
 202         mask = &bi->idx[i];
 203
 204         if ((*mask & ((u_long)1 << v)) != 0)
 205                 return (1);
 206
 207         /* Mark as free */
 208         *mask |= (u_long)1 << v;
 209         bi->items_count--;
 210
 211         /* Update free offset */
 212         if (bi->free_off > i)
 213                 bi->free_off = i;
 214
 215         return (0);
 216 }