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
  50 static void _bitmask_init_idx(void *index, uint32_t items);
  51
  52 void
  53 bitmask_init(struct bitmask_head *bh, void *idx, uint32_t num_items)
  54 {
  55
  56         if (idx != NULL)
  57                 _bitmask_init_idx(idx, num_items);
  58
  59         memset(bh, 0, sizeof(struct bitmask_head));
  60         bh->blocks = _ITEMS_TO_BLOCKS(num_items);
  61         bh->idx = (u_long *)idx;
  62 }
  63
  64 uint32_t
  65 bitmask_get_resize_items(const struct bitmask_head *bh)
  66 {
  67         if ((bh->items_count * 2 > _BLOCKS_TO_ITEMS(bh->blocks)) && bh->items_count < 65536)
  68                 return (_BLOCKS_TO_ITEMS(bh->blocks) * 2);
  69
  70         return (0);
  71 }
  72
  73 int
  74 bitmask_should_resize(const struct bitmask_head *bh)
  75 {
  76
  77         return (bitmask_get_resize_items(bh) != 0);
  78 }
  79
  80 #if 0
  81 uint32_t
  82 _bitmask_get_blocks(uint32_t items)
  83 {
  84
  85         return (items / BLOCK_ITEMS);
  86 }
  87 #endif
  88
  89 size_t
  90 bitmask_get_size(uint32_t items)
  91 {
  92 #if _KERNEL
  93         KASSERT((items % BLOCK_ITEMS) == 0,
  94            ("bitmask size needs to power of 2 and greater or equal to %zu",
  95             BLOCK_ITEMS));
  96 #else
  97         assert((items % BLOCK_ITEMS) == 0);
  98 #endif
  99
 100         return (items / 8);
 101 }
 102
 103 static void
 104 _bitmask_init_idx(void *_idx, uint32_t items)
 105 {
 106         size_t size = bitmask_get_size(items);
 107         u_long *idx = (u_long *)_idx;
 108
 109         /* Mark all as free */
 110         memset(idx, 0xFF, size);
 111         *idx &= ~(u_long)1; /* Always skip index 0 */
 112 }
 113
 114
 115 /*
 116  * _try_merge api to allow shrinking?
 117  */
 118 int
 119 bitmask_copy(const struct bitmask_head *bi, void *new_idx, uint32_t new_items)
 120 {
 121         uint32_t new_blocks = _BLOCKS_TO_ITEMS(new_items);
 122
 123         _bitmask_init_idx(new_idx, new_items);
 124
 125         if (bi->blocks < new_blocks) {
 126                 /* extend current blocks */
 127                 if (bi->blocks > 0)
 128                         memcpy(new_idx, bi->idx, _BLOCKS_TO_SZ(bi->blocks));
 129                 return (0);
 130         } else {
 131                 /* XXX: ensure all other blocks are non-zero */
 132                 for (int i = new_blocks; i < bi->blocks; i++) {
 133                 }
 134
 135                 return (1);
 136         }
 137 }
 138
 139 void
 140 bitmask_swap(struct bitmask_head *bh, void *new_idx, uint32_t new_items, void **pidx)
 141 {
 142         void *old_ptr;
 143
 144         old_ptr = bh->idx;
 145
 146         bh->idx = (u_long *)new_idx;
 147         bh->blocks = _ITEMS_TO_BLOCKS(new_items);
 148
 149         if (pidx != NULL)
 150                 *pidx = old_ptr;
 151 }
 152
 153 /*
 154  * Allocate new index in given instance and stores in in @pidx.
 155  * Returns 0 on success.
 156  */
 157 int
 158 bitmask_alloc_idx(struct bitmask_head *bi, uint16_t *pidx)
 159 {
 160         u_long *mask;
 161         int i, off, v;
 162
 163         off = bi->free_off;
 164         mask = &bi->idx[off];
 165
 166         for (i = off; i < bi->blocks; i++, mask++) {
 167                 if ((v = ffsl(*mask)) == 0)
 168                         continue;
 169
 170                 /* Mark as busy */
 171                 *mask &= ~ ((u_long)1 << (v - 1));
 172
 173                 bi->free_off = i;
 174
 175                 v = BLOCK_ITEMS * i + v - 1;
 176
 177                 *pidx = v;
 178                 bi->items_count++;
 179                 return (0);
 180         }
 181
 182         return (1);
 183 }
 184
 185 /*
 186  * Removes index from given set.
 187  * Returns 0 on success.
 188  */
 189 int
 190 bitmask_free_idx(struct bitmask_head *bi, uint16_t idx)
 191 {
 192         u_long *mask;
 193         int i, v;
 194
 195         if (idx == 0)
 196                 return (1);
 197
 198         i = idx / BLOCK_ITEMS;
 199         v = idx % BLOCK_ITEMS;
 200
 201         if (i >= bi->blocks)
 202                 return (1);
 203
 204         mask = &bi->idx[i];
 205
 206         if ((*mask & ((u_long)1 << v)) != 0)
 207                 return (1);
 208
 209         /* Mark as free */
 210         *mask |= (u_long)1 << v;
 211         bi->items_count--;
 212
 213         /* Update free offset */
 214         if (bi->free_off > i)
 215                 bi->free_off = i;
 216
 217         return (0);
 218 }
 219