sched: add smp function call

Support smp function call, calling smp_call_function allows
a specific core to execute a function. It should be noted
that there should be no waiting operations in the executed
function.

Signed-off-by: zhangyuan21 <zhangyuan21@xiaomi.com>

include/nuttx/arch.h

@@ -1581,6 +1581,18 @@ void up_secure_irq(int irq, bool secure);
 # define up_secure_irq(i, s)
 #endif
 
+#ifdef CONFIG_SMP_CALL
+/****************************************************************************
+ * Name: up_send_smp_call
+ *
+ * Description:
+ *   Send smp call to target cpu
+ *
+ ****************************************************************************/
+
+void up_send_smp_call(cpu_set_t cpuset);
+#endif
+
 /****************************************************************************
  * Name: up_secure_irq_all
  *

include/nuttx/sched.h

@@ -28,6 +28,7 @@
 #include <nuttx/config.h>
 
 #include <sys/types.h>
+#include <stdbool.h>
 #include <stdint.h>
 #include <sched.h>
 #include <signal.h>
@@ -743,6 +744,12 @@ begin_packed_struct struct tcbinfo_s
 
 typedef CODE void (*nxsched_foreach_t)(FAR struct tcb_s *tcb, FAR void *arg);
 
+/* This is the callback type used by nxsched_smp_call() */
+
+#ifdef CONFIG_SMP_CALL
+typedef CODE int (*nxsched_smp_call_t)(FAR void *arg);
+#endif
+
 #endif /* __ASSEMBLY__ */
 
 /****************************************************************************
@@ -1566,6 +1573,67 @@ pid_t nxsched_getppid(void);
 
 size_t nxsched_collect_deadlock(FAR pid_t *pid, size_t count);
 
+#ifdef CONFIG_SMP_CALL
+/****************************************************************************
+ * Name: nxsched_smp_call_handler
+ *
+ * Description:
+ *   SMP function call handler
+ *
+ * Input Parameters:
+ *   irq     - Interrupt id
+ *   context - Regs context before irq
+ *   arg     - Interrupt arg
+ *
+ * Returned Value:
+ *   Result
+ *
+ ****************************************************************************/
+
+int nxsched_smp_call_handler(int irq, FAR void *context,
+                             FAR void *arg);
+
+/****************************************************************************
+ * Name: nxsched_smp_call_single
+ *
+ * Description:
+ *   Call function on single processor
+ *
+ * Input Parameters:
+ *   cpuid - Target cpu id
+ *   func  - Function
+ *   arg   - Function args
+ *   wait  - Wait function callback or not
+ *
+ * Returned Value:
+ *   Result
+ *
+ ****************************************************************************/
+
+int nxsched_smp_call_single(int cpuid, nxsched_smp_call_t func,
+                            FAR void *arg, bool wait);
+
+/****************************************************************************
+ * Name: nxsched_smp_call
+ *
+ * Description:
+ *   Call function on multi processors
+ *
+ * Input Parameters:
+ *   cpuset - Target cpuset
+ *   func   - Function
+ *   arg    - Function args
+ *   wait   - Wait function callback or not
+ *
+ * Returned Value:
+ *   Result
+ *
+ ****************************************************************************/
+
+int nxsched_smp_call(cpu_set_t cpuset, nxsched_smp_call_t func,
+                     FAR void *arg, bool wait);
+#endif
+
 #undef EXTERN
 #if defined(__cplusplus)
 }

sched/Kconfig

@@ -396,6 +396,12 @@ config SMP_DEFAULT_CPUSET
 		Set the Default CPU bits. The way to use the unset CPU is to call the
 		sched_setaffinity function to bind a task to the CPU. bit0 means CPU0.
 
+config SMP_CALL
+	bool "Support SMP function call"
+	default n
+	---help---
+		Enable to support SMP function call.
+
 endif # SMP
 
 choice

sched/sched/CMakeLists.txt

@@ -119,4 +119,8 @@ if(CONFIG_SCHED_BACKTRACE)
   list(APPEND SRCS sched_backtrace.c)
 endif()
 
+if(CONFIG_SMP_CALL)
+  list(APPEND SRCS sched_smp.c)
+endif()
+
 target_sources(sched PRIVATE ${SRCS})

sched/sched/Make.defs

@@ -96,6 +96,10 @@ ifeq ($(CONFIG_SCHED_BACKTRACE),y)
 CSRCS += sched_backtrace.c
 endif
 
+ifeq ($(CONFIG_SMP_CALL),y)
+CSRCS += sched_smp.c
+endif
+
 # Include sched build support
 
 DEPPATH += --dep-path sched

sched/sched/sched_smp.c

