On an event-driven embedded platform the firmware will wait for incoming events and then handle them appropriately given the current state of the system. This can be summarised as such:

  • Idle while waiting for events
  • Event happens via Interrupt Service Routine (ISR)
  • Event is handled by program

While this initially looks straight forward there are various bits of plumbing that is necessary to facilitate the handling of the event. One way would be to add the event to an event queue, which can be implemented as a FIFO. The main loop would then extract the event and handle it as appropriate (See Appendix). You could handle the event inside the ISR itself, though ideally you want to be spending as little time in the ISR as possible. The FIFO allows events to be queued up and then handled in the order that they were delivered. There is no priority or urgency, whichever event came first will be handled first.

But what if you wanted your system to handle events based on urgency? Enter the Priority Queue!

What is a Priority Queue

A Priority Queue is a partially sorted data structure where the first element is guaranteed to have the highest priority. It is typically implemented using the Heap data structure which itself can be implemented using an array1. There is plenty of literature1,2,3 out there explaining the mechanics of PQs/Heaps and how to implement them but the crucial part to know is that the elements are sorted by a key, where the highest (or lowest) value has the highest priority. For this example I’ve used a min-heap, where the lowest key value indicates the highest priority.

Implementation

As the priority queue is associating data with each key a data structure is necessary to group the information:

typedef struct
{
    uint32_t data;
    event_t event;
    /* Anything else */
}
pq_data_t;

This data structure is then used to define the heap at compile time with a fixed length. Along with some additional information the priority queue can be implemented using the following data structure:

#define HEAP_LEN (16U)

typedef struct 
{
    /* An array of pointers which is the actual priority queue */
    heap_data_t * heap[HEAP_LEN];

    /* Buffer that holds the heap data. */
    heap_data_t buffer[HEAP_LEN];
    
    /* Number of items currently in the
     * heap
     */
    uint32_t fill;

    /* Maximum number of values that can be stored
     * in the heap
     */
    uint32_t max;
}
heap_t;

The struct contains both an array of heap_data_t (which holds all the data currently on the heap) as well as an array of heap_data_t * which is an array of pointers pointing to each element in buffer at initialisation time. The initialisation would require the following step:

extern void Heap_Init(heap_t * heap)
{
    /* Other init code */
    ...

    /* Populate the heap by pointing to each
     * element in the buffer.
     */
    for(uint32_t idx = 0; idx < HEAP_LEN; idx++)
    {
        heap->heap[idx] = &heap->buffer[idx];
    }
}

The heap can then be ordered simply by swapping pointers to the elements of buffer depending on the dereferenced key value. The reason for this is depending on the size of your heap_data_t struct, it is arguably faster to simply swap pointers in the heap rather than full structs when re-ordering is necessary. The swap function can be implemented like this:

static void swap( heap_data_t ** a, heap_data_t ** b )
{
    /* Is the heap pointing to a valid pointer? */
    ASSERT(a != NULL);
    ASSERT(b != NULL);
    
    /* Is the element valid? */
    ASSERT(*a != NULL);
    ASSERT(*b != NULL);

    /* Swap pointers in the heap */
    heap_data_t * temp = *a;
    *a = *b;
    *b = temp;
}

With reference to 2, a minimal implementation of the priority queue can be completed by adding the following functionality:

  • Peek()
    • Look at the top of the heap without removing it.
extern uint32_t Heap_Peek(heap_t * heap)
{
    ASSERT(heap != NULL);
    ASSERT(heap->fill > 0U);
    ASSERT(heap->heap[0] != NULL);

    return heap->heap[0U]->key;
}
  • Push()
    • Add a new element to the queue and re-order.
    • Element is added to the bottom and swims to the appropriate position2.
extern void Heap_Push(heap_t * heap, heap_data_t data)
{
    ASSERT(heap != NULL);
    ASSERT(heap->fill < heap->max);

    /* Add the value to the bottom of the heap */
    *heap->heap[heap->fill] = data;

    /* Starting from the bottom of the heap, 
     * swim through the heap to ensure heap 
     * order is correct 
     */
    uint32_t idx = heap->fill;
    while( idx > 0U )
    {
        uint32_t parent = (idx - 1) >> 1U;

        /* Does the current node have a higher priority
         * than its parent?
         */
        if( heap->heap[idx]->key < heap->heap[parent]->key )
        {
            swap(&heap->heap[idx], &heap->heap[parent]);
        }
        idx = parent;
    }
    heap->fill++; 
}
  • Pop()
    • Remove top and re-order.
    • Place bottom of the heap at the top and sink through to the appropriate position2.
