static inline void udelay(unsigned usecs)
{
    unsigned cycles_per_usec;
    unsigned loops_per_usec;

    if (cpu_frequency == CPUFREQ_MAX) {
        cycles_per_usec = (CPUFREQ_MAX + 999999) / 1000000;
    } else {
        cycles_per_usec = (CPUFREQ_NORMAL + 999999) / 1000000;
    }

    /*
     * One iteration of the inner loop takes 4 cycles,
     * sustract 1 to account for the outer loop.
     */
    loops_per_usec = (cycles_per_usec+3)/4 - 1;

    asm volatile(
        "2: mov  r0, %1      \n"
        "1: subs r0, r0, #1  \n"
        "   bne  1b          \n"
        "   subs %0, %0, #1  \n"
        "   bne  2b          \n"
        : : "r"(usecs), "r"(loops_per_usec) : "r0"
    );
}