Measuring RTT & RSSI through Radio States

Question

Hi everyone, for the project I'm developing it's extremely important to measure RTT as accurately as possible and as such, after some SoftDevice specification I imagined the following scenario:

Both devices would be connected and through the connection events packets that are exchanged periodically, I would generate interrupts from the SoftDevice upon reaching three specific states. Now, these interrupts would merely start / stop counters, hopefully, not ruining radio synchronization.

For the init the code is:

#define RADIO_IRQN_PRIORITY	        APP_IRQ_PRIORITY_LOW		/**< Radio Interrupt Request priority. */

// Timer Evt_handler is required not to be NULL, so here it is, a full fledge, state of the art event handler.
void ble_pds_counter_evt_handler(nrf_timer_event_t event_type, void * p_context)
{
    return;
}


/**@brief Function for the Timer initialization.
 *
 * @details Initializes the timer module.
 *
 */
static void timers_init(void)
{
    ret_code_t err_code = app_timer_init();
    APP_ERROR_CHECK(err_code);
    
    nrf_drv_timer_config_t counter_cfg = NRF_DRV_TIMER_DEFAULT_CONFIG; 
    err_code = nrf_drv_timer_init(&delay_counter,  &counter_cfg, ble_pds_counter_evt_handler);
    APP_ERROR_CHECK(err_code);
}

/**@brief Function for initializing the radio interrupts needed.
 */
static uint32_t ble_radio_notif_init(void)
{
    ret_code_t err_code;
    
    NRF_RADIO->INTENSET =   RADIO_INTENSET_ADDRESS_Enabled  << RADIO_INTENSET_ADDRESS_Pos   |
	            RADIO_INTENSET_DEVMATCH_Enabled << RADIO_INTENSET_DEVMATCH_Pos  |
	            RADIO_INTENCLR_CRCOK_Enabled    << RADIO_INTENCLR_CRCOK_Pos     |
	            RADIO_INTENCLR_DISABLED_Enabled << RADIO_INTENCLR_DISABLED_Pos;
	            
    
    // Initialize Radio Notification software interrupt
    err_code = sd_nvic_ClearPendingIRQ(RADIO_IRQn);
    APP_ERROR_CHECK(err_code);
    
    APP_IRQ_PRIORITY_LOW;
        
    err_code = sd_nvic_SetPriority(RADIO_IRQn, RADIO_IRQN_PRIORITY);
    APP_ERROR_CHECK(err_code);
        
    err_code = sd_nvic_EnableIRQ(RADIO_IRQn);
    APP_ERROR_CHECK(err_code);
        
    // Configure the event - ONLY SHOWS ON/OFF ?
    //return sd_radio_notification_cfg_set(NRF_RADIO_NOTIFICATION_TYPE_INT_ON_BOTH, distance);    
}

/**@brief Function for application main entry.
 */
int main(void)
{
    (...)
    ble_radio_notif_init();
    ble_stack_init();
    swi_init();
    (...)
    
    for (;;)
    {
        idle_state_handle();
    }
}

And the radio notification event handler (SWI is used to process both reading and notify central) :

void RADIO_IRQHandler(void)
{
    if (m_pds.state != 0 || m_pds.state == 4)
    {
        if   (NRF_RADIO->EVENTS_DEVMATCH &&
             m_pds.state == 1)
        {
            // Rx   DEVICE MATCH
            nrf_drv_timer_enable(&delay_counter);
            m_pds.state = 2;
        }  else if  (NRF_RADIO->EVENTS_CRCOK &&
	    m_pds.state == 2)
        {
            // Rx   CRC OK
            m_pds.state = 3;
            
        } else if  (NRF_RADIO->EVENTS_ADDRESS &&
	    NRF_RADIO->STATE == RADIO_STATE_STATE_Tx &&
	    m_pds.state == 3)
        {
            // Tx   ADDRESS
            nrf_drv_timer_pause(&delay_counter);
            m_pds.state = 4;    
        } else   
            // Invalid State
            m_pds.state = 0;
            
    } else if	(NRF_RADIO->EVENTS_DISABLED &&
	NRF_RADIO->STATE == RADIO_STATE_STATE_Tx)
    {
        // Tx Disable
        if (m_pds.state == 4)
        {
            // Good Sample
            m_pds.sample.p_rssi = NRF_RADIO->RSSISAMPLE;
            m_pds.sample.p_delay  = nrf_drv_timer_capture(&delay_counter, NRF_TIMER_CC_CHANNEL1);
            m_pds.state = 1;
            nrf_drv_swi_trigger(pds_swi, 3);
        } else 
            m_pds.state = 1;
        nrf_drv_timer_disable(&delay_counter);
    }
}

The problem arrives when testing the program, at exception is thrown when initiating the radio notifications and the program terminates.

Now upon reading some more from the SDS I discovered:

4.1
"Software triggered interrupts in a reserved IRQ are used to signal events from the SoftDevice to the
application. The application is then responsible for handling the interrupt and for invoking the relevant
SoftDevice functions to obtain the event data."

5.1
"Once the SoftDevice has been enabled, more restrictions apply:
(...)
Interrupts from the reserved SoftDevice peripherals will not be forwarded to the application. See
Interrupt forwarding to the application on page 85 for more details."

This means that what I want to do probably is not correct and hence the error thrown. I wonder what the most correct approach is...

Thank you,

Marco.