extern heap_data_t Heap_Pop(heap_t * heap)
{
    ASSERT(heap != NULL);
    ASSERT(heap->fill > 0U);

    /* Copy the data contained at the head of
     * the heap.
     */
    heap_data_t top = *heap->heap[0];

    /* Place bottom of heap at top */
    swap(&heap->heap[0], &heap->heap[heap->fill - 1U]);
    heap->fill--;

    /* Sink through the heap to ensure highest
     * priority element is placed at the head
     */
    uint32_t idx = 0U;
    uint32_t jdx = (idx << 1U) + 1U;

    while(jdx < heap->fill)
    {
        if(heap->heap[jdx]->key > heap->heap[jdx+1]->key)
        {
            jdx++;
        }

        if(heap->heap[idx]->key > heap->heap[jdx]->key)
        {
            swap(&heap->heap[idx], &heap->heap[jdx]);
            idx = jdx;
            jdx = (idx << 1U) + 1U;
        }
        else
        {
            break;
        }
    }

    return top;
}
  • IsEmpty() / IsFull()
    • Query the state of the queue to determine whether elements can be popped or pushed.
extern bool Heap_IsFull(heap_t * heap)
{
    ASSERT(heap != NULL);
    return (heap->fill < heap->max);
}

extern bool Heap_IsEmpty(heap_t * heap)
{
    ASSERT(heap != NULL);
    return (heap->fill == 0U);
}

Timer Implementation

In this example I’ll be using a priority queue to emit events to a state machine after a given number of milliseconds. The key will store the number of milliseconds in the future to emit the event, thus the sooner the event the higher the priority. The microcontroller’s timer peripheral will be used in Compare Mode to facilitate and drive the implementation of the priority queue. The timer will be configured (via the prescalar) to increment its counter every 1ms, with a rollover value of 0xFFFF (assuming a 16/32 bit timer). When configured in Compare mode, the timer will generate an interrupt when the counter (CNT) value matches the value currently stored in the Compare (CCR) register. In order to drive the priority queue, the Compare register will be loaded with a counter value (the key) from the head of the priority queue which will subsequently trigger the associated event when the counter (CNT) matches.

Timer peripherals with Compare Mode/Capture Compare are generally ubiquitous in the world of microcontrollers4,5. I’m using an STM32L432KC Nucleo that I had around, so the configuration of the timer (TIM2 specifically) can be implemented using the following code5:

static void Timer_Configure(void)
{

    /* The prescalar is used to divide the Timer's base frequency
     * so that the counter increments every 1ms.
     *
     * Timer's default clock is 4MHz, so need to divide by 4000
     * to get a 1ms tick. Note: The datasheet states it divides
     * by PSC + 1, hence 3999 as the value.
     */
    TIM2->PSC = 3999;

    /* Set the timer auto-reload value to 0xFFFF.
     * When the counter reaches this value it
     * resets back to 0
     */
    TIM2->ARR = 0xFFFF;

    /* Prescalar and Auto-Reload values do not propagate
     * until a timer update event has happened, so explicitly
     * force an update */
    TIM2->EGR |= (0x1 << 0U);

    /* Explicitly set the counter to 0 */
    TIM2->CNT = 0; 
    
    /* Enable interrupt in NVIC */
    NVIC_Enable(ISR_TIMER2);

    /* Enable update event interrupt (counter overflow) */
    TIM2->DIER |= (0x1 << 0U);

    /* Start Timer */
    TIM2->CR1 |= (0x1 << 0U);
}

Upon running this code Timer 2 will start ticking away from 0 to 0xFFFF at a rate of 1kHz. Upon a rollover an Update Event will occur and is handled by the ISR. In order to generate an interrupt to dispatch events it is necessary to create a function which ‘primes’ the CCR value and enables the corresponding interrupt. The priming of the Compare register needs to happen inside a critical section in case the interrupt is triggered while it is being updated. As the timer only counts from 0 to 0xFFFF, the timeout needs to be AND’d with 0xFFFF to wrap it within this range. The timeout is allowed to exceed this range prior to priming the register as wrapping it beforehand would erroneously cause it to be higher up the priority queue than it should. For example:

  • Current counter value is 65280 (0xFF00)
  • The next event Event_A is scheduled to elapse at 65535 (0xFFFF), which is 255ms in the future.
  • A new event Event_B is scheduled for 1000ms ahead:
    • 65280 + 1000 = 68280 (0x102E8)
      • If this value is wrapped to 16 bit prior to storing in the priority queue, then Event_B will erroneously be at the head of the priority queue, as the value will have wrapped to 744 (0x2e8), which in a min-heap would have a higher priority than 65535 (0xFFFF)
      • If instead we store it as 32 bit, then Event_A will still be the next event as 65535 < 65280.

