1 /*
2  * Copyright (c) 2025 Nordic Semiconductor ASA
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 #include <zephyr/sys/util.h>
7 #include <zephyr/sys/math_extras.h>
8 
bitmask_find_gap(uint32_t mask,size_t num_bits,size_t total_bits,bool first_match)9 int bitmask_find_gap(uint32_t mask, size_t num_bits, size_t total_bits, bool first_match)
10 {
11 	uint32_t max = UINT32_MAX;
12 	int max_loc = -1;
13 
14 	if (total_bits < 32) {
15 		mask |= ~BIT_MASK(total_bits);
16 	}
17 
18 	mask = ~mask;
19 	while (mask != 0U) {
20 		uint32_t block_size;
21 		uint32_t loc;
22 		int nidx;
23 		uint32_t idx = 31 - u32_count_leading_zeros(mask);
24 		uint32_t rmask = ~BIT_MASK(idx);
25 
26 		rmask |= mask;
27 		rmask = ~rmask;
28 		if (rmask != 0U) {
29 			nidx = 31 - u32_count_leading_zeros(rmask);
30 			block_size = idx - nidx;
31 			loc = nidx + 1;
32 			mask &= BIT_MASK(nidx);
33 		} else {
34 			mask = 0;
35 			block_size = idx + 1;
36 			loc = 0;
37 		}
38 
39 		if ((block_size == num_bits) || (first_match && block_size > num_bits)) {
40 			max_loc = loc;
41 			max = block_size;
42 			break;
43 		} else if (block_size >= num_bits && block_size < max) {
44 			max_loc = loc;
45 			max = block_size;
46 		}
47 	}
48 
49 	return max_loc;
50 }
51