File content as of revision 3:b902729a1675:

/* mbed simplified access to Microchip MCP23x17 GPIO expender devices (I2C)
 * Copyright (c) 2010-2012 ygarcia, MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
 * and associated documentation files (the "Software"), to deal in the Software without restriction, 
 * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or 
 * substantial pinions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
#include <iostream>
#include <sstream>

#include "MCP23017_I2C.h"

#define IODIRA      0x00
#define IODIRB      0x01
#define IPOLA       0x02
#define IPOLB       0x03
#define GPINTENA    0x04
#define GPINTENB    0x05
#define DEFVALA     0x06
#define DEFVALB     0x07
#define INTCONA     0x08
#define INTCONB     0x09
#define IOCONA      0x0a
#define IOCONB      0x0b
#define GPPUA       0x0c
#define GPPUB       0x0d
#define INTFA       0x0e
#define INTFB       0x0f
#define INTCAPA     0x10
#define INTCAPB     0x11
#define GPIOA       0x12
#define GPIOB       0x13
#define OLATA       0x14
#define OLATB       0x15

namespace MCP23017_I2C {

    unsigned char CMCP23017_I2C::I2CModuleRefCounter = 0;

    CMCP23017_I2C::CMCP23017_I2C(const PinName p_sda, const PinName p_scl, const unsigned char p_address, const PinName p_intA, const PinName p_intB, const PinName p_reset, const bool p_internalPullUp, const unsigned int p_frequency) : _gpioAFlags(0x00), _gpioBFlags(0x00), _buses(), _busesIndex(0x00), _internalId("") {
//        DEBUG_ENTER("CMCP23017_I2C")

        if (CMCP23017_I2C::I2CModuleRefCounter != 0) {
            error("CMCP23017_I2C: Wrong params");
        }
#ifdef __DEBUG
        std::ostringstream out(std::ostringstream::out);
        out << "CMCP23017_I2C #" << CMCP23017_I2C::I2CModuleRefCounter;
        _internalId.assign(out.str());
//        DEBUG("CMCP23017_I2C: _internalId='%s'", _internalId.c_str())
#endif // __DEBUG
        _i2cInstance = new I2C(p_sda, p_scl);
        CMCP23017_I2C::I2CModuleRefCounter += 1;
//        DEBUG_ENTER("CMCP23017_I2C: refCounter=%d", CMCP23017_I2C::I2CModuleRefCounter)

        _slaveAddress = (p_address << 1) | 0x40; // Slave address format is: 0 0 1 0 A3 A2 A1 R/W
//        DEBUG("CMCP23017_I2C: I2C slave adress: 0x%02x", _slaveAddress)
        _i2cInstance->frequency(p_frequency); // Set the frequency of the I2C interface
        
        if (p_intA != NC) {
//            DEBUG("CMCP23017_I2C: INTA managed");
            _intA = new InterruptIn(p_intA);
            if (p_internalPullUp) _intA->mode(::PullDown);
            _intA->enable_irq(); // Enable interrupt
        } else {
//            DEBUG("CMCP23017_I2C: INTA not managed");
            _intA = NULL; // Not used
        }
        if (p_intB != NC) {
//            DEBUG("CMCP23017_I2C: INTB managed");
            _intB = new InterruptIn(p_intB);
            if (p_internalPullUp) _intB->mode(::PullDown);
            _intB->enable_irq(); // Enable interrupt
        } else {
//            DEBUG("CMCP23017_I2C: INTB not managed");
            _intB = NULL; // Not used
        }
        if (p_reset != NC) {
//            DEBUG("CMCP23017_I2C: RESET managed");
            _reset = new DigitalOut(p_reset);
            _reset->write(1); // Disable reset
        } else {
//            DEBUG("CMCP23017_I2C: RESET not managed");
            _reset = NULL; // Not used
        }
    
//        DEBUG_LEAVE("CMCP23017_I2C")
    }

    CMCP23017_I2C::~CMCP23017_I2C() {
//        DEBUG_ENTER("~CMCP23017_I2C")
    
        // Release I2C instance
//        DEBUG("~CMCP23017_I2C: refCounter=%d", CMCP23017_I2C::I2CModuleRefCounter)
        CMCP23017_I2C::I2CModuleRefCounter -= 1;
        if (CMCP23017_I2C::I2CModuleRefCounter == 0) {
            delete _i2cInstance;
            _i2cInstance = NULL;
        }
        // Release _wp if required
        if (_intA != NULL) {
            delete _intA;
        }
        if (_intB != NULL) {
            delete _intB;
        }
        if (_reset != NULL) {
            delete _reset;
        }
    
//        DEBUG_LEAVE("~CMCP23017_I2C")
    }
    
    bool CMCP23017_I2C::Initialize(const unsigned char p_gpioAFlags, const unsigned char p_gpioBFlags) {
        
        // Configure default behavior
        _gpioAFlags = p_gpioAFlags;
        _gpioBFlags = p_gpioBFlags;
        configure(_gpioAFlags, _gpioBFlags);
        
        return true;
    }
    
    void CMCP23017_I2C::configure(const unsigned char p_gpioAFlags, const unsigned char p_gpioBFlags) { // TODO Optimization with sequential access
//        DEBUG_ENTER("CMCP23017_I2C::configure: 0x%02x 0x%02x", p_gpioAFlags, p_gpioBFlags)
        
        // Setup IOCON - See REGISTER 1-6: IOCON – I/O EXPANDER CONFIGURATION REGISTER
        writeRegister(IOCONA, 0x10); // Sequential operation disabled
        writeRegister(IOCONB, 0x10); // Sequential operation disabled
        // Setup IODIR - See REGISTER 1-1: IODIR – I/O DIRECTION REGISTER (ADDR 0x00)
        writeRegister(IODIRA, p_gpioAFlags);
        writeRegister(IODIRB, p_gpioBFlags);
        // Setup IPOL - See REGISTER 1-2: IPOL – INPUT POLARITY PORT REGISTER
        writeRegister(IPOLA, 0x00); // GPIO register bit will reflect the same logic state of the input pin
        writeRegister(IPOLB, 0x00);
        // Setup GPPU - See REGISTER 1-7: GPPU – GPIO PULL-UP RESISTOR REGISTER
        writeRegister(GPPUA, 0x00);
        writeRegister(GPPUB, 0x00);
        
        // Setup interrupt       
        if (_intA != NULL) {
//            DEBUG("CMCP23017_I2C::configure: Setup INTA")
            // Setup GPINTEN - See GPINTEN – INTERRUPT-ON-CHANGE PINS
            writeRegister(GPINTENA, 0x00); // Disable GPIO interrupt-on-change events
            // Setup DEFVAL - See REGISTER 1-4: DEFVAL – DEFAULT VALUE REGISTER
            writeRegister(DEFVALA, 0x00); // Pin level change from 0 to 1 raises an interrupt
            // Setup INTCON - REGISTER 1-5: INTCON – INTERRUPT-ON-CHANGE CONTROL REGISTER
            writeRegister(INTCONA, 0xff); // Pin level change from 0 to 1 raises an interrupt
        }
        if (_intB != NULL) {
//            DEBUG("CMCP23017_I2C::configure: Setup INTB")
            // Setup GPINTEN - See GPINTEN – INTERRUPT-ON-CHANGE PINS
            writeRegister(GPINTENB, 0x00); // Disable GPIO interrupt-on-change events
            // Setup DEFVAL - See REGISTER 1-4: DEFVAL – DEFAULT VALUE REGISTER
            writeRegister(DEFVALB, 0x00); // Pin level change from 0 to 1 raises an interrupt
            // Setup INTCON - REGISTER 1-5: INTCON – INTERRUPT-ON-CHANGE CONTROL REGISTER
            writeRegister(INTCONB, 0xff); // Pin level change from 0 to 1 raises an interrupt
        }
        
//        DumpRegisters();
        
//        DEBUG_LEAVE("CMCP23017_I2C::configure")
    }
    
    void CMCP23017_I2C::reset() {
//        DEBUG_ENTER("CMCP23017_I2C::reset")
        if (_reset != NULL) {
            _reset->write(0);
            wait_us(1);
            _reset->write(1);
            wait_us(1);
            if ((_gpioAFlags != 0x00) || (_gpioBFlags != 0x00)) { // Apply configuration
                configure(_gpioAFlags, _gpioBFlags);
            } // else, POR reset values, see DS21952B-page 10 Clause 1.6 Configuration and Control Registers
        }
//        DEBUG_LEAVE("CMCP23017_I2C::reset")
    }
    
    void CMCP23017_I2C::setIntrACallback(void (* p_fptr)(void)) {
        if (_intA != NULL) {
            unsigned char vregister;
            if (!readRegister(IOCONA, &vregister)) {
                return;    
            }
            if (isBitSet(vregister, 1)) {
                _intA->rise(p_fptr);
            } else {
                _intA->fall(p_fptr);
            }
        }
    }
    
    void CMCP23017_I2C::setIntrBCallback(void (* p_fptr)(void)) {
        if (_intB != NULL) {
            unsigned char vregister;
            if (!readRegister(IOCONB, &vregister)) {
                return;    
            }
            if (isBitSet(vregister, 1)) {
                _intB->rise(p_fptr);
            } else {
                _intB->fall(p_fptr);
            }
        }
    }
        
    template<typename T> 
    void CMCP23017_I2C::setIntrACallback (const T * p_tptr, void(T::* p_mptr)(void)) {
        if (_intA != NULL) {
            unsigned char vregister;
            if (!readRegister(IOCONA, &vregister)) {
                return;    
            }
            if (isBitSet(vregister, 1)) {
                _intA->rise(p_tptr, p_mptr);
            } else {
                _intA->fall(p_tptr, p_mptr);
            }
        }
    }
    
    template<typename T> 
    void CMCP23017_I2C::setIntrBCallback (const T * p_tptr, void(T::* p_mptr)(void)) {
            unsigned char vregister;
            if (!readRegister(IOCONB, &vregister)) {
                return;    
            }
            if (isBitSet(vregister, 1)) {
                _intB->rise(p_tptr, p_mptr);
            } else {
                _intB->fall(p_tptr, p_mptr);
            }
    }
    
    void CMCP23017_I2C::setupInterrupts(const unsigned char p_mirroring, const unsigned char p_openDrain, const unsigned char p_polarity) {
        DEBUG_ENTER("CMCP23017_I2C::setupInterrupts: %02x, %02x, %02x", p_mirroring, p_openDrain, p_polarity)
        
        // Setup IOCONA
        unsigned char vregister;
        if (!readRegister(INTCONA, &vregister)) {
            return;    
        }
        // Setup INTCON - REGISTER 1-5: INTCON – INTERRUPT-ON-CHANGE CONTROL REGISTER
        vregister = setBit(vregister, 6, p_mirroring);
        vregister = setBit(vregister, 2, p_openDrain);
        vregister = setBit(vregister, 1, p_polarity);        
        writeRegister(INTCONA, vregister);
        
        // Setup IOCONB
        if (!readRegister(INTCONB, &vregister)) {
            return;    
        }
        // Setup INTCON - REGISTER 1-5: INTCON – INTERRUPT-ON-CHANGE CONTROL REGISTER
        vregister = setBit(vregister, 6, p_mirroring);
        vregister = setBit(vregister, 2, p_openDrain);
        vregister = setBit(vregister, 1, p_polarity);        
        writeRegister(INTCONB, vregister);
        
    }
    
    int CMCP23017_I2C::setupInterruptPin(const unsigned char p_gpioId, const InterruptModes p_mode) {
        DEBUG_ENTER("CMCP23017_I2C::setupInterruptPin: %02x, %02x", p_gpioId, p_mode)
        
        // Retrieve the register address
        unsigned char gpioIntconId, gpioDefvalId, gpioGpintenId;
        if (!registerIdFromGpioId(p_gpioId, &gpioIntconId)) {
//            DEBUG_LEAVE("CMCP23017_I2C::setupInterruptPin: -1")
            return -1;
        }
        unsigned char gpioFlags;
        if (gpioIntconId == GPIOA) {
            gpioIntconId = INTCONA;
            gpioDefvalId = DEFVALA; 
            gpioGpintenId = GPINTENA;
            gpioFlags = _gpioAFlags;
        } else {
            gpioIntconId = INTCONB;
            gpioDefvalId = DEFVALB; 
            gpioGpintenId = GPINTENB;
            gpioFlags = _gpioBFlags;
        }
        DEBUG("CMCP23017_I2C::setupInterruptPin: gpioIntconId=%02x gpioDefvalId=%02x gpioGpintenId=%02x gpioFlags=%02x", gpioIntconId, gpioDefvalId, gpioGpintenId, gpioFlags)
        
        // Retrieve the GPIO pin number
        unsigned char gpioBit = gpioBitFromGpioId(p_gpioId);
        DEBUG("CMCP23017_I2C::setupInterruptPin: gpioBit=%02x", gpioBit)
        if (!isBitEqual(gpioFlags, gpioBit, 0x01)) { // Port pin is not configure as input
//            DEBUG_LEAVE("CMCP23017_I2C::setupPullPin: -1")
            return -1;
        }        
        // Read it
        unsigned char gpioIntconValue, gpioDefvalValue, gpioGpintenValue;
        if (!readRegister(gpioIntconId, &gpioIntconValue)) {
//            DEBUG_LEAVE("CMCP23017_I2C::setupInterruptPin: -2")
            return -2;
        }
        if (!readRegister(gpioDefvalId, &gpioDefvalValue)) {
//            DEBUG_LEAVE("CMCP23017_I2C::setupInterruptPin: -2")
            return -2;
        }
        if (!readRegister(gpioGpintenId, &gpioGpintenValue)) {
//            DEBUG_LEAVE("CMCP23017_I2C::setupInterruptPin: -2")
            return -2;
        }
        DEBUG("CMCP23017_I2C::setupInterruptPin: gpioIntconValue=%02x gpioDefvalValue=%02x gpioGpintenValue=%02x", gpioIntconValue, gpioDefvalValue, gpioGpintenValue)
        
        //         
        switch (static_cast<unsigned char>(p_mode)) {
            case static_cast<unsigned char>(OnChange):
                gpioIntconValue = setBit(gpioIntconValue, gpioBit, 0x00);
                gpioDefvalValue = setBit(gpioDefvalValue, gpioBit, 0x00);
                break;
            case static_cast<unsigned char>(OnRising):
                gpioIntconValue = setBit(gpioIntconValue, gpioBit, 0x01);
                gpioDefvalValue = setBit(gpioDefvalValue, gpioBit, 0x00);
                break;
            case static_cast<unsigned char>(OnFalling):
                gpioIntconValue = setBit(gpioIntconValue, gpioBit, 0x01);
                gpioDefvalValue = setBit(gpioDefvalValue, gpioBit, 0x01);
                break;
        } // End of 'switch' statement
        
        // Enable interrupt
        gpioGpintenValue = setBit(gpioGpintenValue, gpioBit, 0x01); 
        
        // Write register
        DEBUG("CMCP23017_I2C::setupInterruptPin: gpioIntconValue=%02x gpioDefvalValue=%02x gpioGpintenValue=%02x", gpioIntconValue, gpioDefvalValue, gpioGpintenValue)
        writeRegister(gpioDefvalId, gpioDefvalValue);
        writeRegister(gpioIntconId, gpioIntconValue);
        writeRegister(gpioGpintenId, gpioGpintenValue);
        
//        DEBUG_LEAVE("CMCP23017_I2C::setupInterruptPin: 0")
        return 0;
    }
    
    int CMCP23017_I2C::setupPullPin(const unsigned char p_gpioId, const PullModes p_mode) {
//        DEBUG_ENTER("CMCP23017_I2C::setupPullPin: %02x, %02x", p_gpioId, p_mode)
        
        // Retrieve the register address
        unsigned char gpioGppuId;
        if (!registerIdFromGpioId(p_gpioId, &gpioGppuId)) {
//            DEBUG_LEAVE("CMCP23017_I2C::setupPullPin: -1")
            return -1;
        }
        unsigned char gpioFlags;
        if (gpioGppuId == GPIOA) {
            gpioGppuId = GPPUA;
            gpioFlags = _gpioAFlags;
        } else {
            gpioGppuId = GPPUB;
            gpioFlags = _gpioBFlags;
        }
//        DEBUG("CMCP23017_I2C::setupPullPin: gpioGppuId=%02x gpioFlags=%02x", gpioGppuId, gpioFlags)
        
        // Retrieve the GPIO pin number
        unsigned char gpioBit = gpioBitFromGpioId(p_gpioId);
//        DEBUG("CMCP23017_I2C::setupPullPin: gpioBit=%02x", gpioBit)
        if (!isBitEqual(gpioFlags, gpioBit, 0x01)) { // Port pin is not configure as input
//            DEBUG_LEAVE("CMCP23017_I2C::setupPullPin: -1")
            return -1;
        }
        
        // Read it
        unsigned char gpioGppuValue;
        if (!readRegister(gpioGppuId, &gpioGppuValue)) {
//            DEBUG_LEAVE("CMCP23017_I2C::setupPullPin: -2")
            return -2;
        }
//        DEBUG("CMCP23017_I2C::setupPullPin: gpioGppuId=%02x", gpioGppuId)
        
        //         
        switch (static_cast<unsigned char>(p_mode)) {
            case static_cast<unsigned char>(AbstractGpioExpender::PullOff):
                gpioGppuValue = setBit(gpioGppuValue, gpioBit, 0x00);
                break;
            case static_cast<unsigned char>(AbstractGpioExpender::PullUp):
                gpioGppuValue = setBit(gpioGppuValue, gpioBit, 0x01);
                break;
            case static_cast<unsigned char>(AbstractGpioExpender::PullDown):
                // Not supporte, nothing to do
                break;
        } // End of 'switch' statement
        
        // Write register
//        DEBUG("CMCP23017_I2C::setupPullPin: gpioGppuValue=%02x", gpioGppuValue)
        writeRegister(gpioGppuId, gpioGppuValue);
        
        
//        DEBUG_LEAVE("CMCP23017_I2C::setupPullPin: 0")
        return 0;
    }
    
    int CMCP23017_I2C::getLastInterruptPinAndValue(unsigned char * p_gpioId, unsigned char * p_value) {
//        DEBUG_ENTER("CMCP23017_I2C::getLastInterruptPinAndValue")
        
        // Read first INTFA if required
        unsigned char vregister;
        if (_gpioAFlags != 0x00) {
            if (!readRegister(INTFA, &vregister)) {
//                DEBUG_LEAVE("CMCP23017_I2C::setupInterruptPin: -1")
                return -1;    
            }
            vregister &= _gpioAFlags; // Cannot have interrupt on output port
            for (unsigned char bit = 0; bit < GPIO_SIZE; bit++) {
                if (isBitSet(vregister, bit)) {
                    *p_gpioId = bit;
                    readRegister(INTCAPA, p_value);
                    *p_value = (*p_value >> bit) & 0x01;
                    
//                    DEBUG_LEAVE("CMCP23017_I2C::getLastInterruptPinAndValue (A): %02x %02x", *p_gpioId, *p_value)
                    return 0;
                }
            } // End of 'for' statement
        }
        
        // If not interrupt on GPIOA, try with GPIOB
        if (_gpioBFlags != 0x00) {
            if (!readRegister(INTFB, &vregister)) {
//                DEBUG_LEAVE("CMCP23017_I2C::setupInterruptPin: -2")
                return -2;    
            }
            vregister &= _gpioBFlags; // Cannot have interrupt on output port
            for (unsigned char bit = 0; bit < GPIO_SIZE; bit++) {
                if (isBitSet(vregister, bit)) {
                    *p_gpioId = bit + GPIO_SIZE;
                    readRegister(INTCAPB, p_value);
                    *p_value = (*p_value >> bit) & 0x01;
                    
//                    DEBUG_LEAVE("CMCP23017_I2C::getLastInterruptPinAndValue (B): %02x %02x", *p_gpioId, *p_value)
                    return 0;
                }
            } // End of 'for' statement
        }
        
//        DEBUG_LEAVE("CMCP23017_I2C::getLastInterruptPinAndValue: 0")
        return 0;
    }
    
    int CMCP23017_I2C::read(const unsigned char p_gpioId, unsigned char * p_value) {
//        DEBUG_ENTER("CMCP23017_I2C::read: 0x%02x", p_gpioId)
        
        // Retrieve the register address
        unsigned char gpioRegisterId;
        if (!registerIdFromGpioId(p_gpioId, &gpioRegisterId)) {
//            DEBUG_LEAVE("CMCP23017_I2C::read: -1")
            return -1;
        }
//        DEBUG("CMCP23017_I2C::read: gpioRegisterId=%02x", gpioRegisterId)
        unsigned char gpioBit = gpioBitFromGpioId(p_gpioId);
//        DEBUG("CMCP23017_I2C::read: gpioBit=%02x", gpioBit)
        
        // Read it
        unsigned char gpioRegisterValue;
        if (!readRegister(gpioRegisterId, &gpioRegisterValue)) {
//            DEBUG_LEAVE("CMCP23017_I2C::read: -2")
            return -2;
        }
//        DEBUG("CMCP23017_I2C::read: gpioRegisterValue=%02x", gpioRegisterValue)
        
        *p_value = (isBitSet(gpioRegisterValue, gpioBit)) ? 0x01 : 0x00;
//        DEBUG("CMCP23017_I2C::read: p_value=%02x", *p_value)
        
//        DEBUG_LEAVE("CMCP23017_I2C::read: 0")
        return 0;
    }
    
    bool CMCP23017_I2C::registerIdFromGpioId(const unsigned char p_gpioId, unsigned char * p_gpioRegisterId) {
//        DEBUG_ENTER("CMCP23017_I2C::registerIdFromGpioId: 0x%02x", p_gpioId)
        
        // Sanity check
        if (p_gpioId > GPIO_MAX) {
//            DEBUG_ENTER("CMCP23017_I2C::registerIdFromGpioId: false")
            return false;
        }
        
        *p_gpioRegisterId = (p_gpioId < GPIO_MED) ? GPIOA : GPIOB;
        
//        DEBUG_ENTER("CMCP23017_I2C::registerIdFromGpioId: true")
        return true;
    }
    
    int CMCP23017_I2C::write(const unsigned char p_gpioId, const unsigned char p_value) {
//        DEBUG_ENTER("CMCP23017_I2C::write: 0x%02x 0x%02x", p_gpioId, p_value)
        
        // Retrieve the register address
        unsigned char gpioRegisterId;
        if (!registerIdFromGpioId(p_gpioId, &gpioRegisterId)) {
//            DEBUG_LEAVE("CMCP23017_I2C::write: -1")
            return -1;
        }
//        DEBUG("CMCP23017_I2C::write: gpioRegisterId=%02x", gpioRegisterId)
        
        // Retrieve the GPIO pin number
        unsigned char gpioBit = gpioBitFromGpioId(p_gpioId);
//        DEBUG("CMCP23017_I2C::write: gpioBit=%02x", gpioBit)
        
        // Read it
        unsigned char gpioRegisterValue;
        if (!readRegister(gpioRegisterId, &gpioRegisterValue)) {
//            DEBUG_LEAVE("CMCP23017_I2C::write: -2")
            return -2;
        }
//        DEBUG("CMCP23017_I2C::write: gpioRegisterValue=%02x", gpioRegisterValue)
        
        // Update GPIO bit
        if (!isBitEqual(gpioRegisterValue, gpioBit, p_value)) {
            // Write it if required
            gpioRegisterValue = setBit(gpioRegisterValue, gpioBit, p_value);
//            DEBUG("CMCP23017_I2C::write: New gpioRegisterValue=%02x", gpioRegisterValue)
            if (!writeRegister(gpioRegisterId, gpioRegisterValue)) {
//                DEBUG_LEAVE("CMCP23017_I2C::write: -3")
                return -3;
            }
        }
        
//        DEBUG_LEAVE("CMCP23017_I2C::write: 0")
        return 0;
    }
    
    unsigned char CMCP23017_I2C::createBus(const std::list<unsigned char> p_lines, const PinMode p_mode) {
        unsigned char busId = _busesIndex++;
        
        _buses.insert(_buses.end(), std::pair<unsigned char, std::list<unsigned char> >(busId, p_lines));
        
        return busId;
    }

    void CMCP23017_I2C::deleteBus(const unsigned char p_busId) {
        // Sanity check
        std::map<unsigned char, std::list<unsigned char> >::iterator result = _buses.find(p_busId);
        if (result == _buses.end()) { // Invalid bus identifier
            return;
        }
        
        _buses.erase(p_busId);
    }

    int CMCP23017_I2C::busRead(const unsigned char p_busId, unsigned short * p_value) {
//        DEBUG_ENTER("CMCP23017_I2C::busRead: 0x%02x", p_busId)

        // Sanity checks
        if (_buses.size() == 0) {
//            DEBUG_LEAVE("CMCP23017_I2C::busRead: -1")
            return -1;
        }
        std::map<unsigned char, std::list<unsigned char> >::iterator result = _buses.find(p_busId);
        if (result == _buses.end()) { // Invalid bus identifier
//            DEBUG_LEAVE("CMCP23017_I2C::busRead: -1")
            return -1;
        }
        
        std::list<unsigned char>::reverse_iterator rit;
        for (std::list<unsigned char>::reverse_iterator rit = result->second.rbegin(); rit != result->second.rend(); ++rit) {
            unsigned char regvalue;
            if (read(*rit, &regvalue) == 0) {
                *p_value = (*p_value | regvalue) << 1;
            } else {
                *p_value <<= 1;
            }
        } // End of 'for' statement
        
//        DEBUG_LEAVE("CMCP23017_I2C::busRead: 0")
        return 0;
    }

    int CMCP23017_I2C::busWrite(const unsigned char p_busId, const unsigned short p_value) {
//        DEBUG_ENTER("CMCP23017_I2C::busWrite: 0x%02x - 0x%02x", p_busId, p_value)

        // Sanity checks
        if (_buses.size() == 0) {
            return -1;
        }
        std::map<unsigned char, std::list<unsigned char> >::iterator result = _buses.find(p_busId);
        if (result == _buses.end()) { // Invalid bus identifier
//            DEBUG_LEAVE("CMCP23017_I2C::busWrite: -1")
            return -1;
        }
        
        std::list<unsigned char>::reverse_iterator rit;
        unsigned short value = p_value;
        for (std::list<unsigned char>::reverse_iterator rit = result->second.rbegin(); rit != result->second.rend(); ++rit) {
            write(*rit, value & 0x01);
            value >>= 1;
        } // End of 'for' statement
        
//        DEBUG_LEAVE("CMCP23017_I2C::busWrite: 0")
        return 0;
    }

    bool CMCP23017_I2C::writeRegister(const unsigned char p_registerId, const unsigned char p_value) {
//        DEBUG_ENTER("CMCP23017_I2C::writeRegister: Memory address: 0x%02x - 0x%02x", p_registerId, p_value)
    
        // 1.Prepare buffer
        char i2cBuffer[2]; // Register address + one byte of data
        // 1.1. Memory address
        i2cBuffer[0] = p_registerId;
        // 1.2. Datas
        i2cBuffer[1] = p_value;
//        DEBUG("CMCP23017_I2C::writeRegister: Data=0x%02x 0x%02x", i2cBuffer[0], i2cBuffer[1])
    
        // 2. Send I2C start + I2C address + Memory Address + Datas + I2C stop
        int result = _i2cInstance->write(_slaveAddress, i2cBuffer, 2);
//        wait(0.02);
    
//        DEBUG_LEAVE("CMCP23017_I2C::writeRegister %x", (bool)(result == 0))
        return (bool)(result == 0);
    }
    
    bool CMCP23017_I2C::readRegister(const unsigned char p_registerId, unsigned char * p_value) {
//        DEBUG_ENTER("CMCP23017_I2C::readRegister: Memory address: 0x%02x", p_registerId)
    
        // 1.Prepare buffer
        char i2cBuffer[1]; // Register address + one byte of data
        // 1.1. Memory address
        i2cBuffer[0] = p_registerId;
//        DEBUG("CMCP23017_I2C::readRegister: Data=0x%02x", i2cBuffer[0])
    
        // 2. Send I2C start + memory address
        if (_i2cInstance->write(_slaveAddress, i2cBuffer, 1, true) == 0) {
//            wait(0.02);
//            DEBUG("CMCP23017_I2C::readRegister: Write memory done")
            // 2. Read data + I2C stop
            int result = _i2cInstance->read(_slaveAddress, (char *)p_value, 1);
//            wait(0.02);
    
//            DEBUG_LEAVE("C24LCXX_I2C::readRegister (byte): %x", (bool)(result == 0))
            return (bool)(result == 0);
        }
            
//        DEBUG_LEAVE("CMCP23017_I2C::readRegister false")
        return false;
    }
    
    void CMCP23017_I2C::DumpRegisters() {
        
        unsigned char value;
        for (unsigned int registerId = 0; registerId < 0x16; registerId++) {
            readRegister(registerId, &value);
//            DEBUG("CMCP23017_I2C::DumpRegisters: register[%d] = 0x%02x", registerId, value)
        }
    }
    
    void CMCP23017_I2C::DumpRegister(unsigned int p_registerId) {
        
        unsigned char value;
        readRegister(p_registerId, &value);
//        DEBUG("CMCP23017_I2C::DumpRegister: register[%d] = 0x%02x", p_registerId, value)
    }
    
} // End of namespace MCP23017_I2C