Отключить прерывания таймера в источнике ядра для переключения режимов в ARM

Question

У меня есть небольшая функция, которую я добавил в исходный код ядра (linux 3.0), которая выполняет некоторые задачи и прекрасно работает в режиме SVC. Но в режиме прерывания (то есть, когда прерывания по таймеру включены в ядре через local_irq_enable) это не работает. Поэтому я думаю о переходе в режим SVC перед выполнением своей функции. Я взял код для переключения режимов из одного из постов.

void myfunction(..some args)
{ 
    unsigned int oldmode = get_cpsr() & 0x1f;

    if (oldmode == 18) // Check for IRQ mode
        change_mode(MODE_SVC);
    // ...My code ....
    restore_mode(oldmode);
}

Определения следующие:

#define MODE_IRQ        0x12
#define MODE_SVC        0x13

#define change_mode(mode) asm("cps %0" : : "I"(mode))

#define restore_mode(mode) \
    mode &= 0x1f; \
    asm(" msr cpsr, %0\n" : : "r"(mode) : "cc")

static inline unsigned int get_cpsr(void)
{
    unsigned int val;

    __asm__ __volatile__ (" mrs %0, cpsr\n"
                          : "=r" (val));
    return val;
}

Даже после этого я не получаю правильный вывод. Выполнение функции где-то застревает. Я отладил адрес, где он застрял, и он указывает на эти символы System.map

__exception_text_start _stext asm_do_IRQ

Правильно ли мое использование функции. Можете ли вы предложить мне другое решение этой проблемы. Я запускаю этот образ на плате ARMv7. И, наконец, есть вызовы некоторых API-интерфейсов ядра, таких как printch() в моей функции после смены режима.