/*
 * Copyright (c) 2018-2021 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <stdint.h>

#include "hal/ccm.h"
#include "hal/radio.h"

#include <soc.h>
#include "hal/debug.h"

static uint8_t chan_sel_remap(uint8_t *chan_map, uint8_t chan_index);
#if defined(CONFIG_BT_CTLR_CHAN_SEL_2)
static uint16_t chan_prn_s(uint16_t counter, uint16_t chan_id);
static uint16_t chan_prn_e(uint16_t counter, uint16_t chan_id);

#if defined(CONFIG_BT_CTLR_BROADCAST_ISO)
static uint8_t chan_sel_remap_index(uint8_t *chan_map, uint8_t chan_index);
static uint16_t chan_prn_subevent_se(uint16_t chan_id,
				     uint16_t *prn_subevent_lu);
static uint8_t chan_d(uint8_t n);
#endif /* CONFIG_BT_CTLR_BROADCAST_ISO */
#endif /* CONFIG_BT_CTLR_CHAN_SEL_2 */

#if defined(CONFIG_BT_CONN)
/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.2
 * Channel Selection algorithm #1
 */
uint8_t lll_chan_sel_1(uint8_t *chan_use, uint8_t hop, uint16_t latency, uint8_t *chan_map,
		    uint8_t chan_count)
{
	uint8_t chan_next;

	chan_next = ((*chan_use) + (hop * (1 + latency))) % 37;
	*chan_use = chan_next;

	if ((chan_map[chan_next >> 3] & (1 << (chan_next % 8))) == 0U) {
		uint8_t chan_index;

		chan_index = chan_next % chan_count;
		chan_next = chan_sel_remap(chan_map, chan_index);

	} else {
		/* channel can be used, return it */
	}

	return chan_next;
}
#endif /* CONFIG_BT_CONN */

#if defined(CONFIG_BT_CTLR_CHAN_SEL_2)
/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3.2
 * Inputs and basic components
 */
uint16_t lll_chan_id(uint8_t *access_addr)
{
	uint16_t aa_ls = ((uint16_t)access_addr[1] << 8) | access_addr[0];
	uint16_t aa_ms = ((uint16_t)access_addr[3] << 8) | access_addr[2];

	return aa_ms ^ aa_ls;
}

/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
 * Channel Selection algorithm #2, and Section 4.5.8.3.1 Overview
 * Below interface is used for ACL connections.
 */
uint8_t lll_chan_sel_2(uint16_t counter, uint16_t chan_id, uint8_t *chan_map,
		    uint8_t chan_count)
{
	uint8_t chan_next;
	uint16_t prn_e;

	prn_e = chan_prn_e(counter, chan_id);
	chan_next = prn_e % 37;

	if ((chan_map[chan_next >> 3] & (1 << (chan_next % 8))) == 0U) {
		uint8_t chan_index;

		chan_index = ((uint32_t)chan_count * prn_e) >> 16;
		chan_next = chan_sel_remap(chan_map, chan_index);

	} else {
		/* channel can be used, return it */
	}

	return chan_next;
}

#if defined(CONFIG_BT_CTLR_BROADCAST_ISO)
/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
 * Channel Selection algorithm #2, and Section 4.5.8.3.1 Overview
 *
 * Below interface is used for ISO first subevent.
 */
uint8_t lll_chan_iso_event(uint16_t counter, uint16_t chan_id,
			   uint8_t *chan_map, uint8_t chan_count,
			   uint16_t *prn_s, uint16_t *remap_idx)
{
	uint8_t chan_idx;
	uint16_t prn_e;

	*prn_s = chan_prn_s(counter, chan_id);
	prn_e = *prn_s ^ chan_id;
	chan_idx = prn_e % 37;

	if ((chan_map[chan_idx >> 3] & (1 << (chan_idx % 8))) == 0U) {
		*remap_idx = ((uint32_t)chan_count * prn_e) >> 16;
		chan_idx = chan_sel_remap(chan_map, *remap_idx);

	} else {
		*remap_idx = chan_sel_remap_index(chan_map, chan_idx);
	}

	return chan_idx;
}

/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
 * Channel Selection algorithm #2, and Section 4.5.8.3.1 Overview
 *
 * Below interface is used for ISO next subevent.
 */
