[DM/PCI] Add memory window pool for EP

Signed-off-by: GuEe-GUI <[email protected]>

3 files changed, 230 insertions, 1 deletions

diff --git a/components/drivers/include/drivers/pci_endpoint.h b/components/drivers/include/drivers/pci_endpoint.h
index 3e6fd7c918..c43c182d95 100644
--- a/components/drivers/include/drivers/pci_endpoint.h
+++ b/components/drivers/include/drivers/pci_endpoint.h
@@ -73,6 +73,7 @@ struct rt_pci_ep_msix_tbl
 };
 
 struct rt_pci_ep_ops;
+struct rt_pci_ep_mem;
 
 struct rt_pci_ep
 {
@@ -84,6 +85,9 @@ struct rt_pci_ep
     const struct rt_device *rc_dev;
     const struct rt_pci_ep_ops *ops;
 
+    rt_size_t mems_nr;
+    struct rt_pci_ep_mem *mems;
+
     rt_uint8_t max_functions;
     RT_BITMAP_DECLARE(functions_map, 8);
     rt_list_t epf_nodes;
@@ -92,6 +96,16 @@ struct rt_pci_ep
     void *priv;
 };
 
+struct rt_pci_ep_mem
+{
+    rt_ubase_t cpu_addr;
+    rt_size_t size;
+    rt_size_t page_size;
+
+    rt_bitmap_t *map;
+    rt_size_t bits;
+};
+
 struct rt_pci_epf
 {
     rt_list_t list;
@@ -170,6 +184,16 @@ rt_err_t rt_pci_ep_stop(struct rt_pci_ep *ep);
 rt_err_t rt_pci_ep_register(struct rt_pci_ep *ep);
 rt_err_t rt_pci_ep_unregister(struct rt_pci_ep *ep);
 
+rt_err_t rt_pci_ep_mem_array_init(struct rt_pci_ep *ep,
+        struct rt_pci_ep_mem *mems, rt_size_t mems_nr);
+rt_err_t rt_pci_ep_mem_init(struct rt_pci_ep *ep,
+        rt_ubase_t cpu_addr, rt_size_t size, rt_size_t page_size);
+
+void *rt_pci_ep_mem_alloc(struct rt_pci_ep *ep,
+        rt_ubase_t *out_cpu_addr, rt_size_t size);
+void rt_pci_ep_mem_free(struct rt_pci_ep *ep,
+        void *vaddr, rt_ubase_t cpu_addr, rt_size_t size);
+
 rt_err_t rt_pci_ep_add_epf(struct rt_pci_ep *ep, struct rt_pci_epf *epf);
 rt_err_t rt_pci_ep_remove_epf(struct rt_pci_ep *ep, struct rt_pci_epf *epf);
 
diff --git a/components/drivers/pci/endpoint/SConscript b/components/drivers/pci/endpoint/SConscript
index ca3121619b..9b76a96123 100644
--- a/components/drivers/pci/endpoint/SConscript
+++ b/components/drivers/pci/endpoint/SConscript
@@ -8,7 +8,7 @@ if not GetDepend(['RT_PCI_ENDPOINT']):
 cwd = GetCurrentDir()
 CPPPATH = [cwd + '/../../include']
 
-src = ['endpoint.c']
+src = ['endpoint.c', 'mem.c']
 
 group = DefineGroup('DeviceDrivers', src, depend = [''], CPPPATH = CPPPATH)
 
diff --git a/components/drivers/pci/endpoint/mem.c b/components/drivers/pci/endpoint/mem.c
new file mode 100644
index 0000000000..297db161c7
--- /dev/null
+++ b/components/drivers/pci/endpoint/mem.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (c) 2006-2022, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2022-08-25     GuEe-GUI     first version
+ */
+
+#include <drivers/pci_endpoint.h>
+
+#define DBG_TAG "pci.ep.mem"
+#define DBG_LVL DBG_INFO
+#include <rtdbg.h>
+
+rt_err_t rt_pci_ep_mem_array_init(struct rt_pci_ep *ep,
+        struct rt_pci_ep_mem *mems, rt_size_t mems_nr)
+{
+    rt_size_t idx;
+    rt_err_t err = RT_EOK;
+
+    if (!ep || !mems)
+    {
+        return -RT_EINVAL;
+    }
+
+    rt_mutex_take(&ep->lock, RT_WAITING_FOREVER);
+
+    ep->mems_nr = mems_nr;
+    ep->mems = rt_calloc(mems_nr, sizeof(*ep->mems));
+
+    if (!ep->mems)
+    {
+        return -RT_ENOMEM;
+    }
+
+    for (idx = 0; idx < mems_nr; ++idx)
+    {
+        struct rt_pci_ep_mem *mem = &ep->mems[idx];
+
+        mem->cpu_addr = mems->cpu_addr;
+        mem->size = mems->size;
+        mem->page_size = mems->page_size;
+        mem->bits = mems->size / mems->page_size;
+        mem->map = rt_calloc(RT_BITMAP_LEN(mem->bits), sizeof(*mem->map));
+
+        if (!mem->map)
+        {
+            err = -RT_ENOMEM;
+            goto _out_lock;
+        }
+    }
+
+_out_lock:
+    if (err)
+    {
+        while (idx --> 0)
+        {
+            rt_free(ep->mems[idx].map);
+        }
+        rt_free(ep->mems);
+
+        ep->mems_nr = 0;
+        ep->mems = RT_NULL;
+    }
+
+    rt_mutex_release(&ep->lock);
+
+    return err;
+}
+
+rt_err_t rt_pci_ep_mem_init(struct rt_pci_ep *ep,
+        rt_ubase_t cpu_addr, rt_size_t size, rt_size_t page_size)
+{
+    struct rt_pci_ep_mem mem;
+
+    if (!ep)
+    {
+        return -RT_EINVAL;
+    }
+
+    mem.cpu_addr = cpu_addr;
+    mem.size = size;
+    mem.page_size = page_size;
+
+    return rt_pci_ep_mem_array_init(ep, &mem, 1);
+}
+
+static rt_ubase_t bitmap_region_alloc(struct rt_pci_ep_mem *mem, rt_size_t size)
+{
+    rt_size_t bit, next_bit, end_bit, max_bits;
+
+    size /= mem->page_size;
+    max_bits = mem->bits - size;
+
+    rt_bitmap_for_each_clear_bit(mem->map, bit, max_bits)
+    {
+        end_bit = bit + size;
+
+        for (next_bit = bit + 1; next_bit < end_bit; ++next_bit)
+        {
+            if (rt_bitmap_test_bit(mem->map, next_bit))
+            {
+                bit = next_bit;
+                goto _next;
+            }
+        }
+
+        if (next_bit == end_bit)
+        {
+            while (next_bit --> bit)
+            {
+                rt_bitmap_set_bit(mem->map, next_bit);
+            }
+
+            return mem->cpu_addr + bit * mem->page_size;
+        }
+    _next:
+    }
+
+    return ~0ULL;
+}
+
+static void bitmap_region_free(struct rt_pci_ep_mem *mem,
+        rt_ubase_t cpu_addr, rt_size_t size)
+{
+    rt_size_t bit = (cpu_addr - mem->cpu_addr) / mem->page_size, end_bit;
+
+    size /= mem->page_size;
+    end_bit = bit + size;
+
+    for (; bit < end_bit; ++bit)
+    {
+        rt_bitmap_clear_bit(mem->map, bit);
+    }
+}
+
+void *rt_pci_ep_mem_alloc(struct rt_pci_ep *ep,
+        rt_ubase_t *out_cpu_addr, rt_size_t size)
+{
+    void *vaddr = RT_NULL;
+
+    if (!ep || !out_cpu_addr)
+    {
+        return vaddr;
+    }
+
+    rt_mutex_take(&ep->lock, RT_WAITING_FOREVER);
+
+    for (rt_size_t idx = 0; idx < ep->mems_nr; ++idx)
+    {
+        rt_ubase_t cpu_addr;
+        struct rt_pci_ep_mem *mem = &ep->mems[idx];
+
+        cpu_addr = bitmap_region_alloc(mem, size);
+
+        if (cpu_addr != ~0ULL)
+        {
+            vaddr = rt_ioremap((void *)cpu_addr, size);
+
+            if (!vaddr)
+            {
+                bitmap_region_free(mem, cpu_addr, size);
+
+                /* Try next memory */
+                continue;
+            }
+
+            *out_cpu_addr = cpu_addr;
+            break;
+        }
+    }
+
+    rt_mutex_release(&ep->lock);
+
+    return vaddr;
+}
+
+void rt_pci_ep_mem_free(struct rt_pci_ep *ep,
+        void *vaddr, rt_ubase_t cpu_addr, rt_size_t size)
+{
+    if (!ep || !vaddr || !size)
+    {
+        return;
+    }
+
+    rt_mutex_take(&ep->lock, RT_WAITING_FOREVER);
+
+    for (rt_size_t idx = 0; idx < ep->mems_nr; ++idx)
+    {
+        struct rt_pci_ep_mem *mem = &ep->mems[idx];
+
+        if (mem->cpu_addr > cpu_addr &&
+            mem->cpu_addr + mem->size >= cpu_addr + size)
+        {
+            rt_iounmap(mem);
+            bitmap_region_free(mem, cpu_addr, size);
+
+            break;
+        }
+    }
+
+    rt_mutex_release(&ep->lock);
+}