[PIC] Make affinity helper interface (#8995)

* [PIC] Make affinity helper interface

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

* [PIC/GIC] Fixup GIC affinity init

GICv2 is not support if current CPU is not startup,
but RT-Thread SMP is startup after the devices init
in the `main` thread.

This patch fixup the cpumask to init once and check
the init status in affinity interface.

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

---------

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

6 files changed, 49 insertions, 13 deletions

diff --git a/components/drivers/include/drivers/pic.h b/components/drivers/include/drivers/pic.h
index 5ab1434047..c2ca0e0079 100755
--- a/components/drivers/include/drivers/pic.h
+++ b/components/drivers/include/drivers/pic.h
@@ -40,7 +40,7 @@ struct rt_pic
 
     rt_list_t list;
 
-    struct rt_pic_ops *ops;
+    const struct rt_pic_ops *ops;
 
     void *priv_data;
     void *user_data;
@@ -91,6 +91,10 @@ struct rt_pic_isr
     struct rt_irq_desc action;
 };
 
+#define RT_IRQ_AFFINITY_DECLARE(name)           RT_BITMAP_DECLARE(name, RT_CPUS_NR)
+#define RT_IRQ_AFFINITY_SET(affinity, cpuid)    rt_bitmap_set_bit(affinity, cpuid)
+#define RT_IRQ_AFFINITY_CLEAR(affinity, cpuid)  rt_bitmap_clear_bit(affinity, cpuid)
+
 #ifdef RT_USING_PIC_STATISTICS
 struct rt_pic_irq_statistics
 {
@@ -117,7 +121,7 @@ struct rt_pic_irq
     rt_uint32_t mode;
 
     rt_uint32_t priority;
-    RT_BITMAP_DECLARE(affinity, RT_CPUS_NR);
+    RT_IRQ_AFFINITY_DECLARE(affinity);
 
     rt_list_t list;
     rt_list_t children_nodes;
diff --git a/components/drivers/pic/pic-gic-common.c b/components/drivers/pic/pic-gic-common.c
index dda43979c4..3051b04be6 100644
--- a/components/drivers/pic/pic-gic-common.c
+++ b/components/drivers/pic/pic-gic-common.c
@@ -173,3 +173,11 @@ void gic_common_cpu_config(void *base, int nr, void (*sync_access)(void *), void
         sync_access(data);
     }
 }
+
+void gic_fill_ppi_affinity(bitmap_t *affinity)
+{
+    for (int cpuid = 0; cpuid < RT_CPUS_NR; ++cpuid)
+    {
+        RT_IRQ_AFFINITY_SET(affinity, cpuid);
+    }
+}
diff --git a/components/drivers/pic/pic-gic-common.h b/components/drivers/pic/pic-gic-common.h
index 2734d434a2..cd8dbd5875 100644
--- a/components/drivers/pic/pic-gic-common.h
+++ b/components/drivers/pic/pic-gic-common.h
@@ -16,7 +16,7 @@
 
 #define GIC_SGI_NR  16
 