There is also the scenario where the 16-bit counter overflows while there are events in the priority queue that haven’t been handled. For example:

  • Current counter value is 65280 (0xFF00)
  • The next event Event_C is added to the queue to emit 1000ms in the future.
    • This would be stored as 68280 (0x102E8) in the heap.
  • Counter wraps round to 0 (0x0000).
  • Event_D is added to the queue to emit 1500ms in the future.
    • This would be stored as 1500 (0x05DC) in the heap.
  • This would erroneously cause Event_D to happen before Event_C. Event_C is supposed to occur first when the counter reaches 744 (0x2e8), but it is stored in the heap unwrapped.

The solution to this is to go through the heap and wrap all the elements when an overflow occurs:

static void Heap_HandleOverflow(heap_t * heap)
{
    for(uint32_t idx = 0; idx < heap->fill; idx++)
    {
        heap->heap[idx]->key &= 0xFFFF;
    }
}

The function to ‘prime’ the compare match functionality of the timer can then be defined as follows:

/* NOTE: needs to be called in a critical section block */
static void Timer_PrimeCompare(uint32_t timeout)
{
    /* Prime Compare Match register */
    TIM2->CCR1 = timeout & 0xFFFF;

    /* Enable Interrupt for Compare mode */
    TIM2->DIER |= ( 0x1 << 1U );
}

An interrupt handler is also necessary to handle the Compare-Match and Counter Overflow events. This ISR will be used to push the given event to the event queue which can then be handled in the main execution loop. It is important to clarify that upon a Compare Match the event emitted is a generic priority queue timeout event, the actual Pop() of the timed event will be handled in the state machine in order to keep the interrupt code to a minimum.

void  __attribute__((interrupt("IRQ"))) Timer2_ISR( void )
{
    
    /* Has an overflow occurred? */
    if((TIM2->SR & (0x1 << 0)))
    {
        ENTER_CRITICAL();
        FIFO_Enqueue( &events, EVENT(TimerOverflow));
        EXIT_CRITICAL();

        /* Clear status register */
        TIM2->SR &= ~( 0x1 << 0U );
    }
    
    if((TIM2->SR & (0x1 << 1)))
    {
        ENTER_CRITICAL();

        /* This function would add the timeout event
         * to an event queue, which will subsequently
         * be handled during the main execution loop
         */
        FIFO_Enqueue( &events, EVENT(PQTimeout));
        
        EXIT_CRITICAL();
        TIM2->SR &= ~( 0x1 << 1U );
    }

    /* Clear / Acknowledge interrupt event
     * in NVIC 
     */
    NVIC_Clear(ISR_TIMER2);
}

All that remains is functionality for pushing and popping prioritised events and reconfiguring the timer as appropriate. Here the function is named EmitEventAfter_ms() to indicate that the event will be emitted by the timer after time_ms milliseconds. It is worth emphasising that this is stored as a uint16_t as the maximum timeout that can be requested is 0xFFFF, ie 65535ms.

static void EmitEventAfter_ms(event_t e, uint16_t time_ms)
{
    /* Get the current value in milliseconds
     * from the timer
     */
    uint32_t current_tick = TIM2->CNT;
    
    /* The timeout is the value of 
     * TIM2->CNT where the event is to be 
     * emitted, so add time_ms to the
     * current tick.
     */
    uint32_t timeout = current_tick + (uint32_t)time_ms;
    
    /* Put data in struct so it can be
     * enqueued
     */
    heap_data_t data =
    {
        .key = timeout,
        .event = e,
    };

    ENTER_CRITICAL();
    Heap_Push(&heap, data);

    /* Adding a new element to the
     * queue may change the head, so
     * peek the value and modify the
     * compare match register.
     */
    uint32_t peek_ms = Heap_Peek(&heap);
    Timer_PrimeCompare(peek_ms);
    
    EXIT_CRITICAL();
}

Finally, in the state machine the event can be extracted and subsequently added to the event queue:

/* This switch statement would typically be
 * inside a given state and handle the various
 * events of the system
 */
