usb: Added gpio read/write support for ft4222 with i/o expander

Signed-off-by: Henry Bruce <henry.bruce@intel.com>
Signed-off-by: Brendan Le Foll <brendan.le.foll@intel.com>

include/mraa_adv_func.h

@@ -28,7 +28,12 @@
 #include "mraa.h"
 #include "types.h"
 
+// FIXME: Nasty macro to test for presence of function in context structure function table
+#define IS_FUNC_DEFINED(dev, func)   (dev != NULL && dev->advance_func != NULL && dev->advance_func->func != NULL)
+
+
 typedef struct {
+    mraa_result_t (*gpio_init_internal_replace) (int pin);
     mraa_result_t (*gpio_init_pre) (int pin);
     mraa_result_t (*gpio_init_post) (mraa_gpio_context dev);
 
@@ -37,11 +42,15 @@ typedef struct {
     mraa_result_t (*gpio_mode_replace) (mraa_gpio_context dev, mraa_gpio_mode_t mode);
     mraa_result_t (*gpio_mode_pre) (mraa_gpio_context dev, mraa_gpio_mode_t mode);
     mraa_result_t (*gpio_mode_post) (mraa_gpio_context dev, mraa_gpio_mode_t mode);
+    
+    mraa_result_t (*gpio_edge_mode_replace) (mraa_gpio_context dev, mraa_gpio_edge_t mode);
 
     mraa_result_t (*gpio_dir_replace) (mraa_gpio_context dev, mraa_gpio_dir_t dir);
     mraa_result_t (*gpio_dir_pre) (mraa_gpio_context dev, mraa_gpio_dir_t dir);
     mraa_result_t (*gpio_dir_post) (mraa_gpio_context dev, mraa_gpio_dir_t dir);
 
+    int (*gpio_read_replace) (mraa_gpio_context dev);
+    mraa_result_t (*gpio_write_replace) (mraa_gpio_context dev, int value);
     mraa_result_t (*gpio_write_pre) (mraa_gpio_context dev, int value);
     mraa_result_t (*gpio_write_post) (mraa_gpio_context dev, int value);
     mraa_result_t (*gpio_mmap_setup) (mraa_gpio_context dev, mraa_boolean_t en);
@@ -63,8 +72,6 @@ typedef struct {
     mraa_result_t (*i2c_write_word_data_replace) (mraa_i2c_context dev, const uint16_t data, const uint8_t command);
     mraa_result_t (*i2c_stop_replace) (mraa_i2c_context dev);
 
-
-
     mraa_result_t (*aio_get_valid_fp) (mraa_aio_context dev);
     mraa_result_t (*aio_init_pre) (unsigned int aio);
     mraa_result_t (*aio_init_post) (mraa_aio_context dev);

src/gpio/gpio.c

@@ -52,56 +52,14 @@ mraa_gpio_get_valfp(mraa_gpio_context dev)
     return MRAA_SUCCESS;
 }
 
-mraa_gpio_context
+
-mraa_gpio_init(int pin)
+static mraa_gpio_context
+mraa_gpio_init_internal(mraa_adv_func_t* func_table, int pin)
 {
-    if (plat == NULL) {
-        syslog(LOG_ERR, "gpio: platform not initialised");
-        return NULL;
-    }
-    if (pin < 0 || pin > plat->phy_pin_count) {
-        syslog(LOG_ERR, "gpio: pin %i beyond platform definition", pin);
-        return NULL;
-    }
-    if (plat->pins[pin].capabilites.gpio != 1) {
-        syslog(LOG_ERR, "gpio: pin %i not capable of gpio", pin);
-        return NULL;
-    }
-    if (plat->pins[pin].gpio.mux_total > 0) {
-        if (mraa_setup_mux_mapped(plat->pins[pin].gpio) != MRAA_SUCCESS) {
-            syslog(LOG_ERR, "gpio: unable to setup muxes");
-            return NULL;
-        }
-    }
-
-    mraa_gpio_context r = mraa_gpio_init_raw(plat->pins[pin].gpio.pinmap);
-    if (r == NULL) {
-        syslog(LOG_CRIT, "gpio: mraa_gpio_init_raw(%d) returned error", pin);
-        return NULL;
-    }
-    r->phy_pin = pin;
-
-    if (advance_func->gpio_init_post != NULL) {
-        mraa_result_t ret = advance_func->gpio_init_post(r);
-        if (ret != MRAA_SUCCESS) {
-            free(r);
-            return NULL;
-        }
-    }
-    return r;
-}
-
-mraa_gpio_context
-mraa_gpio_init_raw(int pin)
-{
-    if (advance_func->gpio_init_pre != NULL) {
-        if (advance_func->gpio_init_pre(pin) != MRAA_SUCCESS)
-            return NULL;
-    }
-
     if (pin < 0)
         return NULL;
 
+    mraa_result_t status = MRAA_SUCCESS;
     char bu[MAX_SIZE];
     int length;
 
@@ -110,6 +68,22 @@ mraa_gpio_init_raw(int pin)
         syslog(LOG_CRIT, "gpio: Failed to allocate memory for context");
         return NULL;
     }
+    
+    dev->advance_func = func_table;
+    
+    if (IS_FUNC_DEFINED(dev, gpio_init_internal_replace)) {
+        status = dev->advance_func->gpio_init_internal_replace(pin);
+        if (status == MRAA_SUCCESS)
+            return dev;
+        else
+            goto init_internal_cleanup;
+    }
+        
+    if (IS_FUNC_DEFINED(dev, gpio_init_pre)) {
+        status = dev->advance_func->gpio_init_pre(pin);
+        if (status != MRAA_SUCCESS)
+            goto init_internal_cleanup;
+    }
 
     memset(dev, 0, sizeof(struct _gpio));
     dev->value_fp = -1;
@@ -127,23 +101,86 @@ mraa_gpio_init_raw(int pin)
         int export = open(SYSFS_CLASS_GPIO "/export", O_WRONLY);
         if (export == -1) {
             syslog(LOG_ERR, "gpio: Failed to open export for writing");
-            free(dev);
+            status = MRAA_ERROR_NO_RESOURCES;
-            return NULL;
+            goto init_internal_cleanup;
         }
         length = snprintf(bu, sizeof(bu), "%d", dev->pin);
         if (write(export, bu, length * sizeof(char)) == -1) {
             syslog(LOG_ERR, "gpio: Failed to write %d to export", dev->pin);
             close(export);
-            free(dev);
+            status = MRAA_ERROR_NO_RESOURCES;
-            return NULL;
+            goto init_internal_cleanup;
         }
         dev->owner = 1;
         close(export);
     }
 
+init_internal_cleanup:
+    if (status != MRAA_SUCCESS) {
+        if (dev != NULL)
+            free(dev);
+        return NULL;
+    }
     return dev;
 }
 