-#define GICD_INT_DEF_PRI        0xa0
+#define GICD_INT_DEF_PRI        0xa0U
 #define GICD_INT_DEF_PRI_X4     \
 (                               \
     (GICD_INT_DEF_PRI << 24) |  \
@@ -43,6 +43,8 @@ rt_err_t gic_common_configure_irq(void *base, int irq, rt_uint32_t mode, void (*
 void gic_common_dist_config(void *base, int max_irqs, void (*sync_access)(void *), void *data);
 void gic_common_cpu_config(void *base, int nr, void (*sync_access)(void *), void *data);
 
+void gic_fill_ppi_affinity(bitmap_t *affinity);
+
 #ifdef RT_PIC_ARM_GIC_V2M
 rt_err_t gicv2m_ofw_probe(struct rt_ofw_node *ic_np, const struct rt_ofw_node_id *id);
 #endif
diff --git a/components/drivers/pic/pic-gicv2.c b/components/drivers/pic/pic-gicv2.c
index ac9cb5a719..96d62cbe2b 100644
--- a/components/drivers/pic/pic-gicv2.c
+++ b/components/drivers/pic/pic-gicv2.c
@@ -66,6 +66,8 @@ static void gicv2_dist_init(struct gicv2 *gic)
     rt_uint32_t i;
     rt_uint32_t cpumask = gicv2_cpumask_map(gic);
 
+    _init_cpu_id = rt_hw_cpu_id();
+
     gic->max_irq = HWREG32(base + GIC_DIST_TYPE) & 0x1f;
     gic->max_irq = (gic->max_irq + 1) * 32;
 
@@ -102,7 +104,7 @@ static void gicv2_cpu_init(struct gicv2 *gic)
     rt_uint32_t cpumask;
     void *base = gic->cpu_base;
     rt_uint32_t config = GICC_ENABLE;
-    int cpu_id = _init_cpu_id = rt_hw_cpu_id();
+    int cpu_id = rt_hw_cpu_id();
 
     cpumask = gicv2_cpumask_map(gic);
     _gicv2_cpumask_map[cpu_id] = cpumask;
@@ -204,6 +206,11 @@ static rt_err_t gicv2_irq_set_affinity(struct rt_pic_irq *pirq, rt_bitmap_t *aff
     rt_uint8_t valb = _gicv2_cpumask_map[__rt_ffs(target_list) - 1];
     void *io_addr = gic->dist_base + GIC_DIST_TARGET + hwirq;
 
+    if (valb == 0xfe)
+    {
+        return -RT_EIO;
+    }
+
     if (needs_rmw_access)
     {
         /* RMW write byte */
@@ -278,7 +285,15 @@ static int gicv2_irq_map(struct rt_pic *pic, int hwirq, rt_uint32_t mode)
     {
         pirq->mode = mode;
         pirq->priority = GICD_INT_DEF_PRI;
-        rt_bitmap_set_bit(pirq->affinity, _init_cpu_id);
+
+        if (hwirq < 32)
+        {
+            gic_fill_ppi_affinity(pirq->affinity);
+        }
+        else
+        {
+            RT_IRQ_AFFINITY_SET(pirq->affinity, _init_cpu_id);
+        }
 
         irq = rt_pic_config_irq(pic, irq_index, hwirq);
 
@@ -326,7 +341,7 @@ static rt_err_t gicv2_irq_parse(struct rt_pic *pic, struct rt_ofw_cell_args *arg
     return err;
 }
 
-static struct rt_pic_ops gicv2_ops =
+const static struct rt_pic_ops gicv2_ops =
 {
     .name = "GICv2",
     .irq_init = gicv2_irq_init,
diff --git a/components/drivers/pic/pic-gicv3.c b/components/drivers/pic/pic-gicv3.c
index d9e17695db..ea7d5cfc7a 100644
--- a/components/drivers/pic/pic-gicv3.c
+++ b/components/drivers/pic/pic-gicv3.c
@@ -622,10 +622,13 @@ static int gicv3_irq_map(struct rt_pic *pic, int hwirq, rt_uint32_t mode)
 
         switch (gicv3_hwirq_type(hwirq))
         {
+        case PPI_TYPE:
+            gic_fill_ppi_affinity(pirq->affinity);
+            break;
         case SPI_TYPE:
         case ESPI_TYPE:
             pirq->priority = GICD_INT_DEF_PRI;
-            rt_bitmap_set_bit(pirq->affinity, _init_cpu_id);
+            RT_IRQ_AFFINITY_SET(pirq->affinity, _init_cpu_id);
         default:
             break;
         }
@@ -701,7 +704,7 @@ static rt_err_t gicv3_irq_parse(struct rt_pic *pic, struct rt_ofw_cell_args *arg
     return err;
 }
 
-static struct rt_pic_ops gicv3_ops =
+const static struct rt_pic_ops gicv3_ops =
 {
     .name = "GICv3",
     .irq_init = gicv3_irq_init,
diff --git a/components/drivers/pic/pic.c b/components/drivers/pic/pic.c
index cdf8a8ff62..a6abd1c526 100644
--- a/components/drivers/pic/pic.c
+++ b/components/drivers/pic/pic.c
@@ -192,10 +192,17 @@ static void config_pirq(struct rt_pic *pic, struct rt_pic_irq *pirq, int irq, in
 int rt_pic_config_ipi(struct rt_pic *pic, int ipi_index, int hwirq)
 {
     int ipi = ipi_index;
+    struct rt_pic_irq *pirq;
 
     if (pic && ipi < RT_ARRAY_SIZE(_ipi_hash) && hwirq >= 0 && pic->ops->irq_send_ipi)
     {
-        config_pirq(pic, &_pirq_hash[ipi], ipi, hwirq);
+        pirq = &_pirq_hash[ipi];
+        config_pirq(pic, pirq, ipi, hwirq);
+
+        for (int cpuid = 0; cpuid < RT_CPUS_NR; ++cpuid)
+        {
+            RT_IRQ_AFFINITY_SET(pirq->affinity, cpuid);
+        }
 
         LOG_D("%s config %s %d to hwirq %d", pic->ops->name, "ipi", ipi, hwirq);
     }
@@ -404,7 +411,7 @@ rt_err_t rt_pic_detach_irq(int irq, void *uid)
             }
             else
             {
-                struct rt_pic_isr *next_isr = rt_list_entry(isr->list.next, struct rt_pic_isr, list);
+                struct rt_pic_isr *next_isr = rt_list_first_entry(&isr->list, struct rt_pic_isr, list);
 
                 rt_list_remove(&next_isr->list);
 
@@ -518,9 +525,6 @@ rt_err_t rt_pic_handle_isr(struct rt_pic_irq *pirq)
     pirq->stat.current_irq_begin[rt_hw_cpu_id()] = ts.tv_sec * (1000UL * 1000 * 1000) + ts.tv_nsec;
 #endif
 
-    /* Corrected irq affinity */
-    rt_bitmap_set_bit(pirq->affinity, rt_hw_cpu_id());
-
     handler_nodes = &pirq->isr.list;
     action = &pirq->isr.action;