ims: Updated IMS sensor addressing

The IMS library will now change its I2C address after resetting the HW to a new
address.  It will attempt to close the mraa context and re-init each
time.

Signed-off-by: Noel Eck <noel.eck@intel.com>

src/ims/ims.c

@@ -28,37 +28,25 @@
 #include "ims.h"
 #include "upm_utilities.h"
 
-ims_context* ims_init(int16_t i2c_bus, int16_t i2c_address)
+static upm_result_t i2c_init(ims_context* dev, int16_t i2c_bus, int16_t i2c_address)
 {
-    /* Allocate space for the sensor structure */
-    ims_context* dev = (ims_context*) malloc(sizeof(ims_context));
-    if(dev == NULL)
-    {
-        syslog(LOG_CRIT, "%s: malloc() failed\n", __FUNCTION__);
-        goto ims_init_fail;
-    }
-
-    /* Initialize mraa */
-    mraa_result_t result = mraa_init();
-    if (result != MRAA_SUCCESS)
-    {
-        syslog(LOG_ERR, "%s: mraa_init() failed (%d)\n", __FUNCTION__, result);
-        goto ims_init_fail;
-    }
+    /* Attempt to stop the i2c device context if previously initialized */
+    if (dev->_i2c_context != NULL)
+        mraa_i2c_stop(dev->_i2c_context);
 
     /* Init i2c */
     dev->_i2c_context = mraa_i2c_init(i2c_bus);
     if(dev->_i2c_context == NULL)
     {
         syslog(LOG_ERR, "%s: mraa_i2c_init() failed\n", __FUNCTION__);
-        goto ims_init_fail;
+        return UPM_ERROR_OPERATION_FAILED;
     }
 
     /* Set the i2c slave address for this device */
     if (mraa_i2c_address(dev->_i2c_context, i2c_address) != MRAA_SUCCESS)
     {
         syslog(LOG_ERR, "%s: mraa_i2c_address() failed\n", __FUNCTION__);
-        goto ims_init_fail;
+        return UPM_ERROR_OPERATION_FAILED;
     }
 
     /* This device must run at 100kHz */
@@ -67,17 +55,41 @@ ims_context* ims_init(int16_t i2c_bus, int16_t i2c_address)
         syslog(LOG_ERR, "%s: mraa_i2c_frequency() failed.  %s\n",
                 __FUNCTION__,
                 "This device requires I2C standard mode (100 kb/s)");
-        goto ims_init_fail;
+        return UPM_ERROR_OPERATION_FAILED;
+    }
+    /* Save the new bus */
+    dev->_i2c_bus = i2c_bus;
+
+    return UPM_SUCCESS;
+}
+
+ims_context* ims_init(int16_t i2c_bus, int16_t i2c_address)
+{
+    /* Allocate space for the sensor structure */
+    ims_context* dev = (ims_context*) calloc(1, sizeof(ims_context));
+    if(dev == NULL)
+    {
+        syslog(LOG_CRIT, "%s: malloc() failed\n", __FUNCTION__);
+        return NULL;
+    }
+
+    /* Initialize mraa */
+    mraa_result_t result = mraa_init();
+    if (result != MRAA_SUCCESS)
+    {
+        syslog(LOG_ERR, "%s: mraa_init() failed (%d)\n", __FUNCTION__, result);
+        free(dev);
+        return NULL;
+    }
+
+    /* Setup the I2C bus */
+    if (i2c_init(dev, i2c_bus, i2c_address) != UPM_SUCCESS)
+    {
+        free(dev);
+        return NULL;
     }
 
     return dev;
-
-/* Handle all failing cases here */
-ims_init_fail:
-    /* Free structure memory if allocated */
-    if (dev != NULL)
-        free(dev);
-    return NULL;
 }
 
 void ims_close(ims_context* dev)
@@ -196,12 +208,20 @@ upm_result_t ims_reset(const ims_context* dev)
     return ims_write(dev, IMS_RESET, 0);
 }
 
-upm_result_t ims_reset_i2c_address(const ims_context* dev, uint8_t address_new)
+upm_result_t ims_reset_i2c_address(ims_context* dev, uint8_t address_new)
 {
+    /* Set the new address */
     upm_result_t res = ims_write(dev, IMS_SET_ADDRESS, address_new);
     if (res != UPM_SUCCESS) return res;
 
-    return ims_reset(dev);
+    /* Reset the device to load the new I2C address */
+    res = ims_reset(dev);
+    if (res != UPM_SUCCESS) return res;
+
+    /* Re-init the I2C bus */
+    res = i2c_init(dev, dev->_i2c_bus, address_new);
+
+    return res;
 }
 
 upm_result_t ims_sleep(const ims_context* dev)

src/ims/ims.h

@@ -51,6 +51,8 @@ extern "C" {
 typedef struct {
     /* mraa i2c context */
     mraa_i2c_context _i2c_context;
+    /* Save the I2C bus (used when changing the device address) */
+    int16_t _i2c_bus;
 } ims_context;
 
 /**
@@ -136,7 +138,7 @@ upm_result_t ims_reset(const ims_context* dev);
  * @param address_new New I2C for device
  * @return Function result code
  */
-upm_result_t ims_reset_i2c_address(const ims_context* dev, uint8_t address_new);
+upm_result_t ims_reset_i2c_address(ims_context* dev, uint8_t address_new);
 
 /**
  * Put device into low-power mode.  Device wakes on any I2C command.