[PIC] Make a new flags for affinity post

Some affinity config for PIC is not support if
current CPU is not startup, but RT-Thread SMP
is startup after the devices init in `main` thread.

This patch add a thread(lower level of priority for `main`)
to reconfig the affinity of all PIRQs if they have
configed affinities. There is a new flags for PIC to
set a special property for affinity post.

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

Author: GuEe-GUI

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;
@@ -79,6 +79,7 @@ struct rt_pic_ops
     rt_err_t    (*irq_parse)(struct rt_pic *pic, struct rt_ofw_cell_args *args, struct rt_pic_irq *out_pirq);
 
 #define RT_PIC_F_IRQ_ROUTING    RT_BIT(0)   /* Routing ISR when cascade */
+#define RT_PIC_F_AFFINITY_POST  RT_BIT(1)   /* Affinity is valid after CPUs post */
     rt_ubase_t  flags;
 };
 
@@ -91,6 +92,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 +122,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;

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)
+    {
+        bitmap_set_bit(affinity, cpuid);
+    }
+}

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

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;
@@ -278,7 +280,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
+        {
+            bitmap_set_bit(pirq->affinity, _init_cpu_id);
+        }
 
         irq = rt_pic_config_irq(pic, irq_index, hwirq);
 
@@ -326,7 +336,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,
@@ -340,6 +350,7 @@ static struct rt_pic_ops gicv2_ops =
     .irq_send_ipi = gicv2_irq_send_ipi,
     .irq_map = gicv2_irq_map,
     .irq_parse = gicv2_irq_parse,
+    .flags = RT_PIC_F_AFFINITY_POST,
 };
 
 static rt_bool_t gicv2_handler(void *data)

components/drivers/pic/pic-gicv3.c

@@ -622,6 +622,9 @@ 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;
@@ -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,
@@ -715,6 +718,7 @@ static struct rt_pic_ops gicv3_ops =
     .irq_send_ipi = gicv3_irq_send_ipi,
     .irq_map = gicv3_irq_map,
     .irq_parse = gicv3_irq_parse,
+    .flags = RT_PIC_F_AFFINITY_POST,
 };
 
 static rt_bool_t gicv3_handler(void *data)

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)
+        {
+            bitmap_set_bit(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;
 
@@ -593,6 +597,48 @@ rt_weak rt_err_t rt_pic_user_extends(struct rt_pic *pic)
     return -RT_ENOSYS;
 }
 
+static void pic_fixup_runner(void *param)
+{
+    /* Fixup system PIC setting */
+    struct rt_pic_irq *pirq, *pirq_end;
+
+    pirq = &_pirq_hash[RT_ARRAY_SIZE(_ipi_hash)];
+    pirq_end = &_pirq_hash[RT_ARRAY_SIZE(_pirq_hash)];
+
+    for (; pirq < pirq_end; ++pirq)
+    {
+        if (!pirq->pic)
+        {
+            continue;
+        }
+
+        /* Only fixup who no bind to CPU#0 */
+        if (!bitmap_test_bit(pirq->affinity, 0) &&
+            pirq->pic->ops->flags & RT_PIC_F_AFFINITY_POST)
+        {
+            rt_pic_irq_set_affinity(pirq->irq, pirq->affinity);
+        }
+    }
+}
+
+static int pic_fixup_prepare(void)
+{
+    rt_thread_t tid;
+
+    /*
+     * We assume that the SMP startup has been confirmed (system is working)
+     * to be complete after the "main" thread finishes.
+     */
+    tid = rt_thread_create("pic", pic_fixup_runner, RT_NULL,
+                           RT_MAIN_THREAD_STACK_SIZE, RT_MAIN_THREAD_PRIORITY + 1, 20);
+    RT_ASSERT(tid != RT_NULL);
+
+    rt_thread_startup(tid);
+
+    return 0;
+}
+INIT_PREV_EXPORT(pic_fixup_prepare);
+
 rt_err_t rt_pic_irq_init(void)
 {
     rt_err_t err = RT_EOK;