switch( event )
{
    ...
    case EVENT(TimerOverflow):
    {
        ENTER_CRITICAL();
        Heap_HandleOverflow(&heap);
        EXIT_CRITICAL();

        break;
    }
    case EVENT(PQTimeout):
    {
        ENTER_CRITICAL();
        heap_data_t data = Heap_Pop(&heap); 
        if(Heap_IsEmpty(&heap))
        {
            /* If the heap is empty, turn off
             * compare match interrupts 
             */
            TIM2->DIER &= ~( 0x1 << 1U );
        }
        else
        {
            /* If heap ISN'T empty, then peek
             * the head and re-arm
             */
            uint32_t peek_ms = Heap_Peek(&heap);
            Timer_PrimeCompare(peek_ms);
        }
        EXIT_CRITICAL(); 
        
        ENTER_CRITICAL();

        /* Add the newly popped event from the
         * priority queue to the state machine 
         * event FIFO
         */
        FIFO_Enqueue( &events, data.event);
        EXIT_CRITICAL(); 

        break;
    }
    ...
}

Demo

In order to demonstrate the timer driven priority queue a test program was created that enqueued an event at different timeouts which would toggle a GPIO pin when handled by the state machine. This means that the timing can be checked with a logic analyser to ensure it is correct.

...

/* Event that toggles the GPIO */
case EVENT(GPIOToggle):
{
    GPIO_B->ODR ^= (1 << PIN);
    break;
}

...

The following code snippet was used to add GPIOToggle events to the priority queue to be handled at different timeouts. When the code is executed on target it would be expected to see GPIO toggles at the following intervals:

  1. 125ms
  2. 126ms (+1ms)
  3. 250ms (+124ms)
  4. 1500ms (+1250ms)
  5. 2000ms (+500ms)
/* Set GPIO High so it can be used to measure
 * GPIO toggle events
 */

GPIO_B->ODR |= (1 << PIN);

/* Add events to priority queue */
EmitEventAfter_ms(EVENT(GPIOToggle), 2000U);
EmitEventAfter_ms(EVENT(GPIOToggle), 125U);
EmitEventAfter_ms(EVENT(GPIOToggle), 126U);
EmitEventAfter_ms(EVENT(GPIOToggle), 1500U);
EmitEventAfter_ms(EVENT(GPIOToggle), 250U);

Using an old logic analyser I measured the time between GPIO toggles to see if the timer driven priority queue is working correctly. I’ve colour coded the areas to make it easier to see.

pq

The table below summarises the measurements and shows the error between expected time and what was actually measured:

Expected Time(ms) Measured Time(ms) Error (%)
125 126.9 1.50
126 127.9 1.49
250 253.7 1.47
1500 1521.7 1.43
2000 2028.8 1.42

The average error across the toggles is 1.46%. This is likely due to the latency between the Compare event and the actual GPIO toggle when the event is being handled inside the state machine. The error is always slightly longer than the expected timeout which further indicates this is the case. It is beyond the scope of this article but there are further improvements such as increasing the core clock, optimising the code etc that might reduce this latency.

Conclusion

This article aimed to discuss and demonstrate implementing a priority queue as a way to handle time sensitive events on an embedded platform. Definitely a lot of room for improvement and optimisation, but nonetheless a useful experiment for implementing an event driven system.

Appendix

Below is an example of what a main execution loop might look like in an event / interrupt driven embedded system. More info about the __asm("wfi") instruction can be found here 6 and assumes the target is ARM based. I highly recommend reading some of Miro Samek’s work on state machines for further insight into designing and implementing event driven systems7,8.

while(1)
{
    while( FIFO_IsEmpty( (fifo_base_t*)&events ) )
    {
        /* If event queue is empty there is 
         * nothing to do, so Wait for Interrupt
         * (WFI)
         */
        __asm("wfi");
    }
    
    /* Retrieve latest event from queue */
    ENTER_CRITICAL();
    event_t e = FIFO_Dequeue( &events );
    EXIT_CRITICAL();

    /* Handle event in state machine */
    STATEMACHINE_HandleEvent(&state_machine, e);
}

  1. Wikipedia - Heap (data structure) link ↩ ↩2

  2. Algorithms 4th Edition - Priority Queues link ↩ ↩2 ↩3 ↩4

  3. Wikipedia - Priority Queue link ↩

  4. Microchip: Capture/Compare/PWM (CCP)/Enhanced PWM (ECCP) Peripheral link ↩

  5. RM 0394 Reference Manual link ↩ ↩2

  6. What is the purpose of WFI and WFE instructions and the event signals? link ↩

  7. Introduction to Hierarchical State Machines link ↩

  8. Modern Embedded Programming Course link ↩