136 lines
3.0 KiB
C
136 lines
3.0 KiB
C
#include <clock-enable-manager.h>
|
|
#include <helper-macros/helper-macros.h>
|
|
#include <stdlib.h>
|
|
|
|
struct rcc_enable_count {
|
|
volatile 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(volatile 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(volatile 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(volatile 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;
|
|
}
|