uint8_t lll_chan_iso_subevent(uint16_t chan_id, uint8_t *chan_map,
			      uint8_t chan_count, uint16_t *prn_subevent_lu,
			      uint16_t *remap_idx)
{
	uint16_t prn_subevent_se;
	uint8_t chan_idx;
	uint8_t d;
	uint8_t x;

	prn_subevent_se = chan_prn_subevent_se(chan_id, prn_subevent_lu);

	d = chan_d(chan_count);

	/* Sub-expression to get natural number (N - 2d + 1) to be used in the
	 * calculation of d.
	 */
	if ((chan_count + 1) > (d << 1)) {
		x = (chan_count + 1) - (d << 1);
	} else {
		x = 0;
	}

	chan_idx = ((((uint32_t)prn_subevent_se * x) >> 16) +
		    d + *remap_idx) % chan_count;

	if ((chan_map[chan_idx >> 3] & (1 << (chan_idx % 8))) == 0U) {
		*remap_idx = chan_idx;
		chan_idx = chan_sel_remap(chan_map, *remap_idx);
	} else {
		*remap_idx = chan_idx;
	}

	return chan_idx;
}
#endif /* CONFIG_BT_CTLR_BROADCAST_ISO */
#endif /* CONFIG_BT_CTLR_CHAN_SEL_2 */

/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
 * Channel Selection algorithm #2, and Section 4.5.8.3.4 Event mapping to used
 * channel index
 */
static uint8_t chan_sel_remap(uint8_t *chan_map, uint8_t chan_index)
{
	uint8_t chan_next;
	uint8_t byte_count;

	chan_next = 0U;
	byte_count = 5U;
	while (byte_count--) {
		uint8_t bite;
		uint8_t bit_count;

		bite = *chan_map;
		bit_count = 8U;
		while (bit_count--) {
			if (bite & 0x01) {
				if (chan_index == 0U) {
					break;
				}
				chan_index--;
			}
			chan_next++;
			bite >>= 1;
		}

		if (bit_count < 8) {
			break;
		}

		chan_map++;
	}

	return chan_next;
}

#if defined(CONFIG_BT_CTLR_CHAN_SEL_2)
/* Attribution:
 * http://graphics.stanford.edu/%7Eseander/bithacks.html#ReverseByteWith32Bits
 */
/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3.2
 * Inputs and basic components, for below operations
 */
