51 lines
1007 B
C++
51 lines
1007 B
C++
#include "stm.hpp"
|
|
|
|
using Callback = void(*)(void);
|
|
static constexpr uint8_t numCallbacks = 10;
|
|
static Callback callbacks[numCallbacks];
|
|
uint8_t read_index, write_index;
|
|
|
|
static void increment(uint8_t &index) {
|
|
if(index < numCallbacks - 1) {
|
|
index++;
|
|
} else {
|
|
index = 0;
|
|
}
|
|
}
|
|
static uint8_t callbackFifoLevel() {
|
|
int8_t level = (int8_t) write_index - (int8_t) read_index;
|
|
if(level < 0) {
|
|
level += numCallbacks;
|
|
}
|
|
return (uint8_t) level;
|
|
}
|
|
|
|
void STM::Init() {
|
|
read_index = write_index = 0;
|
|
HAL_NVIC_SetPriority(COMP4_IRQn, 6, 0);
|
|
HAL_NVIC_EnableIRQ(COMP4_IRQn);
|
|
}
|
|
|
|
bool STM::DispatchToInterrupt(void (*cb)(void)) {
|
|
if(callbackFifoLevel() < numCallbacks - 1) {
|
|
callbacks[write_index] = cb;
|
|
increment(write_index);
|
|
HAL_NVIC_SetPendingIRQ(COMP4_IRQn);
|
|
return true;
|
|
} else {
|
|
// already at limit
|
|
return false;
|
|
}
|
|
}
|
|
|
|
extern "C" {
|
|
void COMP4_IRQHandler() {
|
|
while(callbackFifoLevel() > 0) {
|
|
if (callbacks[read_index]) {
|
|
callbacks[read_index]();
|
|
}
|
|
increment(read_index);
|
|
}
|
|
}
|
|
}
|