/*
 * Copyright (c) 2016 Nordic Semiconductor ASA
 * Copyright (c) 2016 Vinayak Kariappa Chettimada
 *
 * SPDX-License-Identifier: Apache-2.0
 */
#include <string.h>

#include <zephyr/arch/cpu.h>
#include <zephyr/types.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/drivers/entropy.h>

#include "util.h"
#include "util/memq.h"

#include "ll_sw/lll.h"

#include "ll_sw/pdu_df.h"
#include "lll/pdu_vendor.h"
#include "ll_sw/pdu.h"

/**
 * @brief Population count: Count the number of bits set to 1
 * @details
 * TODO: Faster methods available at [1].
 * [1] http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
 *
 * @param octets     Data to count over
 * @param octets_len Must not be bigger than 255/8 = 31 bytes
 *
 * @return popcnt of 'octets'
 */
uint8_t util_ones_count_get(const uint8_t *octets, uint8_t octets_len)
{
	uint8_t one_count = 0U;

	while (octets_len--) {
		uint8_t bite;

		bite = *octets;
		while (bite) {
			bite &= (bite - 1);
			one_count++;
		}
		octets++;
	}

	return one_count;
}

/** @brief Prepare access address as per BT Spec.
 *
 * - It shall have no more than six consecutive zeros or ones.
 * - It shall not be the advertising channel packets' Access Address.
 * - It shall not be a sequence that differs from the advertising channel
 *   packets Access Address by only one bit.
 * - It shall not have all four octets equal.
 * - It shall have no more than 24 transitions.
 * - It shall have a minimum of two transitions in the most significant six
 *   bits.
 *
 * LE Coded PHY requirements:
 * - It shall have at least three ones in the least significant 8 bits.
 * - It shall have no more than eleven transitions in the least significant 16
 *   bits.
 */
