From 6dae7890c18159f19da9a539f843489961644e29 Mon Sep 17 00:00:00 2001
From: Kent McLeod <kent@kry10.com>
Date: Fri, 16 Feb 2024 07:45:44 +1100
Subject: [PATCH] elfloader: Check return code of device init

Device initialization returns early if an error is encountered. Handle
this error by printing an Error message and aborting the booting
process.

Signed-off-by: Kent McLeod <kent@kry10.com>
---
 elfloader-tool/src/arch-arm/smp_boot.c |  5 ++++-
 elfloader-tool/src/arch-arm/sys_boot.c | 11 +++++++++--
 elfloader-tool/src/drivers/driver.c    | 10 ++++++++--
 3 files changed, 21 insertions(+), 5 deletions(-)

diff --git a/elfloader-tool/src/arch-arm/smp_boot.c b/elfloader-tool/src/arch-arm/smp_boot.c
index 2745211c..d429d113 100644
--- a/elfloader-tool/src/arch-arm/smp_boot.c
+++ b/elfloader-tool/src/arch-arm/smp_boot.c
@@ -40,7 +40,10 @@ void non_boot_main(void)
     }
 
     /* Do any driver specific non_boot core init */
-    initialise_devices_non_boot();
+    if (initialise_devices_non_boot()) {
+        printf("ERROR: Did not successfully return from initialise_devices_non_boot()\n");
+        abort();
+    }
 
 #ifndef CONFIG_ARM_HYPERVISOR_SUPPORT
     if (is_hyp_mode()) {
diff --git a/elfloader-tool/src/arch-arm/sys_boot.c b/elfloader-tool/src/arch-arm/sys_boot.c
index 9f6b17a0..bf98aaf2 100644
--- a/elfloader-tool/src/arch-arm/sys_boot.c
+++ b/elfloader-tool/src/arch-arm/sys_boot.c
@@ -104,7 +104,11 @@ void main(UNUSED void *arg)
     void *bootloader_dtb = NULL;
 
     /* initialize platform to a state where we can print to a UART */
-    initialise_devices();
+    if (initialise_devices()) {
+        printf("ERROR: Did not successfully return from initialise_devices()\n");
+        abort();
+    }
+
     platform_init();
 
     /* Print welcome message. */
@@ -175,7 +179,10 @@ void continue_boot(int was_relocated)
      * driver model so all its pointers are set up properly.
      */
     if (was_relocated) {
-        initialise_devices();
+        if (initialise_devices()) {
+            printf("ERROR: Did not successfully return from initialise_devices()\n");
+            abort();
+        }
     }
 
 #if (defined(CONFIG_ARCH_ARM_V7A) || defined(CONFIG_ARCH_ARM_V8A)) && !defined(CONFIG_ARM_HYPERVISOR_SUPPORT)
diff --git a/elfloader-tool/src/drivers/driver.c b/elfloader-tool/src/drivers/driver.c
index 5edd00ab..e433dc16 100644
--- a/elfloader-tool/src/drivers/driver.c
+++ b/elfloader-tool/src/drivers/driver.c
@@ -33,7 +33,10 @@ static int init_device(struct elfloader_device *dev)
         int ret = table_has_match(dev->compat, drv->match_table);
         if (ret >= 0) {
             dev->drv = drv;
-            drv->init(dev, drv->match_table[ret].match_data);
+            ret = drv->init(dev, drv->match_table[ret].match_data);
+            if (ret) {
+                return ret;
+            }
         }
         drvp++;
     }
@@ -64,7 +67,10 @@ static int init_device_non_boot(struct elfloader_device *dev)
             int ret = table_has_match(dev->compat, drv->match_table);
             if (ret >= 0) {
                 dev->drv = drv;
-                drv->init_on_secondary_cores(dev, drv->match_table[ret].match_data);
+                ret = drv->init_on_secondary_cores(dev, drv->match_table[ret].match_data);
+                if (ret) {
+                    return ret;
+                }
             }
         }
         drvp++;