#include <clock-enable-manager.h>
#include <stdlib.h>

#define COUNT_OF(x) ((sizeof(x)/sizeof(0[x])) / ((size_t)(!(sizeof(x) % sizeof(0[x])))))

struct rcc_enable_count {
	uint32_t *rcc_reg_addr;
	uint32_t *enable_bit_cnt;
	uint8_t rcc_enable_bit_pos;
};

#if RCC_ENABLE_MANAGER_STATIC

static struct rcc_enable_count enable_count_list[RCC_ENABLE_MANAGER_COUNT] = {0};

#else
#error "RCC manager with dynamic memory not implemented!"
#endif

#if RCC_ENABLE_MANAGER_STATIC
static struct rcc_enable_count *search_enable_entry_in_list(uint32_t *reg_addr, uint8_t bit_pos)
{
	int i;
	struct rcc_enable_count *ret_element = NULL;
	struct rcc_enable_count *current_element;

	for (i = 0; i < COUNT_OF(enable_count_list); i++) {
		current_element = &enable_count_list[i];

		/* Check if register address and bit position match */
		if (reg_addr != current_element->rcc_reg_addr)
			continue;
		if (bit_pos != current_element->rcc_enable_bit_pos)
			continue;

		/* Found entry. Wohoo! */
		ret_element = current_element;
	}

	return ret_element;
}

static struct rcc_enable_count *enable_entry_list_get_free_entry()
{
	struct rcc_enable_count *ret_ptr = NULL;
	const int list_len = COUNT_OF(enable_count_list);
	int i;

	for (i = 0; i < list_len; i++) {
		if (enable_count_list[i].rcc_reg_addr == NULL) {
			ret_ptr = &enable_count_list[i];

			/* Clear the count value to be safe */
			ret_ptr->enable_bit_cnt = 0;

			break;
		}
	}

	return ret_ptr;
}

static void enable_entry_list_remove_entry(struct rcc_enable_count *entry)
{
	if (!entry)
		return;

	entry->rcc_reg_addr = NULL;
	entry->enable_bit_cnt = 0;
	entry->rcc_enable_bit_pos = 0;
}
#endif

int rcc_manager_enable_clock(uint32_t *rcc_enable_register, uint8_t bit_no)
{
	int ret_val = 0;
	struct rcc_enable_count *entry;

	if (!rcc_enable_register || bit_no > 31) {
		return -1000;
	}

	/* Enable the clock in any case, no matter what follows */
	*rcc_enable_register |= (1U<<bit_no);

	/* Check if bit is already in list */
	entry = search_enable_entry_in_list(rcc_enable_register, bit_no);

	if (!entry) {
		/* Create ne entry at first free place in list */
		entry = enable_entry_list_get_free_entry();

		/* Check if entry is valid. If not return */
		if (!entry) {
			ret_val = -1;
			goto ret_error_code;
		}

		/* Set entry */
		entry->rcc_reg_addr = rcc_enable_register;
		entry->rcc_enable_bit_pos = bit_no;
	}

	/* Increment enable counter */
	entry->enable_bit_cnt++;

ret_error_code:
	return ret_val;
}

int rcc_manager_disable_clock(uint32_t *rcc_enable_register, uint8_t bit_no)
{
	int ret_val = -1;
	struct rcc_enable_count *entry;

	if (!rcc_enable_register || bit_no > 31) {
		return -1000;
	}

	entry = search_enable_entry_in_list(rcc_enable_register, bit_no);

	/* If entry is found and has a count of zero, disable clock */
	if (entry) {
		/* Found entry => Decrement count and delete if zero */
		entry->enable_bit_cnt--;
		if (entry->enable_bit_cnt <= 0) {
			enable_entry_list_remove_entry(entry);

			/* Disable clock */
			*rcc_enable_register &= ~(1U<<bit_no);
		}

		ret_val = 0;
	}

	return ret_val;
}