int util_aa_le32(uint8_t *dst)
{
#if defined(CONFIG_BT_CTLR_PHY_CODED)
	uint8_t transitions_lsb16;
	uint8_t ones_count_lsb8;
#endif /* CONFIG_BT_CTLR_PHY_CODED */
	uint8_t consecutive_cnt;
	uint8_t consecutive_bit;
	uint32_t adv_aa_check;
	uint32_t aa;
	uint8_t transitions;
	uint8_t bit_idx;
	uint8_t retry;

	retry = 3U;
again:
	if (!retry) {
		return -EFAULT;
	}
	retry--;

	lll_csrand_get(dst, sizeof(uint32_t));
	aa = sys_get_le32(dst);

	bit_idx = 31U;
	transitions = 0U;
	consecutive_cnt = 1U;
#if defined(CONFIG_BT_CTLR_PHY_CODED)
	ones_count_lsb8 = 0U;
	transitions_lsb16 = 0U;
#endif /* CONFIG_BT_CTLR_PHY_CODED */
	consecutive_bit = (aa >> bit_idx) & 0x01;
	while (bit_idx--) {
#if defined(CONFIG_BT_CTLR_PHY_CODED)
		uint8_t transitions_lsb16_prev = transitions_lsb16;
#endif /* CONFIG_BT_CTLR_PHY_CODED */
		uint8_t consecutive_cnt_prev = consecutive_cnt;
		uint8_t transitions_prev = transitions;
		uint8_t bit;

		bit = (aa >> bit_idx) & 0x01;
		if (bit == consecutive_bit) {
			consecutive_cnt++;
		} else {
			consecutive_cnt = 1U;
			consecutive_bit = bit;
			transitions++;

#if defined(CONFIG_BT_CTLR_PHY_CODED)
			if (bit_idx < 15) {
				transitions_lsb16++;
			}
#endif /* CONFIG_BT_CTLR_PHY_CODED */
		}

#if defined(CONFIG_BT_CTLR_PHY_CODED)
		if ((bit_idx < 8) && consecutive_bit) {
			ones_count_lsb8++;
		}
#endif /* CONFIG_BT_CTLR_PHY_CODED */

		/* It shall have no more than six consecutive zeros or ones. */
		/* It shall have a minimum of two transitions in the most
		 * significant six bits.
		 */
		if ((consecutive_cnt > 6) ||
#if defined(CONFIG_BT_CTLR_PHY_CODED)
		    (!consecutive_bit && (((bit_idx < 6) &&
					   (ones_count_lsb8 < 1)) ||
					  ((bit_idx < 5) &&
					   (ones_count_lsb8 < 2)) ||
					  ((bit_idx < 4) &&
					   (ones_count_lsb8 < 3)))) ||
#endif /* CONFIG_BT_CTLR_PHY_CODED */
		    ((consecutive_cnt < 6) &&
		     (((bit_idx < 29) && (transitions < 1)) ||
		      ((bit_idx < 28) && (transitions < 2))))) {
			if (consecutive_bit) {
				consecutive_bit = 0U;
				aa &= ~BIT(bit_idx);
#if defined(CONFIG_BT_CTLR_PHY_CODED)
				if (bit_idx < 8) {
					ones_count_lsb8--;
				}
#endif /* CONFIG_BT_CTLR_PHY_CODED */
			} else {
				consecutive_bit = 1U;
				aa |= BIT(bit_idx);
#if defined(CONFIG_BT_CTLR_PHY_CODED)
				if (bit_idx < 8) {
					ones_count_lsb8++;
				}
#endif /* CONFIG_BT_CTLR_PHY_CODED */
			}

			if (transitions != transitions_prev) {
				consecutive_cnt = consecutive_cnt_prev;
				transitions = transitions_prev;
			} else {
				consecutive_cnt = 1U;
				transitions++;
			}

#if defined(CONFIG_BT_CTLR_PHY_CODED)
			if (bit_idx < 15) {
				if (transitions_lsb16 !=
				    transitions_lsb16_prev) {
					transitions_lsb16 =
						transitions_lsb16_prev;
				} else {
					transitions_lsb16++;
				}
			}
#endif /* CONFIG_BT_CTLR_PHY_CODED */
		}

		/* It shall have no more than 24 transitions
		 * It shall have no more than eleven transitions in the least
		 * significant 16 bits.
		 */
		if ((transitions > 24) ||
#if defined(CONFIG_BT_CTLR_PHY_CODED)
		    (transitions_lsb16 > 11) ||
#endif /* CONFIG_BT_CTLR_PHY_CODED */
		    0) {
			if (consecutive_bit) {
				aa &= ~(BIT(bit_idx + 1) - 1);
			} else {
				aa |= (BIT(bit_idx + 1) - 1);
			}

			break;
		}
	}

	/* It shall not be the advertising channel packets Access Address.
	 * It shall not be a sequence that differs from the advertising channel
	 * packets Access Address by only one bit.
	 */
	adv_aa_check = aa ^ PDU_AC_ACCESS_ADDR;
	if (util_ones_count_get((uint8_t *)&adv_aa_check,
				sizeof(adv_aa_check)) <= 1) {
		goto again;
	}

	/* It shall not have all four octets equal. */
	if (!((aa & 0xFFFF) ^ (aa >> 16)) &&
	    !((aa & 0xFF) ^ (aa >> 24))) {
		goto again;
	}

	sys_put_le32(aa, dst);

	return 0;
}

