plugins: unload plugin if early init fails

Change-Id: I32f68e2ee8f5d32962acdefb0193583f71d342b3
Signed-off-by: Damjan Marion <damarion@cisco.com>
diff --git a/src/vlib/init.h b/src/vlib/init.h
index 12db3f9..1eddbb1 100644
--- a/src/vlib/init.h
+++ b/src/vlib/init.h
@@ -79,6 +79,25 @@
   char name[32];
 } vlib_config_function_runtime_t;
 
+#define VLIB_REMOVE_FROM_LINKED_LIST(first,p,next)              \
+{                                                               \
+  if (first == p)                                               \
+      first = (p)->next;                                        \
+  else                                                          \
+    {                                                           \
+      __typeof__ (p) current = first;                           \
+	while (current->next)                                   \
+	  {                                                     \
+	    if (current->next == p)                             \
+	      {                                                 \
+		current->next = current->next->next;            \
+		break;                                          \
+	      }                                                 \
+	    current = current->next;                            \
+	  }                                                     \
+    }                                                           \
+}
+
 #define _VLIB_INIT_FUNCTION_SYMBOL(x, type)	\
   _vlib_##type##_function_##x
 
@@ -106,6 +125,30 @@
     = vm->tag##_function_registrations;                         \
   vm->tag##_function_registrations = &_vlib_init_function;      \
  _vlib_init_function.f = &x;                                    \
+}                                                               \
+static void __vlib_rm_##tag##_function_##x (void)               \
+    __attribute__((__destructor__)) ;                           \
+static void __vlib_rm_##tag##_function_##x (void)               \
+{                                                               \
+  vlib_main_t * vm = vlib_get_main();                           \
+  _vlib_init_function_list_elt_t *next;                         \
+  if (vm->tag##_function_registrations->f == &x)                \
+    {                                                           \
+      vm->tag##_function_registrations =                        \
+        vm->tag##_function_registrations->next_init_function;   \
+      return;                                                   \
+    }                                                           \
+  next = vm->tag##_function_registrations;                      \
+  while (next->next_init_function)                              \
+    {                                                           \
+      if (next->next_init_function->f == &x)                    \
+        {                                                       \
+          next->next_init_function =                            \
+            next->next_init_function->next_init_function;       \
+          return;                                               \
+        }                                                       \
+      next = next->next_init_function;                          \
+    }                                                           \
 }
 
 #define VLIB_INIT_FUNCTION(x) VLIB_DECLARE_INIT_FUNCTION(x,init)
@@ -129,6 +172,16 @@
     vm->config_function_registrations                           \
        = &VLIB_CONFIG_FUNCTION_SYMBOL(x);                       \
 }                                                               \
+static void __vlib_rm_config_function_##x (void)                \
+    __attribute__((__destructor__)) ;                           \
+static void __vlib_rm_config_function_##x (void)                \
+{                                                               \
+    vlib_main_t * vm = vlib_get_main();                         \
+    vlib_config_function_runtime_t *p =                         \
+       & VLIB_CONFIG_FUNCTION_SYMBOL (x);                       \
+    VLIB_REMOVE_FROM_LINKED_LIST                                \
+      (vm->config_function_registrations, p, next_registration);\
+}                                                               \
   vlib_config_function_runtime_t                                \
     VLIB_CONFIG_FUNCTION_SYMBOL (x)                             \
   = {                                                           \
@@ -150,6 +203,16 @@
     vm->config_function_registrations                           \
        = &VLIB_CONFIG_FUNCTION_SYMBOL(x);                       \
 }                                                               \
+static void __vlib_rm_config_function_##x (void)                \
+    __attribute__((__destructor__)) ;                           \
+static void __vlib_rm_config_function_##x (void)                \
+{                                                               \
+    vlib_main_t * vm = vlib_get_main();                         \
+    vlib_config_function_runtime_t *p =                         \
+       & VLIB_CONFIG_FUNCTION_SYMBOL (x);                       \
+    VLIB_REMOVE_FROM_LINKED_LIST                                \
+      (vm->config_function_registrations, p, next_registration);\
+}                                                               \
   vlib_config_function_runtime_t                                \
     VLIB_CONFIG_FUNCTION_SYMBOL (x)                             \
   = {                                                           \