Create one interrupt server per processor

This allows load balancing of interrupt processing in SMP
configurations.

Update #3071.

1 files changed, 184 insertions, 79 deletions

diff --git a/c/src/lib/libbsp/shared/src/irq-server.c b/c/src/lib/libbsp/shared/src/irq-server.c
index 1f9b75ffd9..a37c9caafc 100644
--- a/c/src/lib/libbsp/shared/src/irq-server.c
+++ b/c/src/lib/libbsp/shared/src/irq-server.c
@@ -30,47 +30,70 @@
 
 #define BSP_INTERRUPT_SERVER_MANAGEMENT_VECTOR (BSP_INTERRUPT_VECTOR_MAX + 1)
 
-RTEMS_INTERRUPT_LOCK_DEFINE(
-  static,
-  bsp_interrupt_server_lock,
-  "Interrupt Server"
+typedef struct {
+  RTEMS_INTERRUPT_LOCK_MEMBER(lock);
+  rtems_chain_control entries;
+  rtems_id server;
+  unsigned errors;
+} bsp_interrupt_server_context;
+
+#if defined(RTEMS_SMP)
+static bsp_interrupt_server_context *bsp_interrupt_server_instances;
+#else
+static bsp_interrupt_server_context bsp_interrupt_server_instance;
+#endif
+
+static bsp_interrupt_server_context *bsp_interrupt_server_get_context(
+  uint32_t server_index,
+  rtems_status_code *sc
 )
-
-static rtems_id bsp_interrupt_server_id = RTEMS_ID_NONE;
-
-static RTEMS_CHAIN_DEFINE_EMPTY(bsp_interrupt_server_chain);
-
-static rtems_status_code bsp_interrupt_server_is_initialized(void)
 {
-  if (bsp_interrupt_server_id != RTEMS_ID_NONE) {
-    return RTEMS_SUCCESSFUL;
-  } else {
-    return RTEMS_INCORRECT_STATE;
+#if defined(RTEMS_SMP)
+  if (bsp_interrupt_server_instances == NULL) {
+    *sc = RTEMS_INCORRECT_STATE;
+    return NULL;
   }
-}
+#else
+  if (bsp_interrupt_server_instance.server == RTEMS_ID_NONE) {
+    *sc = RTEMS_INCORRECT_STATE;
+    return NULL;
+  }
+#endif
 
-static unsigned bsp_interrupt_server_errors;
+  if (server_index >= rtems_get_processor_count()) {
+    *sc = RTEMS_INVALID_ID;
+    return NULL;
+  }
+
+  *sc = RTEMS_SUCCESSFUL;
+#if defined(RTEMS_SMP)
+  return &bsp_interrupt_server_instances[server_index];
+#else
+  return &bsp_interrupt_server_instance;
+#endif
+}
 
 static void bsp_interrupt_server_trigger(void *arg)
 {
   rtems_interrupt_lock_context lock_context;
   rtems_interrupt_server_entry *e = arg;
+  bsp_interrupt_server_context *s = e->server;
 
   if (bsp_interrupt_is_valid_vector(e->vector)) {
     bsp_interrupt_vector_disable(e->vector);
   }
 
-  rtems_interrupt_lock_acquire(&bsp_interrupt_server_lock, &lock_context);
+  rtems_interrupt_lock_acquire(&s->lock, &lock_context);
 
   if (rtems_chain_is_node_off_chain(&e->node)) {
-    rtems_chain_append_unprotected(&bsp_interrupt_server_chain, &e->node);
+    rtems_chain_append_unprotected(&s->entries, &e->node);
   } else {
-    ++bsp_interrupt_server_errors;
+    ++s->errors;
   }
 
-  rtems_interrupt_lock_release(&bsp_interrupt_server_lock, &lock_context);
+  rtems_interrupt_lock_release(&s->lock, &lock_context);
 
-  rtems_event_system_send(bsp_interrupt_server_id, RTEMS_EVENT_SYSTEM_SERVER);
+  rtems_event_system_send(s->server, RTEMS_EVENT_SYSTEM_SERVER);
 }
 
 typedef struct {
@@ -114,6 +137,7 @@ static rtems_interrupt_server_entry *bsp_interrupt_server_query_entry(
 }
 
 typedef struct {
+  bsp_interrupt_server_context *server;
   rtems_vector_number vector;
   rtems_option options;
   rtems_interrupt_handler handler;
@@ -146,6 +170,7 @@ static void bsp_interrupt_server_install_helper(void *arg)
   if (e == NULL) {
     e = calloc(1, sizeof(*e));
     if (e != NULL) {
+      e->server = hd->server;
       e->vector = hd->vector;
       e->actions = a;
 
@@ -162,6 +187,10 @@ static void bsp_interrupt_server_install_helper(void *arg)
     } else {
       sc = RTEMS_NO_MEMORY;
     }
+#if defined(RTEMS_SMP)
+  } else if (e->server != hd->server) {
+    sc = RTEMS_RESOURCE_IN_USE;
+#endif
   } else if (
     RTEMS_INTERRUPT_IS_UNIQUE(hd->options)
       || RTEMS_INTERRUPT_IS_UNIQUE(trigger_options)
@@ -252,6 +281,7 @@ static void bsp_interrupt_server_remove_helper(void *arg)
 }
 
 static rtems_status_code bsp_interrupt_server_call_helper(
+  bsp_interrupt_server_context *s,
   rtems_vector_number vector,
   rtems_option options,
   rtems_interrupt_handler handler,
@@ -260,6 +290,7 @@ static rtems_status_code bsp_interrupt_server_call_helper(
 )
 {
   bsp_interrupt_server_helper_data hd = {
+    .server = s,
     .vector = vector,
     .options = options,
     .handler = handler,
@@ -271,6 +302,7 @@ static rtems_status_code bsp_interrupt_server_call_helper(
     .arg = &hd
   };
   rtems_interrupt_server_entry e = {
+    .server = s,
     .vector = BSP_INTERRUPT_SERVER_MANAGEMENT_VECTOR,
     .actions = &a
   };
@@ -281,30 +313,32 @@ static rtems_status_code bsp_interrupt_server_call_helper(
   return hd.sc;
 }
 
-static rtems_interrupt_server_entry *bsp_interrupt_server_get_entry(void)
+static rtems_interrupt_server_entry *bsp_interrupt_server_get_entry(
+  bsp_interrupt_server_context *s
+)
 {
   rtems_interrupt_lock_context lock_context;
   rtems_interrupt_server_entry *e;
-  rtems_chain_control *chain;
 
-  rtems_interrupt_lock_acquire(&bsp_interrupt_server_lock, &lock_context);
-  chain = &bsp_interrupt_server_chain;
+  rtems_interrupt_lock_acquire(&s->lock, &lock_context);
 
-  if (!rtems_chain_is_empty(chain)) {
+  if (!rtems_chain_is_empty(&s->entries)) {
     e = (rtems_interrupt_server_entry *)
-      rtems_chain_get_first_unprotected(chain);
+      rtems_chain_get_first_unprotected(&s->entries);
     rtems_chain_set_off_chain(&e->node);
   } else {
     e = NULL;
   }
 
-  rtems_interrupt_lock_release(&bsp_interrupt_server_lock, &lock_context);
+  rtems_interrupt_lock_release(&s->lock, &lock_context);
 
   return e;
 }
 
 static void bsp_interrupt_server_task(rtems_task_argument arg)
 {
+  bsp_interrupt_server_context *s = (bsp_interrupt_server_context *) arg;
+
   while (true) {
     rtems_event_set events;
     rtems_interrupt_server_entry *e;
@@ -316,7 +350,7 @@ static void bsp_interrupt_server_task(rtems_task_argument arg)
       &events
     );
 
-    while ((e = bsp_interrupt_server_get_entry()) != NULL) {
+    while ((e = bsp_interrupt_server_get_entry(s)) != NULL) {
       rtems_interrupt_server_action *action = e->actions;
       rtems_vector_number vector = e->vector;
 
@@ -334,7 +368,7 @@ static void bsp_interrupt_server_task(rtems_task_argument arg)
 }
 
 rtems_status_code rtems_interrupt_server_handler_install(
-  rtems_id server,
+  uint32_t server_index,
   rtems_vector_number vector,
   const char *info,
   rtems_option options,
@@ -343,17 +377,15 @@ rtems_status_code rtems_interrupt_server_handler_install(
 )
 {
   rtems_status_code sc;
+  bsp_interrupt_server_context *s;
 
-  sc = bsp_interrupt_server_is_initialized();
-  if (sc != RTEMS_SUCCESSFUL) {
+  s = bsp_interrupt_server_get_context(server_index, &sc);
+  if (s == NULL) {
     return sc;
   }
 
-  if (server != RTEMS_ID_NONE) {
-    return RTEMS_NOT_IMPLEMENTED;
-  }
-
   return bsp_interrupt_server_call_helper(
+    s,
     vector,
     options,
     handler,
@@ -363,24 +395,22 @@ rtems_status_code rtems_interrupt_server_handler_install(
 }
 
 rtems_status_code rtems_interrupt_server_handler_remove(
-  rtems_id server,
+  uint32_t server_index,
   rtems_vector_number vector,
   rtems_interrupt_handler handler,
   void *arg
 )
 {
   rtems_status_code sc;
+  bsp_interrupt_server_context *s;
 
-  sc = bsp_interrupt_server_is_initialized();
-  if (sc != RTEMS_SUCCESSFUL) {
+  s = bsp_interrupt_server_get_context(server_index, &sc);
+  if (s == NULL) {
     return sc;
   }
 
-  if (server != RTEMS_ID_NONE) {
-    return RTEMS_NOT_IMPLEMENTED;
-  }
-
   return bsp_interrupt_server_call_helper(
+    s,
     vector,
     0,
     handler,
@@ -426,7 +456,7 @@ static void bsp_interrupt_server_handler_iterate_helper(void *arg)
 }
 
 rtems_status_code rtems_interrupt_server_handler_iterate(
-  rtems_id server,
+  uint32_t server_index,
   rtems_vector_number vector,
   rtems_interrupt_per_handler_routine routine,
   void *arg
@@ -434,16 +464,13 @@ rtems_status_code rtems_interrupt_server_handler_iterate(
 {
   rtems_status_code sc;
   bsp_interrupt_server_handler_iterate_helper_data hihd;
+  bsp_interrupt_server_context *s;
 
-  sc = bsp_interrupt_server_is_initialized();
-  if (sc != RTEMS_SUCCESSFUL) {
+  s = bsp_interrupt_server_get_context(server_index, &sc);
+  if (s == NULL) {
     return sc;
   }
 
-  if (server != RTEMS_ID_NONE) {
-    return RTEMS_NOT_IMPLEMENTED;
-  }
-
   if (!bsp_interrupt_is_valid_vector(vector)) {
     return RTEMS_INVALID_ID;
   }
@@ -451,6 +478,7 @@ rtems_status_code rtems_interrupt_server_handler_iterate(
   hihd.routine = routine;
   hihd.arg = arg;
   return bsp_interrupt_server_call_helper(
+    s,
     vector,
     0,
     NULL,
@@ -464,42 +492,98 @@ rtems_status_code rtems_interrupt_server_initialize(
   size_t stack_size,
   rtems_mode modes,
   rtems_attribute attributes,
-  rtems_id *server
+  uint32_t *server_count
 )
 {
-  rtems_status_code sc = RTEMS_SUCCESSFUL;
+  uint32_t cpu_index;
+  uint32_t cpu_count;
+  uint32_t dummy;
+  bsp_interrupt_server_context *instances;
 
-  if (server != NULL) {
-    return RTEMS_NOT_IMPLEMENTED;
+  if (server_count == NULL) {
+    server_count = &dummy;
   }
 
-  sc = rtems_task_create(
-    rtems_build_name('I', 'R', 'Q', 'S'),
-    priority,
-    stack_size,
-    modes,
-    attributes,
-    &bsp_interrupt_server_id
-  );
-  if (sc != RTEMS_SUCCESSFUL) {
-    return RTEMS_TOO_MANY;
+  cpu_count = rtems_get_processor_count();
+
+#if defined(RTEMS_SMP)
+  instances = calloc(cpu_count, sizeof(*instances));
+  if (instances == NULL) {
+    return RTEMS_NO_MEMORY;
   }
+#else
+  instances = &bsp_interrupt_server_instance;
+#endif
+
+  for (cpu_index = 0; cpu_index < cpu_count; ++cpu_index) {
+    bsp_interrupt_server_context *s = &instances[cpu_index];
+    rtems_status_code sc;
+#if defined(RTEMS_SMP)
+    rtems_id scheduler;
+    cpu_set_t cpu;
+#endif
+
+    rtems_interrupt_lock_initialize(&s->lock, "Interrupt Server");
+    rtems_chain_initialize_empty(&s->entries);
+
+    sc = rtems_task_create(
+      rtems_build_name('I', 'R', 'Q', 'S'),
+      priority,
+      stack_size,
+      modes,
+      attributes,
+      &s->server
+    );
+    if (sc != RTEMS_SUCCESSFUL) {
+      *server_count = cpu_index;
 
-  sc = rtems_task_start(
-    bsp_interrupt_server_id,
-    bsp_interrupt_server_task,
-    0
-  );
-  _Assert(sc == RTEMS_SUCCESSFUL);
+#if defined(RTEMS_SMP)
+      if (cpu_index > 0) {
+        return RTEMS_SUCCESSFUL;
+      }
+
+      free(instances);
+#endif
+
+      return RTEMS_TOO_MANY;
+    }
+
+#if defined(RTEMS_SMP)
+    sc = rtems_scheduler_ident_by_processor(cpu_index, &scheduler);
+    _Assert(sc == RTEMS_SUCCESSFUL);
+
+    sc = rtems_task_set_scheduler(s->server, scheduler, priority);
+    _Assert(sc == RTEMS_SUCCESSFUL);
+
+    CPU_ZERO(&cpu);
+    CPU_SET(cpu_index, &cpu);
+    sc = rtems_task_set_affinity(s->server, sizeof(cpu), &cpu);
+    _Assert(sc == RTEMS_SUCCESSFUL);
+#endif
+
+    sc = rtems_task_start(
+      s->server,
+      bsp_interrupt_server_task,
+      (rtems_task_argument) s
+    );
+    _Assert(sc == RTEMS_SUCCESSFUL);
+  }
+
+#if defined(RTEMS_SMP)
+  bsp_interrupt_server_instances = instances;
+#endif
+  *server_count = cpu_index;
 
   return RTEMS_SUCCESSFUL;
 }
 
 static void bsp_interrupt_server_entry_initialize(
-  rtems_interrupt_server_entry *entry
+  rtems_interrupt_server_entry *entry,
+  bsp_interrupt_server_context *s
 )
 {
   rtems_chain_set_off_chain(&entry->node);
+  entry->server = s;
   entry->vector = BSP_INTERRUPT_SERVER_MANAGEMENT_VECTOR;
   entry->actions = NULL;
 }
@@ -517,11 +601,21 @@ static void bsp_interrupt_server_action_prepend(
   entry->actions = action;
 }
 
-void rtems_interrupt_server_entry_initialize(
+rtems_status_code rtems_interrupt_server_entry_initialize(
+  uint32_t                      server_index,
   rtems_interrupt_server_entry *entry
 )
 {
-  bsp_interrupt_server_entry_initialize(entry);
+  rtems_status_code sc;
+  bsp_interrupt_server_context *s;
+
+  s = bsp_interrupt_server_get_context(server_index, &sc);
+  if (s == NULL) {
+    return sc;
+  }
+
+  bsp_interrupt_server_entry_initialize(entry, s);
+  return RTEMS_SUCCESSFUL;
 }
 
 void rtems_interrupt_server_action_prepend(
@@ -535,7 +629,6 @@ void rtems_interrupt_server_action_prepend(
 }
 
 void rtems_interrupt_server_entry_submit(
-  rtems_id                      server,
   rtems_interrupt_server_entry *entry
 )
 {
@@ -550,22 +643,24 @@ static void bsp_interrupt_server_entry_destroy_helper(void *arg)
 }
 
 void rtems_interrupt_server_entry_destroy(
-  rtems_id                      server,
   rtems_interrupt_server_entry *entry
 )
 {
+  bsp_interrupt_server_context *s;
   rtems_interrupt_lock_context lock_context;
 
-  rtems_interrupt_lock_acquire(&bsp_interrupt_server_lock, &lock_context);
+  s = entry->server;
+  rtems_interrupt_lock_acquire(&s->lock, &lock_context);
 
   if (!rtems_chain_is_node_off_chain(&entry->node)) {
     rtems_chain_extract_unprotected(&entry->node);
     rtems_chain_set_off_chain(&entry->node);
   }
 
-  rtems_interrupt_lock_release(&bsp_interrupt_server_lock, &lock_context);
+  rtems_interrupt_lock_release(&s->lock, &lock_context);
 
   bsp_interrupt_server_call_helper(
+    s,
     BSP_INTERRUPT_SERVER_MANAGEMENT_VECTOR,
     0,
     NULL,
@@ -574,17 +669,27 @@ void rtems_interrupt_server_entry_destroy(
   );
 }
 
-void rtems_interrupt_server_request_initialize(
+rtems_status_code rtems_interrupt_server_request_initialize(
+  uint32_t                        server_index,
   rtems_interrupt_server_request *request,
   rtems_interrupt_handler         handler,
   void                           *arg
 )
 {
-  bsp_interrupt_server_entry_initialize(&request->entry);
+  rtems_status_code sc;
+  bsp_interrupt_server_context *s;
+
+  s = bsp_interrupt_server_get_context(server_index, &sc);
+  if (s == NULL) {
+    return sc;
+  }
+
+  bsp_interrupt_server_entry_initialize(&request->entry, s);
   bsp_interrupt_server_action_prepend(
     &request->entry,
     &request->action,
     handler,
     arg
   );
+  return RTEMS_SUCCESSFUL;
 }