From 2664bdadd7acf0d024fc8d52433bcee6968b84d4 Mon Sep 17 00:00:00 2001
From: shaojinchun <shaojinchun@rt-thread.com>
Date: Fri, 26 Mar 2021 11:42:08 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96page=20init=E8=BF=87=E7=A8=8B?=
 =?UTF-8?q?=EF=BC=8C=E6=8F=90=E5=8D=87=E5=90=AF=E5=8A=A8=E9=80=9F=E5=BA=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 libcpu/arm/cortex-a/page.c      | 73 +++++++++++++++++++++++++++------
 libcpu/arm/cortex-a/start_gcc.S |  5 +++
 2 files changed, 66 insertions(+), 12 deletions(-)

diff --git a/libcpu/arm/cortex-a/page.c b/libcpu/arm/cortex-a/page.c
index 49a0c4dbd1..5154f8a92b 100644
--- a/libcpu/arm/cortex-a/page.c
+++ b/libcpu/arm/cortex-a/page.c
@@ -37,12 +37,50 @@ static size_t page_nr;
 
 static struct page *page_list[ARCH_PAGE_LIST_SIZE];
 
+RT_WEAK int rt_clz(size_t n)
+{
+    int bits = sizeof(size_t) * 8;
+
+    n |= (n >> 1);
+    n |= (n >> 2);
+    n |= (n >> 4);
+    n |= (n >> 8);
+    n |= (n >> 16);
+
+#ifdef ARCH_CPU_64BIT
+    n |= (n >> 32);
+
+    n = (n & 0x5555555555555555UL) + ((n >> 1) & 0x5555555555555555UL);
+    n = (n & 0x3333333333333333UL) + ((n >> 2) & 0x3333333333333333UL);
+    n = (n & 0x0707070707070707UL) + ((n >> 4) & 0x0707070707070707UL);
+    n = (n & 0x000f000f000f000fUL) + ((n >> 8) & 0x000f000f000f000fUL);
+    n = (n & 0x0000001f0000001fUL) + ((n >> 16) & 0x0000001f0000001fUL);
+    n = (n & 0x000000000000003fUL) + ((n >> 32) & 0x000000000000003fUL);
+#else
+    n = (n & 0x55555555UL) + ((n >> 1) & 0x55555555UL);
+    n = (n & 0x33333333UL) + ((n >> 2) & 0x33333333UL);
+    n = (n & 0x07070707UL) + ((n >> 4) & 0x07070707UL);
+    n = (n & 0x000f000fUL) + ((n >> 8) & 0x000f000fUL);
+    n = (n & 0x0000001fUL) + ((n >> 16) & 0x0000001fUL);
+#endif
+    return bits - n;
+}
+
+RT_WEAK int rt_ctz(size_t n)
+{
+    int ret = sizeof(size_t) * 8;
+
+    if (n)
+    {
+        ret -= (rt_clz(n ^ (n - 1)) + 1);
+    }
+    return ret;
+}
+
 size_t rt_page_bits(size_t size)
 {
-    int bit;
+    int bit = sizeof(size_t) * 8 - rt_clz(size) - 1;
 
-    bit = __builtin_clz(size);
-    bit = (31 - bit);
     if ((size ^ (1UL << bit)) != 0)
     {
         bit++;
@@ -145,6 +183,7 @@ static int _pages_free(struct page *p, uint32_t size_bits)
     struct page *buddy;
 
     RT_ASSERT(p->ref_cnt > 0);
+    RT_ASSERT(p->size_bits == ARCH_ADDRESS_WIDTH_BITS);
 
     p->ref_cnt--;
     if (p->ref_cnt != 0)
@@ -205,6 +244,7 @@ static struct page *_pages_alloc(uint32_t size_bits)
             level--;
         }
     }
+    p->size_bits = ARCH_ADDRESS_WIDTH_BITS;
     p->ref_cnt = 1;
     return p;
 }
@@ -317,6 +357,8 @@ void rt_page_init(rt_region_t reg)
         LOG_D("total = 0x%08x\n", total);
         LOG_D("mnr = 0x%08x\n", mnr);
 
+        RT_ASSERT(mnr < total);
+
         page_start = (struct page*)reg.start;
         reg.start += (mnr << ARCH_PAGE_SHIFT);
         page_addr = (void*)reg.start;
@@ -331,17 +373,24 @@ void rt_page_init(rt_region_t reg)
         page_list[i] = 0;
     }
 
-    /* init page struct */
-    for (i = 0; i < page_nr; i++)
+    /* add pages to free list */
+    while (reg.start != reg.end)
     {
-        page_start[i].size_bits = ARCH_ADDRESS_WIDTH_BITS;
-        page_start[i].ref_cnt = 1;
-    }
+        struct page *p;
+        int align_bits;
+        int size_bits;
 
-    /* add to free list */
-    for (i = 0; i < page_nr; i++)
-    {
-        _pages_free(page_start + i, 0);
+        size_bits = ARCH_ADDRESS_WIDTH_BITS - 1 - rt_clz(reg.end - reg.start);
+        align_bits = rt_ctz(reg.start);
+        if (align_bits < size_bits)
+        {
+            size_bits = align_bits;
+        }
+        p = addr_to_page((void*)reg.start);
+        p->size_bits = ARCH_ADDRESS_WIDTH_BITS;
+        p->ref_cnt = 1;
+        _pages_free(p, size_bits - ARCH_PAGE_SHIFT);
+        reg.start += (1UL << size_bits);
     }
 }
 #endif
diff --git a/libcpu/arm/cortex-a/start_gcc.S b/libcpu/arm/cortex-a/start_gcc.S
index 11e51cbd0a..9b3bb4a932 100644
--- a/libcpu/arm/cortex-a/start_gcc.S
+++ b/libcpu/arm/cortex-a/start_gcc.S
@@ -632,6 +632,11 @@ set_secondary_cpu_boot_address:
 #endif
     mov     pc, lr
 
+.global rt_clz
+rt_clz:
+    clz r0, r0
+    bx lr
+
 .global secondary_cpu_start
 secondary_cpu_start:
 #ifdef RT_USING_USERSPACE
-- 
Gitee