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