#if defined(CONFIG_BT_CTLR_ADV_ISO)
int util_saa_le32(uint8_t *dst, uint8_t handle)
{
	/* Refer to Bluetooth Core Specification Version 5.2 Vol 6, Part B,
	 * section 2.1.2 Access Address
	 */
	uint32_t saa, saa_15, saa_16;
	uint8_t bits;
	int err;

	/* Get access address */
	err = util_aa_le32(dst);
	if (err) {
		return err;
	}

	saa = sys_get_le32(dst);

	/* SAA_19 = SAA_15 */
	saa_15 = (saa >> 15) & 0x01;
	saa &= ~BIT(19);
	saa |= saa_15 << 19;

	/* SAA_16 != SAA_15 */
	saa &= ~BIT(16);
	saa_16 = ~saa_15 & 0x01;
	saa |= saa_16 << 16;

	/* SAA_22 = SAA_16 */
	saa &= ~BIT(22);
	saa |= saa_16 << 22;

	/* SAA_25 = 0 */
	saa &= ~BIT(25);

	/* SAA_23 = 1 */
	saa |= BIT(23);

	/* For any pair of BIGs transmitted by the same device, the SAA 15-0
	 * values shall differ in at least two bits.
	 * - Find the number of bits required to support 3 times the maximum
	 *   ISO connection handles supported
	 * - Clear those number many bits
	 * - Set the value that is 3 times the handle so that consecutive values
	 *   differ in at least two bits.
	 */
	bits = find_msb_set(CONFIG_BT_CTLR_ADV_ISO_SET * 0x03);
	saa &= ~BIT_MASK(bits);
	saa |= (handle * 0x03);

	sys_put_le32(saa, dst);

	return 0;
}
#endif /* CONFIG_BT_CTLR_ADV_ISO */

#if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_SYNC_ISO)
void util_bis_aa_le32(uint8_t bis, uint8_t *saa, uint8_t *dst)
{
	/* Refer to Bluetooth Core Specification Version 5.2 Vol 6, Part B,
	 * section 2.1.2 Access Address
	 */
	uint8_t dwh[2]; /* Holds the two most significant bytes of DW */
	uint8_t d;

	/* 8-bits for d is enough due to wrapping math and requirement to do
	 * modulus 128.
	 */
	d = ((35 * bis) + 42) & 0x7f;

	/* Most significant 6 bits of DW are bit extension of least significant
	 * bit of D.
	 */
	if (d & 1) {
		dwh[1] = 0xFC;
	} else {
		dwh[1] = 0;
	}

	/* Set the bits 25 to 17 of DW */
	dwh[1] |= (d & 0x02) | ((d >> 6) & 0x01);
	dwh[0] = ((d & 0x02) << 6) | (d & 0x30) | ((d & 0x0C) >> 1);

	/* Most significant 16-bits of SAA XOR DW, least significant 16-bit are
	 * zeroes, needing no operation on them.
	 */
	memcpy(dst, saa, sizeof(uint32_t));
	dst[3] ^= dwh[1];
	dst[2] ^= dwh[0];
}
#endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_SYNC_ISO*/

/** @brief Get a bit aligned value from a byte array
 *  Converts bitsets to any size variable (<= 32 bit), which is returned
 *  as a uint32_t value.
 *
 *  @param data     Pointer to bytes containing the requested value
 *  @param bit_offs Bit offset into data[0] for value LSB
 *  @param num_bits Number of bits to extract and convert to value
 */
uint32_t util_get_bits(uint8_t *data, uint8_t bit_offs, uint8_t num_bits)
{
	uint32_t value;
	uint8_t  shift, byteIdx, bits;

	value = 0;
	shift = 0;
	byteIdx = 0;

	while (num_bits) {
		bits = MIN(num_bits, 8 - bit_offs);
		value |= ((data[byteIdx] >> bit_offs) & BIT_MASK(bits)) << shift;
		shift += bits;
		num_bits -= bits;
		bit_offs = 0;
		byteIdx++;
	}

	return value;
}

/** @brief Set a bit aligned value in a byte array
 *  Converts a value up to 32 bits to a bitset in a byte array.
 *
 *  @param data     Pointer to bytes in which to place the value
 *  @param bit_offs Bit offset into data[0] for value LSB
 *  @param num_bits Number of bits to set in data
 */
void util_set_bits(uint8_t *data, uint8_t bit_offs, uint8_t num_bits,
		   uint32_t value)
{
	uint8_t byteIdx, bits;

	byteIdx = 0;

	while (num_bits) {
		bits = MIN(num_bits, 8 - bit_offs);
		data[byteIdx] = (data[byteIdx] & ~(BIT_MASK(bits) << bit_offs)) |
				((value & BIT_MASK(bits)) << bit_offs);
		value >>= bits;
		num_bits -= bits;
		bit_offs = 0;
		byteIdx++;
	}
}