static uint8_t chan_rev_8(uint8_t b)
{
	b = (((uint32_t)b * 0x0802LU & 0x22110LU) |
	     ((uint32_t)b * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16;

	return b;
}

static uint16_t chan_perm(uint16_t i)
{
	return (chan_rev_8((i >> 8) & 0xFF) << 8) | chan_rev_8(i & 0xFF);
}

static uint16_t chan_mam(uint16_t a, uint16_t b)
{
	return ((uint32_t)a * 17U + b) & 0xFFFF;
}

static uint16_t chan_prn_s(uint16_t counter, uint16_t chan_id)
{
	uint8_t iterate;
	uint16_t prn_s;

	prn_s = counter ^ chan_id;

	for (iterate = 0U; iterate < 3; iterate++) {
		prn_s = chan_perm(prn_s);
		prn_s = chan_mam(prn_s, chan_id);
	}

	return prn_s;
}

static uint16_t chan_prn_e(uint16_t counter, uint16_t chan_id)
{
	uint16_t prn_e;

	prn_e = chan_prn_s(counter, chan_id);
	prn_e ^= chan_id;

	return prn_e;
}

#if defined(CONFIG_BT_CTLR_BROADCAST_ISO)
/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
 * Channel Selection algorithm #2, and Section 4.5.8.3.4 Event mapping to used
 * channel index
 *
 * Below function is used in the context of next subevent, return remapping
 * index.
 */
static uint8_t chan_sel_remap_index(uint8_t *chan_map, uint8_t chan_index)
{
	uint8_t octet_count;
	uint8_t remap_index;

	remap_index = 0U;
	octet_count = 5U;
	while (octet_count--) {
		uint8_t octet;
		uint8_t bit_count;

		octet = *chan_map;
		bit_count = 8U;
		while (bit_count--) {
			if (!chan_index) {
				return remap_index;
			}
			chan_index--;

			if (octet & BIT(0)) {
				remap_index++;
			}
			octet >>= 1;
		}

		chan_map++;
	}

	return 0;
}

/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
 * Channel Selection algorithm #2, and Section 4.5.8.3.5 Subevent pseudo-random
 * number generation
 */
static uint16_t chan_prn_subevent_se(uint16_t chan_id,
				     uint16_t *prn_subevent_lu)
{
	uint16_t prn_subevent_se;
	uint16_t lu;

	lu = *prn_subevent_lu;
	lu = chan_perm(lu);
	lu = chan_mam(lu, chan_id);

	*prn_subevent_lu = lu;

	prn_subevent_se = lu ^ chan_id;

	return prn_subevent_se;
}

/* Refer to Bluetooth Specification v5.2 Vol 6, Part B, Section 4.5.8.3
 * Channel Selection algorithm #2, and Section 4.5.8.3.6 Subevent mapping to
 * used channel index
 */
static uint8_t chan_d(uint8_t n)
{
	uint8_t x, y;

	/* Sub-expression to get natural number (N - 5) to be used in the
	 * calculation of d.
	 */
	if (n > 5) {
		x = n - 5;
	} else {
		x = 0;
	}

	/* Sub-expression to get natural number ((N - 10) / 2) to be used in the
	 * calculation of d.
	 */
	if (n > 10) {
		y = (n - 10) >> 1;
	} else {
		y = 0;
	}

	/* Calculate d using the above sub expressions */
	return MAX(1, MAX(MIN(3, x), MIN(11, y)));
}
#endif /* CONFIG_BT_CTLR_BROADCAST_ISO */

#if defined(CONFIG_BT_CTLR_TEST)
/* Refer to Bluetooth Specification v5.2 Vol 6, Part C, Section 3 LE Channel
 * Selection algorithm #2 sample data
 */
void lll_chan_sel_2_ut(void)
{
	uint8_t chan_map_1[] = {0xFF, 0xFF, 0xFF, 0xFF, 0x1F};
	uint8_t const chan_map_1_37_used = 37U;
	uint8_t chan_map_2[] = {0x00, 0x06, 0xE0, 0x00, 0x1E};
	uint8_t const chan_map_2_9_used = 9U;
	uint16_t const chan_id = 0x305F;
	uint8_t m;

	/* Tests when ISO not supported */
	/* Section 3.1 Sample Data 1 (37 used channels) */
	m = lll_chan_sel_2(0, chan_id, chan_map_1, chan_map_1_37_used);
	LL_ASSERT(m == 25U);

	m = lll_chan_sel_2(1, chan_id, chan_map_1, chan_map_1_37_used);
	LL_ASSERT(m == 20U);

	m = lll_chan_sel_2(2, chan_id, chan_map_1, chan_map_1_37_used);
	LL_ASSERT(m == 6U);

	m = lll_chan_sel_2(3, chan_id, chan_map_1, chan_map_1_37_used);
	LL_ASSERT(m == 21U);

	/* Section 3.1 Sample Data 2 (9 used channels) */
	m = lll_chan_sel_2(6, chan_id, chan_map_2, chan_map_2_9_used);
	LL_ASSERT(m == 23U);

	m = lll_chan_sel_2(7, chan_id, chan_map_2, chan_map_2_9_used);
	LL_ASSERT(m == 9U);

	m = lll_chan_sel_2(8, chan_id, chan_map_2, chan_map_2_9_used);
	LL_ASSERT(m == 34U);


#if defined(CONFIG_BT_CTLR_BROADCAST_ISO)
	uint16_t prn_subevent_lu;
	uint16_t prn_subevent_se;
	uint16_t remap_idx;
	uint16_t prn_s;

	/* BIS subevent 2, event counter 0, test prnSubEvent_se */
	prn_s = 56857 ^ chan_id;
	prn_subevent_lu = prn_s;
	prn_subevent_se = chan_prn_subevent_se(chan_id, &prn_subevent_lu);
	LL_ASSERT(prn_subevent_se == 11710);

	/* BIS subevent 3, event counter 0 */
	prn_subevent_se = chan_prn_subevent_se(chan_id, &prn_subevent_lu);
	LL_ASSERT(prn_subevent_se == 16649);

	/* BIS subevent 4, event counter 0 */
	prn_subevent_se = chan_prn_subevent_se(chan_id, &prn_subevent_lu);
	LL_ASSERT(prn_subevent_se == 38198);

	/* Section 3.1 Sample Data 1 (37 used channels) */
	/* BIS subevent 1, event counter 0 */
	m = lll_chan_iso_event(0, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT((prn_s ^ chan_id) == 56857);
	LL_ASSERT(m == 25U);
	LL_ASSERT(remap_idx == 25U);

	/* BIS subvent 2 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 1U);
	LL_ASSERT(m == 1U);

	/* BIS subvent 3 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 16U);
	LL_ASSERT(m == 16U);

	/* BIS subvent 4 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 36U);
	LL_ASSERT(m == 36U);

	/* BIS subevent 1, event counter 1 */
	m = lll_chan_iso_event(1, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT((prn_s ^ chan_id) == 1685);
	LL_ASSERT(m == 20U);
	LL_ASSERT(remap_idx == 20U);

	/* BIS subvent 2 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 36U);
	LL_ASSERT(m == 36U);

	/* BIS subvent 3 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 12U);
	LL_ASSERT(m == 12U);

	/* BIS subvent 4 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 34U);
	LL_ASSERT(m == 34U);

	/* BIS subevent 1, event counter 2 */
	m = lll_chan_iso_event(2, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT((prn_s ^ chan_id) == 38301);
	LL_ASSERT(m == 6U);
	LL_ASSERT(remap_idx == 6U);

	/* BIS subvent 2 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 18U);
	LL_ASSERT(m == 18U);

	/* BIS subvent 3 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 32U);
	LL_ASSERT(m == 32U);

	/* BIS subvent 4 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 21U);
	LL_ASSERT(m == 21U);

	/* BIS subevent 1, event counter 3 */
	m = lll_chan_iso_event(3, chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT((prn_s ^ chan_id) == 27475);
	LL_ASSERT(m == 21U);
	LL_ASSERT(remap_idx == 21U);

	/* BIS subvent 2 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 4U);
	LL_ASSERT(m == 4U);

	/* BIS subvent 3 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 22U);
	LL_ASSERT(m == 22U);

	/* BIS subvent 4 */
	m = lll_chan_iso_subevent(chan_id, chan_map_1, chan_map_1_37_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 8U);
	LL_ASSERT(m == 8U);

	/* Section 3.1 Sample Data 2 (9 used channels) */
	/* BIS subevent 1, event counter 6 */
	m = lll_chan_iso_event(6, chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT((prn_s ^ chan_id) == 10975);
	LL_ASSERT(remap_idx == 4U);
	LL_ASSERT(m == 23U);

	/* BIS subvent 2 */
	m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 7U);
	LL_ASSERT(m == 35U);

	/* BIS subvent 3 */
	m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 2U);
	LL_ASSERT(m == 21U);

	/* BIS subvent 4 */
	m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 8U);
	LL_ASSERT(m == 36U);

	/* BIS subevent 1, event counter 7 */
	m = lll_chan_iso_event(7, chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT((prn_s ^ chan_id) == 5490);
	LL_ASSERT(remap_idx == 0U);
	LL_ASSERT(m == 9U);

	/* BIS subvent 2 */
	m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 3U);
	LL_ASSERT(m == 22U);

	/* BIS subvent 3 */
	m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 8U);
	LL_ASSERT(m == 36U);

	/* BIS subvent 4 */
	m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 5U);
	LL_ASSERT(m == 33U);

	/* BIS subevent 1, event counter 8 */
	m = lll_chan_iso_event(8, chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT((prn_s ^ chan_id) == 46970);
	LL_ASSERT(remap_idx == 6U);
	LL_ASSERT(m == 34U);

	/* BIS subvent 2 */
	m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 0U);
	LL_ASSERT(m == 9U);

	/* BIS subvent 3 */
	m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 5U);
	LL_ASSERT(m == 33U);

	/* BIS subvent 4 */
	m = lll_chan_iso_subevent(chan_id, chan_map_2, chan_map_2_9_used, &prn_s, &remap_idx);
	LL_ASSERT(remap_idx == 1U);
	LL_ASSERT(m == 10U);
#endif /* CONFIG_BT_CTLR_BROADCAST_ISO */
}
#endif /* CONFIG_BT_CTLR_TEST */
#endif /* CONFIG_BT_CTLR_CHAN_SEL_2 */