diff --git a/libcpu/arm/cortex-m3/cpuport.c b/libcpu/arm/cortex-m3/cpuport.c index b85e9a09563b72314a1e19ed084a17cea254971b..60757d6b3d3b30d29d049aab71d68c0fd638f930 100644 --- a/libcpu/arm/cortex-m3/cpuport.c +++ b/libcpu/arm/cortex-m3/cpuport.c @@ -148,3 +148,43 @@ void rt_hw_cpu_shutdown(void) RT_ASSERT(0); } +#ifdef RT_USING_CPU_FFS +/** + * This function finds the first bit set (beginning with the least significant bit) + * in value and return the index of that bit. + * + * Bits are numbered starting at 1 (the least significant bit). A return value of + * zero from any of these functions means that the argument was zero. + * + * @return return the index of the first bit set. If value is 0, then this function + * shall return 0. + */ +#if defined(__CC_ARM) +__asm int __rt_ffs(int value) +{ + CMP r0, #0x00 + BEQ exit + RBIT r0, r0 + CLZ r0, r0 + ADDS r0, r0, #0x01 + +exit + BX lr +} +#elif defined(__IAR_SYSTEMS_ICC__) +int __rt_ffs(int value) +{ + if (value == 0) return value; + + __ASM("RBIT r0, r0"); + __ASM("CLZ r0, r0"); + __ASM("ADDS r0, r0, #0x01"); +} +#elif defined(__GNUC__) +int __rt_ffs(int value) +{ + return __builtin_ffs(value); +} +#endif + +#endif diff --git a/src/kservice.c b/src/kservice.c index 4276fc28ecc18669e1df2c385245b346eaba22d4..67262ff08478432da34808797209877127063021 100644 --- a/src/kservice.c +++ b/src/kservice.c @@ -1190,6 +1190,54 @@ void rt_free_align(void *ptr) RTM_EXPORT(rt_free_align); #endif +#ifndef RT_USING_CPU_FFS +const rt_uint8_t __lowest_bit_bitmap[] = +{ + /* 00 */ 0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* 10 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* 20 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* 30 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* 40 */ 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* 50 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* 60 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* 70 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* 80 */ 7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* 90 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* A0 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* B0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* C0 */ 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* D0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* E0 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, + /* F0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 +}; + +/** + * This function finds the first bit set (beginning with the least significant bit) + * in value and return the index of that bit. + * + * Bits are numbered starting at 1 (the least significant bit). A return value of + * zero from any of these functions means that the argument was zero. + * + * @return return the index of the first bit set. If value is 0, then this function + * shall return 0. + */ +int __rt_ffs(int value) +{ + if (value == 0) return 0; + + if (value & 0xff) + return __lowest_bit_bitmap[value & 0xff] + 1; + + if (value & 0xff00) + return __lowest_bit_bitmap[(value & 0xff00) >> 8] + 9; + + if (value & 0xff0000) + return __lowest_bit_bitmap[(value & 0xff0000) >> 16] + 17; + + return __lowest_bit_bitmap[(value & 0xff000000) >> 24] + 25; +} +#endif + #if !defined (RT_USING_NEWLIB) && defined (RT_USING_MINILIBC) && defined (__GNUC__) #include void *memcpy(void *dest, const void *src, size_t n) __attribute__((weak, alias("rt_memcpy"))); diff --git a/src/scheduler.c b/src/scheduler.c index 03423c9f5defa182d3121d6196eb651d48b62d94..ad618d1a61567371f2e2253ab07aeb06d088b90c 100644 --- a/src/scheduler.c +++ b/src/scheduler.c @@ -31,6 +31,7 @@ static rt_int16_t rt_scheduler_lock_nest; extern volatile rt_uint8_t rt_interrupt_nest; +extern int __rt_ffs(int value); rt_list_t rt_thread_priority_table[RT_THREAD_PRIORITY_MAX]; struct rt_thread *rt_current_thread; @@ -48,26 +49,6 @@ rt_uint32_t rt_thread_ready_priority_group; rt_list_t rt_thread_defunct; -const rt_uint8_t rt_lowest_bitmap[] = -{ - /* 00 */ 0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* 10 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* 20 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* 30 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* 40 */ 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* 50 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* 60 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* 70 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* 80 */ 7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* 90 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* A0 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* B0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* C0 */ 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* D0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* E0 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, - /* F0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 -}; - #ifdef RT_USING_HOOK static void (*rt_scheduler_hook)(struct rt_thread *from, struct rt_thread *to); @@ -164,34 +145,13 @@ void rt_system_scheduler_start(void) register struct rt_thread *to_thread; register rt_ubase_t highest_ready_priority; -#if RT_THREAD_PRIORITY_MAX == 8 - highest_ready_priority = rt_lowest_bitmap[rt_thread_ready_priority_group]; -#else +#if RT_THREAD_PRIORITY_MAX > 32 register rt_ubase_t number; - /* find out the highest priority task */ - if (rt_thread_ready_priority_group & 0xff) - { - number = rt_lowest_bitmap[rt_thread_ready_priority_group & 0xff]; - } - else if (rt_thread_ready_priority_group & 0xff00) - { - number = rt_lowest_bitmap[(rt_thread_ready_priority_group >> 8) & 0xff] + 8; - } - else if (rt_thread_ready_priority_group & 0xff0000) - { - number = rt_lowest_bitmap[(rt_thread_ready_priority_group >> 16) & 0xff] + 16; - } - else - { - number = rt_lowest_bitmap[(rt_thread_ready_priority_group >> 24) & 0xff] + 24; - } -#if RT_THREAD_PRIORITY_MAX > 32 - highest_ready_priority = (number << 3) + - rt_lowest_bitmap[rt_thread_ready_table[number]]; + number = __rt_ffs(rt_thread_ready_priority_group) - 1; + highest_ready_priority = (number << 3) + __rt_ffs(rt_thread_ready_table[number]) - 1; #else - highest_ready_priority = number; -#endif + highest_ready_priority = __rt_ffs(rt_thread_ready_priority_group) - 1; #endif /* get switch to thread */ @@ -231,35 +191,15 @@ void rt_schedule(void) { register rt_ubase_t highest_ready_priority; -#if RT_THREAD_PRIORITY_MAX == 8 - highest_ready_priority = rt_lowest_bitmap[rt_thread_ready_priority_group]; +#if RT_THREAD_PRIORITY_MAX <= 32 + highest_ready_priority = __rt_ffs(rt_thread_ready_priority_group) - 1; #else register rt_ubase_t number; - /* find out the highest priority task */ - if (rt_thread_ready_priority_group & 0xff) - { - number = rt_lowest_bitmap[rt_thread_ready_priority_group & 0xff]; - } - else if (rt_thread_ready_priority_group & 0xff00) - { - number = rt_lowest_bitmap[(rt_thread_ready_priority_group >> 8) & 0xff] + 8; - } - else if (rt_thread_ready_priority_group & 0xff0000) - { - number = rt_lowest_bitmap[(rt_thread_ready_priority_group >> 16) & 0xff] + 16; - } - else - { - number = rt_lowest_bitmap[(rt_thread_ready_priority_group >> 24) & 0xff] + 24; - } -#if RT_THREAD_PRIORITY_MAX > 32 - highest_ready_priority = (number << 3) + - rt_lowest_bitmap[rt_thread_ready_table[number]]; -#else - highest_ready_priority = number; -#endif + number = __rt_ffs(rt_thread_ready_priority_group) - 1; + highest_ready_priority = (number << 3) + __rt_ffs(rt_thread_ready_table[number]) - 1; #endif + /* get switch to thread */ to_thread = rt_list_entry(rt_thread_priority_table[highest_ready_priority].next, struct rt_thread,