+mraa_gpio_context
+mraa_gpio_init(int pin)
+{
+    mraa_board_t* board = plat;
+    if (board == NULL) {
+        syslog(LOG_ERR, "gpio: platform not initialised");
+        return NULL;
+    }
+    
+    if (mraa_is_on_sub_platform(pin)) {
+        syslog(LOG_NOTICE, "gpio: Using sub platform");
+        board = board->sub_platform;
+        if (board == NULL) {
+            syslog(LOG_ERR, "gpio: Sub platform Not Initialised");
+            return NULL;
+        }
+        pin = mraa_get_sub_platform_index(pin);
+    }
+    
+    if (pin < 0 || pin > board->phy_pin_count) {
+        syslog(LOG_ERR, "gpio: pin %i beyond platform definition", pin);
+        return NULL;
+    }
+    if (board->pins[pin].capabilites.gpio != 1) {
+        syslog(LOG_ERR, "gpio: pin %i not capable of gpio", pin);
+        return NULL;
+    }
+    if (board->pins[pin].gpio.mux_total > 0) {
+        if (mraa_setup_mux_mapped(board->pins[pin].gpio) != MRAA_SUCCESS) {
+            syslog(LOG_ERR, "gpio: unable to setup muxes");
+            return NULL;
+        }
+    }
+
+    mraa_gpio_context r = mraa_gpio_init_internal(board->adv_func, board->pins[pin].gpio.pinmap);
+    if (r == NULL) {
+        syslog(LOG_CRIT, "gpio: mraa_gpio_init_raw(%d) returned error", pin);
+        return NULL;
+    }
+    r->phy_pin = pin;
+
+    if (IS_FUNC_DEFINED(r, gpio_init_post)) {
+        mraa_result_t ret = r->advance_func->gpio_init_post(r);
+        if (ret != MRAA_SUCCESS) {
+            free(r);
+            return NULL;
+        }
+    }
+    return r;
+}
+
+mraa_gpio_context
+mraa_gpio_init_raw(int pin)
+{
+    return mraa_gpio_init_internal(plat == NULL ? NULL : plat->adv_func , pin);
+}
+
 
 static mraa_result_t
 mraa_gpio_wait_interrupt(int fd)
