81 lines
2.8 KiB
C
81 lines
2.8 KiB
C
|
|
typedef struct reg {
|
|
volatile uint32_t *address;
|
|
uint32_t value;
|
|
uint32_t check_mask;
|
|
} reg;
|
|
|
|
// 10 bit hash with 23 as a prime
|
|
#define REGISTER_MAP_SIZE 0x3FFU
|
|
#define HASHING_PRIME 23U
|
|
#define CHECK_COLLISION(hash, addr) (((uint32_t) register_map[hash].address != 0U) && (register_map[hash].address != addr))
|
|
|
|
reg register_map[REGISTER_MAP_SIZE];
|
|
|
|
// Hash spread in first and second iterations seems to be reasonable.
|
|
// See: tests/development/register_hashmap_spread.py
|
|
// Also, check the collision warnings in the debug output, and minimize those.
|
|
uint16_t hash_addr(uint32_t input){
|
|
return (((input >> 16U) ^ ((((input + 1U) & 0xFFFFU) * HASHING_PRIME) & 0xFFFFU)) & REGISTER_MAP_SIZE);
|
|
}
|
|
|
|
// Do not put bits in the check mask that get changed by the hardware
|
|
void register_set(volatile uint32_t *addr, uint32_t val, uint32_t mask){
|
|
ENTER_CRITICAL()
|
|
// Set bits in register that are also in the mask
|
|
(*addr) = ((*addr) & (~mask)) | (val & mask);
|
|
|
|
// Add these values to the map
|
|
uint16_t hash = hash_addr((uint32_t) addr);
|
|
uint16_t tries = REGISTER_MAP_SIZE;
|
|
while(CHECK_COLLISION(hash, addr) && (tries > 0U)) { hash = hash_addr((uint32_t) hash); tries--;}
|
|
if (tries != 0U){
|
|
register_map[hash].address = addr;
|
|
register_map[hash].value = (register_map[hash].value & (~mask)) | (val & mask);
|
|
register_map[hash].check_mask |= mask;
|
|
} else {
|
|
#ifdef DEBUG_FAULTS
|
|
puts("Hash collision: address 0x"); puth((uint32_t) addr); puts("!\n");
|
|
#endif
|
|
}
|
|
EXIT_CRITICAL()
|
|
}
|
|
|
|
// Set individual bits. Also add them to the check_mask.
|
|
// Do not use this to change bits that get reset by the hardware
|
|
void register_set_bits(volatile uint32_t *addr, uint32_t val) {
|
|
return register_set(addr, val, val);
|
|
}
|
|
|
|
// Clear individual bits. Also add them to the check_mask.
|
|
// Do not use this to clear bits that get set by the hardware
|
|
void register_clear_bits(volatile uint32_t *addr, uint32_t val) {
|
|
return register_set(addr, (~val), val);
|
|
}
|
|
|
|
// To be called periodically
|
|
void check_registers(void){
|
|
for(uint16_t i=0U; i<REGISTER_MAP_SIZE; i++){
|
|
if((uint32_t) register_map[i].address != 0U){
|
|
ENTER_CRITICAL()
|
|
if((*(register_map[i].address) & register_map[i].check_mask) != (register_map[i].value & register_map[i].check_mask)){
|
|
#ifdef DEBUG_FAULTS
|
|
puts("Register at address 0x"); puth((uint32_t) register_map[i].address); puts(" is divergent!");
|
|
puts(" Map: 0x"); puth(register_map[i].value);
|
|
puts(" Register: 0x"); puth(*(register_map[i].address));
|
|
puts(" Mask: 0x"); puth(register_map[i].check_mask);
|
|
puts("\n");
|
|
#endif
|
|
fault_occurred(FAULT_REGISTER_DIVERGENT);
|
|
}
|
|
EXIT_CRITICAL()
|
|
}
|
|
}
|
|
}
|
|
|
|
void init_registers(void) {
|
|
for(uint16_t i=0U; i<REGISTER_MAP_SIZE; i++){
|
|
register_map[i].address = (volatile uint32_t *) 0U;
|
|
register_map[i].check_mask = 0U;
|
|
}
|
|
} |