peer_id.c

/*
 * Copyright (c) 2015-2025 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
 */

#include <nrf_error.h>
#include <stdint.h>
#include <string.h>
#include <limits.h>
#include <zephyr/sys/atomic.h>
#if !defined(CONFIG_UNITY)
#include <nrfx.h>
#endif
#include <bm/bluetooth/peer_manager/peer_manager_types.h>
#include <modules/peer_id.h>

struct peer_id_flags {
	/** Bitmap designating which peer IDs are in use. */
	ATOMIC_DEFINE(used_peer_ids, PM_PEER_ID_N_AVAILABLE_IDS);
	/** Bitmap designating which peer IDs are marked for deletion. */
	ATOMIC_DEFINE(deleted_peer_ids, PM_PEER_ID_N_AVAILABLE_IDS);
};

static struct peer_id_flags peer_id_flags = {{0}, {0}};

static void internal_state_reset(struct peer_id_flags *pi_flags)
{
	memset(pi_flags, 0, sizeof(struct peer_id_flags));
}

void peer_id_init(void)
{
	internal_state_reset(&peer_id_flags);
}

static uint32_t find_and_set_flag(atomic_t *pi_flags, uint32_t flag_count)
{
	for (uint32_t i = 0; i < ATOMIC_BITMAP_SIZE(flag_count); i++) {
		uint32_t word = atomic_get(&pi_flags[i]);
		uint32_t inverted = ~word;

		while (inverted) {
			/* Find lowest zero bit */
#if defined(CONFIG_UNITY)
			uint32_t first_zero = __builtin_ctz(inverted);
#else
			uint32_t first_zero = NRF_CTZ(inverted);
#endif
			uint32_t first_zero_global = first_zero + (i * 32);

			if (first_zero_global >= flag_count) {
				break;
			}
			if (!atomic_test_and_set_bit(pi_flags, first_zero_global)) {
				return first_zero_global;
			}
			inverted &= ~(1u << first_zero);
		}
	}
	return flag_count;
}

static uint16_t claim(uint16_t peer_id, atomic_t *pi_flags)
{
	uint16_t allocated_peer_id = PM_PEER_ID_INVALID;

	if (peer_id == PM_PEER_ID_INVALID) {
		allocated_peer_id =
			find_and_set_flag(pi_flags, PM_PEER_ID_N_AVAILABLE_IDS);
		if (allocated_peer_id == PM_PEER_ID_N_AVAILABLE_IDS) {
			allocated_peer_id = PM_PEER_ID_INVALID;
		}
	} else if (peer_id < PM_PEER_ID_N_AVAILABLE_IDS) {
		bool lock_success = !atomic_test_and_set_bit(pi_flags, peer_id);

		allocated_peer_id = lock_success ? peer_id : PM_PEER_ID_INVALID;
	}
	return allocated_peer_id;
}

static void release(uint16_t peer_id, atomic_t *pi_flags)
{
	if (peer_id < PM_PEER_ID_N_AVAILABLE_IDS) {
		atomic_clear_bit(pi_flags, peer_id);
	}
}

uint16_t peer_id_allocate(uint16_t peer_id)
{
	return claim(peer_id, peer_id_flags.used_peer_ids);
}

bool peer_id_delete(uint16_t peer_id)
{
	uint16_t deleted_peer_id;

	if (peer_id == PM_PEER_ID_INVALID) {
		return false;
	}

	deleted_peer_id = claim(peer_id, peer_id_flags.deleted_peer_ids);

	return (deleted_peer_id == peer_id);
}

void peer_id_free(uint16_t peer_id)
{
	release(peer_id, peer_id_flags.used_peer_ids);
	release(peer_id, peer_id_flags.deleted_peer_ids);
}

bool peer_id_is_allocated(uint16_t peer_id)
{
	if (peer_id < PM_PEER_ID_N_AVAILABLE_IDS) {
		return atomic_test_bit(peer_id_flags.used_peer_ids, peer_id);
	}
	return false;
}

bool peer_id_is_deleted(uint16_t peer_id)
{
	if (peer_id < PM_PEER_ID_N_AVAILABLE_IDS) {
		return atomic_test_bit(peer_id_flags.deleted_peer_ids, peer_id);
	}
	return false;
}

uint16_t next_id_get(uint16_t prev_peer_id, atomic_t *pi_flags)
{
	uint16_t i = (prev_peer_id == PM_PEER_ID_INVALID) ? 0 : (prev_peer_id + 1);

	for (; i < PM_PEER_ID_N_AVAILABLE_IDS; i++) {
		if (atomic_test_bit(pi_flags, i)) {
			return i;
		}
	}

	return PM_PEER_ID_INVALID;
}

uint16_t peer_id_get_next_used(uint16_t peer_id)
{
	peer_id = next_id_get(peer_id, peer_id_flags.used_peer_ids);

	while (peer_id != PM_PEER_ID_INVALID) {
		if (!peer_id_is_deleted(peer_id)) {
			return peer_id;
		}

		peer_id = next_id_get(peer_id, peer_id_flags.used_peer_ids);
	}

	return peer_id;
}

uint16_t peer_id_get_next_deleted(uint16_t prev_peer_id)
{
	return next_id_get(prev_peer_id, peer_id_flags.deleted_peer_ids);
}

uint32_t peer_id_n_ids(void)
{
	uint32_t n_ids = 0;

	for (uint16_t i = 0; i < PM_PEER_ID_N_AVAILABLE_IDS; i++) {
		n_ids += atomic_test_bit(peer_id_flags.used_peer_ids, i);
	}

	return n_ids;
}