implement __rt_ffs in kernel service library

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

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 <sys/types.h>
 void *memcpy(void *dest, const void *src, size_t n) __attribute__((weak, alias("rt_memcpy")));

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,