[PATCH] cpukit/libi2c allowed to probe on device init

[Alexandru-Sever Horin]

From: Alexandru-Sever Horin <alex.sever.h at gmail.com>

libi2c registration of a device fails if the device initialization fails
Benefits: allows for device probing on multiple addresses
---
 cpukit/libi2c/libi2c.c | 27 ++++++++++++++++-----------
 1 file changed, 16 insertions(+), 11 deletions(-)

diff --git a/cpukit/libi2c/libi2c.c b/cpukit/libi2c/libi2c.c
index 233cb68..315a761 100644
--- a/cpukit/libi2c/libi2c.c
+++ b/cpukit/libi2c/libi2c.c
@@ -736,6 +736,16 @@ rtems_libi2c_register_drv (const char *name, rtems_libi2c_drv_t * drvtbl,
       /* found a free slot; encode slot + 1 ! */
       minor = ((i + 1) << 13) | RTEMS_LIBI2C_MAKE_MINOR (busno, i2caddr);
 
+      /* before registering, try to initialize the device to check it's there */
+      if (drvtbl->ops->initialization_entry) {
+        err = drvtbl->ops->initialization_entry (rtems_libi2c_major, minor, 0);
+        if (err) {
+          LIBUNLOCK ();
+          /* returned value must be negative on failure */
+          return err < 0 ? err : -err;
+        }
+      }
+
       if (name) {
         size_t length = strlen (busses[busno].name) + strlen (name) + 2;
         str = malloc (length);
@@ -751,9 +761,10 @@ rtems_libi2c_register_drv (const char *name, rtems_libi2c_drv_t * drvtbl,
 
         /* note that 'umask' is applied to 'mode' */
         if (mknod (str, mode, dev)) {
-          safe_printf( DRVNM
-                   "Creating device node failed: %s; you can try to do it manually...\n",
-                   strerror (errno));
+          safe_printf ( DRVNM
+                        "Creating device node failed: %s;\n"
+                        "you can try to do it manually...\n",
+                        strerror (errno));
         }
 
         free (str);
@@ -761,17 +772,11 @@ rtems_libi2c_register_drv (const char *name, rtems_libi2c_drv_t * drvtbl,
 
       drvs[i].drv = drvtbl;
 
-      if (drvtbl->ops->initialization_entry)
-        err =
-          drvs[i].drv->ops->initialization_entry (rtems_libi2c_major, minor,
-                                                  0);
-      else
-        err = RTEMS_SUCCESSFUL;
-
       LIBUNLOCK ();
-      return err ? -err : minor;
+      return minor;
     }
   }
+
   LIBUNLOCK ();
   return -RTEMS_TOO_MANY;
 }
-- 
2.3.5