/* Function for incrementing a queue index, and handle wrap-around.  */
static __INLINE uint8_t next_index(uint8_t index)
/* Function for incrementing a queue index, and handle wrap-around.  */
static __INLINE uint8_t next_index(uint8_t index)
{
    return (index < m_queue_size) ? (index + 1) : 0;
}