@@ -0,0 +1,294 @@
+/****************************************************************************
+ * sched/sched/sched_smp.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/nuttx.h>
+#include <nuttx/queue.h>
+#include <nuttx/semaphore.h>
+#include <nuttx/sched.h>
+#include <nuttx/spinlock.h>
+
+#include "sched/sched.h"
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+struct smp_call_cookie_s
+{
+  sem_t       sem;
+  int         error;
+};
+
+struct smp_call_data_s
+{
+  sq_entry_t                    node[CONFIG_SMP_NCPUS];
+  nxsched_smp_call_t            func;
+  FAR void                     *arg;
+  FAR struct smp_call_cookie_s *cookie;
+  spinlock_t                    lock;
+  volatile int                  refcount;
+};
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static sq_queue_t g_smp_call_queue[CONFIG_SMP_NCPUS];
+static struct smp_call_data_s g_smp_call_data;
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsched_smp_call_add
+ *
+ * Description:
+ *   Add call data to other processors
+ *
+ * Input Parameters:
+ *   cpu        - Target cpu id
+ *   call_data  - Call data
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+static void nxsched_smp_call_add(int cpu,
+                                 FAR struct smp_call_data_s *call_data)
+{
+  irqstate_t flags;
+
+  flags = enter_critical_section();
+  sq_addlast(&call_data->node[cpu], &g_smp_call_queue[cpu]);
+  leave_critical_section(flags);
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: nxsched_smp_call_handler
+ *
+ * Description:
+ *   SMP function call handler
+ *
+ * Input Parameters:
+ *   irq     - Interrupt id
+ *   context - Regs context before irq
+ *   arg     - Interrupt arg
+ *
+ * Returned Value:
+ *   Result
+ *
+ ****************************************************************************/
+
+int nxsched_smp_call_handler(int irq, FAR void *context,
+                             FAR void *arg)
+{
+  FAR sq_queue_t *call_queue;
+  FAR sq_entry_t *curr;
+  FAR sq_entry_t *next;
+  int cpu = this_cpu();
+
+  irqstate_t flags = enter_critical_section();
+
+  call_queue = &g_smp_call_queue[cpu];
+
+  sq_for_every_safe(call_queue, curr, next)
+    {
+      FAR struct smp_call_data_s *call_data =
+        container_of(curr, struct smp_call_data_s, node[cpu]);
+      int ret;
+
+      sq_rem(&call_data->node[cpu], call_queue);
+
+      leave_critical_section(flags);
+
+      ret = call_data->func(call_data->arg);
+
+      if (call_data->cookie != NULL)
+        {
+          if (ret < 0)
+            {
+              call_data->cookie->error = ret;
+            }
+
+          nxsem_post(&call_data->cookie->sem);
+        }
+
+      if (spin_is_locked(&call_data->lock))
+        {
+          if (--call_data->refcount == 0)
+            {
+              spin_unlock(&call_data->lock);
+            }
+        }
+
+      flags = enter_critical_section();
+    }
+
+  leave_critical_section(flags);
+  return OK;
+}
+
+/****************************************************************************
+ * Name: nxsched_smp_call_single
+ *
+ * Description:
+ *   Call function on single processor
+ *
+ * Input Parameters:
+ *   cpuid - Target cpu id
+ *   func  - Function
+ *   arg   - Function args
+ *   wait  - Wait function callback or not
+ *
+ * Returned Value:
+ *   Result
+ *
+ ****************************************************************************/
+
+int nxsched_smp_call_single(int cpuid, nxsched_smp_call_t func,
+                            FAR void *arg, bool wait)
+{
+  cpu_set_t cpuset;
+
+  CPU_ZERO(&cpuset);
+  CPU_SET(cpuid, &cpuset);
+  return nxsched_smp_call(cpuset, func, arg, wait);
+}
+
+/****************************************************************************
+ * Name: nxsched_smp_call
+ *
+ * Description:
+ *   Call function on multi processors
+ *
+ * Input Parameters:
+ *   cpuset - Target cpuset
+ *   func   - Function
+ *   arg    - Function args
+ *   wait   - Wait function callback or not
+ *
+ * Returned Value:
+ *   Result
+ *
+ ****************************************************************************/
+
+int nxsched_smp_call(cpu_set_t cpuset, nxsched_smp_call_t func,
+                     FAR void *arg, bool wait)
+{
+  struct smp_call_data_s call_data_stack =
+    {
+      0
+    };
+
+  struct smp_call_cookie_s cookie =
+    {
+      0
+    };
+
+  FAR struct smp_call_data_s *call_data;
+  int remote_cpus = 0;
+  int ret = OK;
+  int i;
+
+  /* Prevent reschedule on another processor */
+
+  sched_lock();
+
+  if (CPU_ISSET(this_cpu(), &cpuset))
+    {
+      ret = func(arg);
+      if (ret < 0)
+        {
+          goto out;
+        }
+
+      CPU_CLR(this_cpu(), &cpuset);
+    }
+
+  /* If waiting is necessary, initialize and wait for the cookie. */
+
+  if (wait)
+    {
+      nxsem_init(&cookie.sem, 0, 0);
+
+      call_data = &call_data_stack;
+      call_data->cookie = &cookie;
+    }
+  else
+    {
+      call_data = &g_smp_call_data;
+      spin_lock(&call_data->lock);
+    }
+
+  call_data->func = func;
+  call_data->arg  = arg;
+
+  for (i = 0; i < CONFIG_SMP_NCPUS; i++)
+    {
+      if (CPU_ISSET(i, &cpuset))
+        {
+          nxsched_smp_call_add(i, call_data);
+          remote_cpus++;
+        }
+    }
+
+  call_data->refcount = remote_cpus;
+
+  if (remote_cpus > 0)
+    {
+      up_send_smp_call(cpuset);
+    }
+
+  if (wait)
+    {
+      for (i = 0; i < remote_cpus; i++)
+        {
+          int wait_ret = nxsem_wait_uninterruptible(&cookie.sem);
+          if (wait_ret < 0)
+            {
+              ret = wait_ret;
+            }
+        }
+
+      if (cookie.error < 0)
+        {
+          ret = cookie.error;
+        }
+
+      nxsem_destroy(&cookie.sem);
+    }
+
+out:
+  sched_unlock();
+  return ret;
+}