@@ -232,6 +269,9 @@ mraa_gpio_interrupt_handler(void* arg)
 mraa_result_t
 mraa_gpio_edge_mode(mraa_gpio_context dev, mraa_gpio_edge_t mode)
 {
+    if (IS_FUNC_DEFINED(dev, gpio_edge_mode_replace))
+        return dev->advance_func->gpio_edge_mode_replace(dev, mode);
+    
     if (dev->value_fp != -1) {
         close(dev->value_fp);
         dev->value_fp = -1;
@@ -329,10 +369,10 @@ mraa_gpio_isr_exit(mraa_gpio_context dev)
 mraa_result_t
 mraa_gpio_mode(mraa_gpio_context dev, mraa_gpio_mode_t mode)
 {
-    if (advance_func->gpio_mode_replace != NULL)
+    if (IS_FUNC_DEFINED(dev, gpio_mode_replace))
-        return advance_func->gpio_mode_replace(dev, mode);
+        return dev->advance_func->gpio_mode_replace(dev, mode);
 
-    if (advance_func->gpio_mode_pre != NULL) {
+    if (IS_FUNC_DEFINED(dev, gpio_mode_pre)) {
         mraa_result_t pre_ret = (advance_func->gpio_mode_pre(dev, mode));
         if (pre_ret != MRAA_SUCCESS)
             return pre_ret;
@@ -378,19 +418,19 @@ mraa_gpio_mode(mraa_gpio_context dev, mraa_gpio_mode_t mode)
     }
 
     close(drive);
-    if (advance_func->gpio_mode_post != NULL)
+    if (IS_FUNC_DEFINED(dev, gpio_mode_post))    
-        return advance_func->gpio_mode_post(dev, mode);
+        return dev->advance_func->gpio_mode_post(dev, mode);
     return MRAA_SUCCESS;
 }
 
 mraa_result_t
 mraa_gpio_dir(mraa_gpio_context dev, mraa_gpio_dir_t dir)
 {
-    if (advance_func->gpio_dir_replace != NULL) {
+    if (IS_FUNC_DEFINED(dev, gpio_dir_replace))
-        return advance_func->gpio_dir_replace(dev, dir);
+        return dev->advance_func->gpio_dir_replace(dev, dir);
-    }
+    
-    if (advance_func->gpio_dir_pre != NULL) {
+    if (IS_FUNC_DEFINED(dev, gpio_dir_pre)) {
-        mraa_result_t pre_ret = (advance_func->gpio_dir_pre(dev, dir));
+        mraa_result_t pre_ret = (dev->advance_func->gpio_dir_pre(dev, dir));
         if (pre_ret != MRAA_SUCCESS) {
             return pre_ret;
         }
@@ -427,7 +467,7 @@ mraa_gpio_dir(mraa_gpio_context dev, mraa_gpio_dir_t dir)
         case MRAA_GPIO_OUT:
             length = snprintf(bu, sizeof(bu), "out");
             break;
-        case MRAA_GPIO_IN:
+        case MRAA_GPIO_IN:printf("gpio_init_internal_replace not defined\n");
             length = snprintf(bu, sizeof(bu), "in");
             break;
         case MRAA_GPIO_OUT_HIGH:
@@ -447,8 +487,8 @@ mraa_gpio_dir(mraa_gpio_context dev, mraa_gpio_dir_t dir)
     }
 
     close(direction);
-    if (advance_func->gpio_dir_post != NULL)
+    if (IS_FUNC_DEFINED(dev, gpio_dir_post))
-        return advance_func->gpio_dir_post(dev, dir);
+        return dev->advance_func->gpio_dir_post(dev, dir);
     return MRAA_SUCCESS;
 }
 
@@ -458,6 +498,9 @@ mraa_gpio_read(mraa_gpio_context dev)
     if (dev == NULL)
         return -1;
 
+    if (IS_FUNC_DEFINED(dev, gpio_read_replace))
+        return dev->advance_func->gpio_read_replace(dev);
+
     if (dev->mmap_read != NULL)
         return dev->mmap_read(dev);
 

src/usb/ftdi_ft4222.c

@@ -32,6 +32,7 @@
 
 #define PLATFORM_NAME "FTDI FT4222"
 #define I2CM_ERROR(status) (((status) & 0x02) != 0)
+#define PCA9672_ADDR 0x20
 
 
 static FT_HANDLE ftHandle = (FT_HANDLE)NULL;
@@ -78,10 +79,19 @@ mraa_ftdi_ft4222_init()
         goto init_exit;
     }
 
+/*
+        FT4222_Version ft4222Version;
+        FT4222_STATUS ft4222Status = FT4222_GetVersion(ftHandle, &ft4222Version);
+        if (FT4222_OK == ft4222Status){
+            syslog(LOG_NOTICE, "FT4222_GetVersion %08X %08X\n", ft4222Version.chipVersion, ft4222Version.dllVersion);
+         } else
+            syslog(LOG_ERR, "FT4222_GetVersion failed with code %d", ft4222Status);
+*/
+
     syslog(LOG_NOTICE, "FT_GetDeviceInfoList returned %d devices\n", numDevs);
     DWORD locationId = 0;
     for (i = 0; i < numDevs && locationId == 0; ++i) {
-printf("%d: type = %d, location = %d\n", i, devInfo[i].Type, devInfo[i].LocId);
+        // printf("%d: type = %d, location = %d\n", i, devInfo[i].Type, devInfo[i].LocId);
         if (devInfo[i].Type == FT_DEVICE_4222H_0 || devInfo[i].Type == FT_DEVICE_4222H_3)
            locationId = devInfo[i].LocId;
     } 
@@ -98,6 +108,25 @@ printf("%d: type = %d, location = %d\n", i, devInfo[i].Type, devInfo[i].LocId);
         mraaStatus = MRAA_ERROR_NO_RESOURCES;
         goto init_exit;
     }
+    
+    // Tell the FT4222 to be an I2C Master.
+    FT4222_STATUS ft4222Status = FT4222_I2CMaster_Init(ftHandle, bus_speed);
+    if (FT4222_OK != ft4222Status)
+    {
+        syslog(LOG_ERR, "FT4222_I2CMaster_Init failed (error %d)!\n", ft4222Status);
+        mraaStatus = MRAA_ERROR_NO_RESOURCES;
+        goto init_exit;
+    }
+    
+    // Reset the I2CM registers to a known state.
+    ft4222Status = FT4222_I2CMaster_Reset(ftHandle);
+    if (FT4222_OK != ft4222Status)
+    {
+        syslog(LOG_ERR, "FT4222_I2CMaster_Reset failed (error %d)!\n", ft4222Status);
+        mraaStatus = MRAA_ERROR_NO_RESOURCES;
+        goto init_exit;
+    }
+    
 
 init_exit:
     if (devInfo != NULL);
@@ -130,6 +159,54 @@ mraa_ftdi_ft4222_get_version(unsigned int* versionChip, unsigned int* versionLib
 }
 
 
+static int
+mraa_ftdi_ft4222_i2c_read_internal(FT_HANDLE handle, uint8_t addr, uint8_t* data, int length)
+{
+    uint16 bytesRead = 0;
+	uint8 controllerStatus;
+    syslog(LOG_NOTICE, "FT4222_I2CMaster_Read(%#02X, %#02X)", addr, length);
+    FT4222_STATUS ft4222Status = FT4222_I2CMaster_Read(handle, addr, data, length, &bytesRead);
+	ft4222Status = FT4222_I2CMaster_GetStatus(ftHandle, &controllerStatus);
+    if (FT4222_OK != ft4222Status || I2CM_ERROR(controllerStatus))
+    {
+        syslog(LOG_ERR, "FT4222_I2CMaster_Read failed (error %d)\n", (int)ft4222Status);
+        return 0;
+    }
+    return bytesRead;
+}
+
+static int
+mraa_ftdi_ft4222_i2c_write_internal(FT_HANDLE handle, uint8_t addr, const uint8_t* data, int bytesToWrite)
+{
+    uint16 bytesWritten = 0;
+	uint8 controllerStatus;
+    syslog(LOG_NOTICE, "FT4222_I2CMaster_Write(%#02X, %#02X, %d)", addr, *data, bytesToWrite);
+    FT4222_STATUS ft4222Status = FT4222_I2CMaster_Write(handle, addr, (uint8_t*)data, bytesToWrite, &bytesWritten);
+	ft4222Status = FT4222_I2CMaster_GetStatus(ftHandle, &controllerStatus);
+    if (FT4222_OK != ft4222Status || I2CM_ERROR(controllerStatus))
+    {
+        syslog(LOG_ERR, "FT4222_I2CMaster_Write failed (error %d)\n", (int)ft4222Status);
+        return 0;
+    }
+
+    if (bytesWritten != bytesToWrite)
+        syslog(LOG_ERR, "FT4222_I2CMaster_Write wrote %u of %u bytes.\n", bytesWritten, bytesToWrite);
+
+   return bytesWritten;
+}
+
+
+static mraa_boolean_t 
+mraa_ftdi_ft4222_detect_io_expander()
+{
+    uint8_t data;
+    return mraa_ftdi_ft4222_i2c_read_internal(ftHandle, PCA9672_ADDR, &data, 1) == 1;
+}
+
+
+
+
+/******************* I2C functions *******************/
 mraa_i2c_context
 mraa_ftdi_ft4222_i2c_init_raw(unsigned int bus)
 {
@@ -205,17 +282,7 @@ mraa_ftdi_ft4222_i2c_address(mraa_i2c_context dev, uint8_t addr)
 static int
 mraa_ftdi_ft4222_i2c_read(mraa_i2c_context dev, uint8_t* data, int length)
 {
-    uint16 bytesRead = 0;
+    return mraa_ftdi_ft4222_i2c_read_internal(dev->handle, dev->addr, data, length);
-	uint8 controllerStatus;
-    syslog(LOG_NOTICE, "FT4222_I2CMaster_Read(%02X, %d)", dev->addr, length);
-    FT4222_STATUS ft4222Status = FT4222_I2CMaster_Read(dev->handle, dev->addr, data, length, &bytesRead);
-	ft4222Status = FT4222_I2CMaster_GetStatus(ftHandle, &controllerStatus);
-    if (FT4222_OK != ft4222Status || I2CM_ERROR(controllerStatus))
-    {
-        syslog(LOG_ERR, "FT4222_I2CMaster_Read failed (error %d)\n", (int)ft4222Status);
-        return 0;
-    }
-    return bytesRead;
 }
 
 static uint8_t
@@ -246,21 +313,8 @@ mraa_ftdi_ft4222_i2c_read_bytes_data(mraa_i2c_context dev, uint8_t command, uint
 static mraa_result_t
 mraa_ftdi_ft4222_i2c_write(mraa_i2c_context dev, const uint8_t* data, int bytesToWrite)
 {
-    uint16 bytesWritten = 0;
+    uint16 bytesWritten = mraa_ftdi_ft4222_i2c_write_internal(dev->handle, dev->addr, data, bytesToWrite);
-	uint8 controllerStatus;
+    return bytesToWrite ==  bytesWritten ? MRAA_SUCCESS : MRAA_ERROR_INVALID_HANDLE;
-    syslog(LOG_NOTICE, "FT4222_I2CMaster_Write(%#02X, %#02X, %d)", dev->addr, *data, bytesToWrite);
-    FT4222_STATUS ft4222Status = FT4222_I2CMaster_Write(dev->handle, dev->addr, (uint8_t*)data, bytesToWrite, &bytesWritten);
-	ft4222Status = FT4222_I2CMaster_GetStatus(ftHandle, &controllerStatus);
-    if (FT4222_OK != ft4222Status || I2CM_ERROR(controllerStatus))
-    {
-        syslog(LOG_ERR, "FT4222_I2CMaster_Write failed (error %d)\n", (int)ft4222Status);
-        return MRAA_ERROR_INVALID_HANDLE;
-    }
-
-    if (bytesWritten != bytesToWrite)
-        syslog(LOG_ERR, "FT4222_I2CMaster_Write wrote %u of %u bytes.\n", bytesWritten, bytesToWrite);
-
-   return bytesToWrite == bytesWritten ? MRAA_SUCCESS : MRAA_ERROR_INVALID_HANDLE;
 }
 
 
@@ -297,40 +351,109 @@ mraa_ftdi_ft4222_i2c_stop(mraa_i2c_context dev)
     return MRAA_SUCCESS;
 }
 
+/******************* GPIO functions *******************/
 
-
+static mraa_result_t
-
+mraa_ftdi_ft4222_gpio_init_internal_replace(int pin)
-mraa_adv_func_t*
-mraa_i2c_ft4222_create_func_table()
 {
-    mraa_adv_func_t* func_table = (mraa_adv_func_t*) calloc(1, sizeof(mraa_adv_func_t));
+    return MRAA_SUCCESS;
-    if (func_table != NULL) {
-        // func_table->i2c_init_raw_replace = &mraa_ftdi_ft4222_i2c_init_raw;
-        func_table->i2c_init_bus_replace = &mraa_ftdi_ft4222_i2c_init_bus_replace;
-        func_table->i2c_set_frequency_replace = &mraa_ftdi_ft4222_i2c_frequency;
-        func_table->i2c_address_replace = &mraa_ftdi_ft4222_i2c_address;
-        func_table->i2c_read_replace = &mraa_ftdi_ft4222_i2c_read;
-        func_table->i2c_read_byte_replace = &mraa_ftdi_ft4222_i2c_read_byte;
-        func_table->i2c_read_byte_data_replace = &mraa_ftdi_ft4222_i2c_read_byte_data;
-        func_table->i2c_read_word_data_replace = &mraa_ftdi_ft4222_i2c_read_word_data;
-        func_table->i2c_read_bytes_data_replace = &mraa_ftdi_ft4222_i2c_read_bytes_data;
-        func_table->i2c_write_replace = &mraa_ftdi_ft4222_i2c_write;
-        func_table->i2c_write_byte_replace = &mraa_ftdi_ft4222_i2c_write_byte;
-        func_table->i2c_write_byte_data_replace = &mraa_ftdi_ft4222_i2c_write_byte_data;
-        func_table->i2c_write_word_data_replace = &mraa_ftdi_ft4222_i2c_write_word_data;
-        func_table->i2c_stop_replace = &mraa_ftdi_ft4222_i2c_stop;    
-    }
-    return func_table;
 }
 
+static mraa_result_t
+mraa_ftdi_ft4222_gpio_mode_replace(mraa_gpio_context dev, mraa_gpio_mode_t mode)
+{
+    return MRAA_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+static mraa_result_t
+mraa_ftdi_ft4222_gpio_edge_mode_replace(mraa_gpio_context dev, mraa_gpio_edge_t mode)
+{
+    return MRAA_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+static int
+mraa_ftdi_ft4222_gpio_read_replace(mraa_gpio_context dev)
+{
+    uint8_t pin = dev->phy_pin;
+    uint8_t mask = 1 << pin;
+    uint8_t value;
+    if (mraa_ftdi_ft4222_i2c_read_internal(ftHandle, PCA9672_ADDR, &value, 1) != 1)
+        return -1;
+    return (value & mask) == mask;
+}
+
+
+static mraa_result_t
+mraa_ftdi_ft4222_gpio_write_replace(mraa_gpio_context dev, int write_value)
+{
+    uint8_t pin = dev->phy_pin;
+    uint8_t mask = 1 << pin;
+    uint8_t value;
+    if (mraa_ftdi_ft4222_i2c_read_internal(ftHandle, PCA9672_ADDR, &value, 1) != 1)
+        return MRAA_ERROR_UNSPECIFIED;
+    if (write_value == 1)
+        value |= mask;
+    else
+        value &= (~mask);
+    if (mraa_ftdi_ft4222_i2c_write_internal(ftHandle, PCA9672_ADDR, &value, 1) != 1)
+        return MRAA_ERROR_UNSPECIFIED;
+    return MRAA_SUCCESS;
+}
+
+static mraa_result_t
+mraa_ftdi_ft4222_gpio_dir_replace(mraa_gpio_context dev, mraa_gpio_dir_t dir)
+{
+    switch (dir) {
+    case MRAA_GPIO_IN:
+    case MRAA_GPIO_OUT_HIGH:        
+        return mraa_ftdi_ft4222_gpio_write_replace(dev, 1);
+    case MRAA_GPIO_OUT_LOW:        
+        return mraa_ftdi_ft4222_gpio_write_replace(dev, 0);
+    default:
+        return MRAA_SUCCESS;
+    }
+}
+    
+
+static void
+mraa_ftdi_ft4222_populate_i2c_func_table(mraa_adv_func_t* func_table)
+{
+    func_table->i2c_init_bus_replace = &mraa_ftdi_ft4222_i2c_init_bus_replace;
+    func_table->i2c_set_frequency_replace = &mraa_ftdi_ft4222_i2c_frequency;
+    func_table->i2c_address_replace = &mraa_ftdi_ft4222_i2c_address;
+    func_table->i2c_read_replace = &mraa_ftdi_ft4222_i2c_read;
+    func_table->i2c_read_byte_replace = &mraa_ftdi_ft4222_i2c_read_byte;
+    func_table->i2c_read_byte_data_replace = &mraa_ftdi_ft4222_i2c_read_byte_data;
+    func_table->i2c_read_word_data_replace = &mraa_ftdi_ft4222_i2c_read_word_data;
+    func_table->i2c_read_bytes_data_replace = &mraa_ftdi_ft4222_i2c_read_bytes_data;
+    func_table->i2c_write_replace = &mraa_ftdi_ft4222_i2c_write;
+    func_table->i2c_write_byte_replace = &mraa_ftdi_ft4222_i2c_write_byte;
+    func_table->i2c_write_byte_data_replace = &mraa_ftdi_ft4222_i2c_write_byte_data;
+    func_table->i2c_write_word_data_replace = &mraa_ftdi_ft4222_i2c_write_word_data;
+    func_table->i2c_stop_replace = &mraa_ftdi_ft4222_i2c_stop;    
+}
+
+static void
+mraa_ftdi_ft4222_populate_gpio_func_table(mraa_adv_func_t* func_table)
+{
+    func_table->gpio_init_internal_replace = &mraa_ftdi_ft4222_gpio_init_internal_replace;
+    func_table->gpio_mode_replace = &mraa_ftdi_ft4222_gpio_mode_replace;
+    func_table->gpio_edge_mode_replace = &mraa_ftdi_ft4222_gpio_edge_mode_replace;    
+    func_table->gpio_dir_replace = &mraa_ftdi_ft4222_gpio_dir_replace;
+    func_table->gpio_read_replace = &mraa_ftdi_ft4222_gpio_read_replace;    
+    func_table->gpio_write_replace = &mraa_ftdi_ft4222_gpio_write_replace;        
+}
+
+
 mraa_board_t*
 mraa_ftdi_ft4222()
 {
     mraa_board_t* sub_plat = (mraa_board_t*) calloc(1, sizeof(mraa_board_t));
     if (sub_plat == NULL)
         return NULL;
+    mraa_boolean_t haveGpio = mraa_ftdi_ft4222_detect_io_expander();
     int pinIndex = 0;
-    int numUsbGpio = 0;
+    int numUsbGpio = haveGpio ? 8 : 0;
     int numUsbPins = numUsbGpio + 2; // Add SDA and SCL
     sub_plat->platform_name = PLATFORM_NAME;
     sub_plat->phy_pin_count = numUsbPins;
@@ -340,7 +463,7 @@ mraa_ftdi_ft4222()
         return NULL;
     }
     sub_plat->pins = pins;
-
+    
     // Virtual gpio pins on i2c I/O expander.
     mraa_pincapabilities_t pinCapsGpio = (mraa_pincapabilities_t){ 1, 1, 0, 0, 0, 0, 0, 0 };
     for (pinIndex=0; pinIndex < numUsbGpio; ++pinIndex) {
@@ -354,8 +477,7 @@ mraa_ftdi_ft4222()
     sub_plat->i2c_bus_count = 1;
     sub_plat->def_i2c_bus = bus;
     sub_plat->i2c_bus[bus].bus_id = bus;
-    // sub_plat->i2c_bus[bus].drv_type = drv_type;
+    
-
     // i2c pins (these are virtual, entries are required to configure i2c layer)
     mraa_pincapabilities_t pinCapsI2c = (mraa_pincapabilities_t){ 1, 0, 0, 0, 0, 1, 0, 0 };
     strncpy(sub_plat->pins[pinIndex].name, "SDA", 8);
@@ -367,10 +489,17 @@ mraa_ftdi_ft4222()
     sub_plat->pins[pinIndex].capabilites = pinCapsI2c;
     sub_plat->pins[pinIndex].i2c.mux_total = 0;
     sub_plat->i2c_bus[bus].scl = pinIndex;
-
+    
     // Set override functions
-    sub_plat->adv_func = mraa_i2c_ft4222_create_func_table();
+    mraa_adv_func_t* func_table = (mraa_adv_func_t*) calloc(1, sizeof(mraa_adv_func_t));
+    if (func_table == NULL) {
+        return NULL;
+    }
+    mraa_ftdi_ft4222_populate_i2c_func_table(func_table);
+    if (haveGpio)
+        mraa_ftdi_ft4222_populate_gpio_func_table(func_table);    
 
+    sub_plat->adv_func = func_table;
     return sub_plat;
 }