I2C module reference

Header :
#include <i2c.h>
Makefile :
MODULES+=i2c

Description

I2C is a widely-used serial communication bus used to interface components such as sensors and other microcontrollers. If you are not familiar with I2C, the Communication buses tutorial is a great start.

The SAM4L offers 4 independant I2C ports. The two first can be either master or slave, while the two others can only be master.

Don't forget the pull-up resistors!

Master/Slave

The I2C peripheral can act either as a master or a slave. If you have multiple SAM4 microcontrollers (multiple Carbides for example), you can create a local I2C bus where one MCU acts as the master and the others as slaves (each slave using a unique address) and have them communicate this way, like in a small local network.

Master mode

In master mode, the MCU has control over the bus and decides when to start transfers. A transfer is directed at a specific slave address and can either be a read or write operation.

The master mode is initialized using enableMaster(). Data can then be read from and written to slaves using the various read and write functions. These functions return either a boolean (true if the transfer succeeded) or an integer (indicating the number of bytes transfered) which should be checked for each transfer to make sure everything went as expected. Otherwise, it can indicate that the slave is either busy or missing.

If you want to check that a specific slave is answering on the bus, the function testAddress() is here to help. This can be useful for a self-check of the system, to detect that a device is connected (if the slave is on a separate board that can be unplugged), or to make a slave detector by testing every of the 127 addresses possible.

Be careful when using I2C0 : the pins PA23 and PA24 are also used by the SWD port. To avoid problems, use I2C1 instead.

Slave mode

In slave mode, the MCU cannot decide when transfers will happen and must listen to the bus, waiting for a request.

The slave mode is initialized using enableSlave() while providing the slave address to use. An address is usually written in hexadecimal format (such as 0x4A) and must be between 0x00 and 0x7F (i.e. 0 and 127).

You can wait for a write from the master using the read() function (the master is writing, meaning the slave is reading). You must prepare beforehand a buffer to store the data that is received and tell the function its length, which will be the maximum number of bytes that the peripheral will accept from the master.

Similarly, you can wait for a read from the master using the write() function. In this case, the buffer must contain the data that you want to send. In asynchronous mode (see below), if the buffer is still waiting for transfer but you want to update its content, simply use write() again.

In slave mode, read and write operations can either be synchronous (sync) or asynchronous (async). Synchronous means that the function will be blocking, i.e. that the execution will stop until the operation has completed (the master has read or written the data that you requested). On the other hand, asynchronous means that the function will return immediately and the operation will happen in the background. You can check if the transfer has completed using isAsyncReadFinished() and isAsyncWriteFinished() or set up callback functions to be triggered with enableInterrupt() (this is the prefered way). Overall, synchronous mode is simpler to use at the beginning, but is quickly limiting when your code needs to do something else than simply transmitting data over I2C. Keep in mind that only the master decides when to start transfers, which can take a long time, or even never happen at all.

When using asynchronous operation, it is possible to set up an asynchronous read and an asynchronous write in the background at the same time.

Pull-ups

The pull-up resistors needed for the bus operation are not provided by the peripheral and should be set externally. The resistor value that should be used is relatively precise and depends most notably on the bus speed. At the moment, the driver only supports the standard mode, which means that the resistors should be between 1.6k and 2.9k. 2.2k is a common value.

Examples

API

Master mode

bool enableMaster(Port port)

Enable this port in Master mode.

unsigned int read(Port port, uint8_t address, uint8_t* buffer, int n, bool* acked=nullptr)

Try to read up to n bytes from a slave into buffer. The actual number of bytes read is returned. If the slave is not answering or has no data to offer, this value will be 0. The slave is allowed to send less bytes than requested by interrupting the transfer with a NAK condition, in which case the returned value will be less than n. If the acked boolean pointer is specified, it will be set to true if the slave has ACKed every byte, or to false otherwise.

uint8_t read(Port port, uint8_t address, bool* acked=nullptr)

Helper function to read a single byte from the slave and return it, using the function above. If the slave doesn't answer or has no data to offer, the function returns 0. In order to distinguish between an actual 0 and a NACK condition, the optional acked parameter must be used.

bool write(Port port, uint8_t address, const uint8_t* buffer, int n)

Write n bytes from buffer to the slave. Return true if the transfer completed successfully.

bool write(Port port, uint8_t address, uint8_t byte)

Write a single byte to the slave. Return true if the transfer completed successfully.

unsigned int writeRead(Port port, uint8_t address, const uint8_t* txBuffer, int nTX, uint8_t* rxBuffer, int nRX, bool* acked=nullptr)

Write nTX bytes from txBuffer then immediately read up to nRX bytes to rxBuffer to/from the slave on the same transfer, with a Repeated Start condition in between. This is especially useful for reading registers on devices by writing the register address then reading its value. Some devices require the Repeted Start condition for register access and this function must therefore be used instead of a write() followed by a read(). The number of bytes read from the slave and copied into rxBuffer, always equal to or lower than nRx, is returned. If the acked boolean pointer is specified, it will be set to true if the slave has ACKed the request.

unsigned int writeRead(Port port, uint8_t address, uint8_t byte, uint8_t* rxBuffer, int nRX, bool* acked=nullptr)

Same as above, but write only one byte. This is easier in most cases where the device's registers addresses are coded on a single byte.

bool testAddress(Port port, uint8_t address, Dir direction)

Return true if a slave is answering to the given address on the given direction on the bus, either READ or WRITE. Note that some devices only answer to one of the directions (for example, a simple sensor from which you can only read the measure).

Slave mode

bool enableSlave(Port port, uint8_t address)

Enable this port in Slave mode. Note that only I2C0 and I2C1 support this mode.

int read(Port port, uint8_t* buffer, int n, bool async=false)

Read up to n bytes on the bus by waiting for a write request from the master. The data is stored into buffer. If async is true the function will return immediately, the return value will be 0 and the peripheral will wait for the master request in background. The status of the operation can be checked with isAsyncReadFinished() and getAsyncReadCounter(), and when the transfer is finished the callback specified with enableInterrupt(..., I2C::Interrupt::ASYNC_READ_FINISHED) will be called. If async is false, the function will block until the operation is completed and the return value indicates the number of bytes actually read, which may be lower than or equal to n.

bool write(Port port, const uint8_t* buffer, int n, bool async=false)

Write up to n bytes from buffer on the bus by waiting for a read request from the master. If async is true the function will return immediately, the return value will be true if the transfer was set up successfully and the peripheral will wait for the master request in background. The status of the operation can be checked with isAsyncWriteFinished() and getAsyncWriteCounter(), and when the transfer is finished the callback specified with enableInterrupt(..., I2C::Interrupt::ASYNC_WRITE_FINISHED) will be called. If async is false, the function will block until the operation is completed and the return value will be true if the buffer was sent successfully.

bool isAsyncReadFinished(Port port)

Return true if no async read operation is pending in background.

bool isAsyncWriteFinished(Port port)

Return true if no async write operation is pending in background.

int getAsyncReadCounter(Port port)

Get the number of bytes which have been read in the current async transfer.

int getAsyncReadBytesSent(Port port)

Get the number of bytes which have been sent by the master during a read transfer. This will usually be equal to getAsyncReadCounter(), but may be greater in case the master sent more bytes than the buffer allowed. Comparing the two values is a way to determine if data was lost during the transfer.

int getAsyncWriteCounter(Port port)

Get the number of bytes which have been written in the current async transfer.

Common functions

void disable(Port port)

Disable the port and free its associated ressources.

uint32_t getStatus(Port port)

Advanced function which returns the raw Status Register. See the datasheet §27.9.8 for more details.

void setPin(Port port, PinFunction function, GPIO::Pin pin)

Set the GPIO pin used for this port. PinFunction can be SDA or SCL.

Hacking

There are a few less-important options that are supported by the peripheral but not implemented in the driver, most notably faster modes and 10-bit addressing. Table 27-1 in the datasheet lists those options.

The peripheral also support the SMBus standard, a higher-level protocol based on I2C and commonly found on computer motherboards which adds a few features such as control sums (CRC) for better reliability. It should be relatively easy to develop an SMBus driver based on the I2C driver.

Code

Header

#ifndef _I2C_H_
#define _I2C_H_

#include <stdint.h>
#include "gpio.h"
#include "error.h"

// This module allows the chip to connect to an I2C bus, either in Master or Slave mode.
// I2C is sometimes called TWI (Two-Wire Interface).
// There are 4 distinct I2C controllers : 2 can be either Master or Slave, and 2 can be only Master.
// The peripheral can also operate in SMBus mode (which is a protocol based on I2C), even though this
// is not currently supported by the driver.
// I2C is slower than SPI, but can connect to a larger bus and uses less wires.
namespace I2C {

    // Peripheral memory space base addresses
    extern const uint32_t I2C_BASE[];

    // Register offsets (Master)
    const uint32_t OFFSET_M_CR =      0x00; // Control Register
    const uint32_t OFFSET_M_CWGR =    0x04; // Clock Waveform Generator Register
    const uint32_t OFFSET_M_SMBTR =   0x08; // SMBus Timing Register
    const uint32_t OFFSET_M_CMDR =    0x0C; // Command Register
    const uint32_t OFFSET_M_NCMDR =   0x10; // Next Command Register
    const uint32_t OFFSET_M_RHR =     0x14; // Receive Holding Register
    const uint32_t OFFSET_M_THR =     0x18; // Transmit Holding Register
    const uint32_t OFFSET_M_SR =      0x1C; // Status Register
    const uint32_t OFFSET_M_IER =     0x20; // Interrupt Enable Register
    const uint32_t OFFSET_M_IDR =     0x24; // Interrupt Disable Register
    const uint32_t OFFSET_M_IMR =     0x28; // Interrupt Mask Register
    const uint32_t OFFSET_M_SCR =     0x2C; // Status Clear Register
    const uint32_t OFFSET_M_PR =      0x30; // Parameter Register
    const uint32_t OFFSET_M_HSCWGR =  0x38; // HS-mode Clock Waveform Generator
    const uint32_t OFFSET_M_SRR =     0x3C; // Slew Rate Register
    const uint32_t OFFSET_M_HSSRR =   0x40; // HS-mode Slew Rate Register

    // Register offsets (Slave)
    const uint32_t OFFSET_S_CR =      0x400; // Control Register
    const uint32_t OFFSET_S_NBYTES =  0x404; // NBYTES Register
    const uint32_t OFFSET_S_TR =      0x408; // Timing Register
    const uint32_t OFFSET_S_RHR =     0x40C; // Receive Holding Register
    const uint32_t OFFSET_S_THR =     0x410; // Transmit Holding Register
    const uint32_t OFFSET_S_PECR =    0x414; // Packet Error Check Register
    const uint32_t OFFSET_S_SR =      0x418; // Status Register
    const uint32_t OFFSET_S_IER =     0x41C; // Interrupt Enable Register
    const uint32_t OFFSET_S_IDR =     0x420; // Interrupt Disable Register
    const uint32_t OFFSET_S_IMR =     0x424; // Interrupt Mask Register
    const uint32_t OFFSET_S_SCR =     0x428; // Status Clear Register
    const uint32_t OFFSET_S_PR =      0x42C; // Parameter Register
    const uint32_t OFFSET_S_HSTR =    0x434; // HS-mode Timing Register
    const uint32_t OFFSET_S_SRR =     0x438; // Slew Rate Register
    const uint32_t OFFSET_S_HSSRR =   0x43C; // HS-mode Slew Rate Register
    

    // Subregisters (Master)
    const uint8_t M_CR_MEN = 0;
    const uint8_t M_CR_MDIS = 1;
    const uint8_t M_CR_SMEN = 4;
    const uint8_t M_CR_SMDIS = 5;
    const uint8_t M_CR_SWRST = 7;
    const uint8_t M_CR_STOP = 8;
    const uint8_t M_CWGR_LOW = 0;
    const uint8_t M_CWGR_HIGH = 8;
    const uint8_t M_CWGR_STASTO = 16;
    const uint8_t M_CWGR_DATA = 24;
    const uint8_t M_CWGR_EXP = 28;
    const uint8_t M_CMDR_READ = 0;
    const uint8_t M_CMDR_SADR = 1;
    const uint8_t M_CMDR_TENBIT = 11;
    const uint8_t M_CMDR_REPSAME = 12;
    const uint8_t M_CMDR_START = 13;
    const uint8_t M_CMDR_STOP = 14;
    const uint8_t M_CMDR_VALID = 15;
    const uint8_t M_CMDR_NBYTES = 16;
    const uint8_t M_CMDR_PECEN = 24;
    const uint8_t M_CMDR_ACKLAST = 25;
    const uint8_t M_CMDR_HS = 26;
    const uint8_t M_CMDR_HSMCODE = 28;
    const uint8_t M_SR_RXRDY = 0;
    const uint8_t M_SR_TXRDY = 1;
    const uint8_t M_SR_CRDY = 2;
    const uint8_t M_SR_CCOMP = 3;
    const uint8_t M_SR_IDLE = 4;
    const uint8_t M_SR_BUSFREE = 5;
    const uint8_t M_SR_ANAK = 8;
    const uint8_t M_SR_DNAK = 9;
    const uint8_t M_SR_ARBLST = 10;
    const uint8_t M_SR_TOUT = 12;
    const uint8_t M_SR_PECERR = 13;
    const uint8_t M_SR_STOP = 14;
    const uint8_t M_SR_MENB = 16;
    const uint8_t M_SR_HSMCACK = 17;
    const uint8_t M_SRR_DADRIVEL = 0;
    const uint8_t M_SRR_DASLEW = 8;
    const uint8_t M_SRR_CLDRIVEL = 16;
    const uint8_t M_SRR_CLSLEW = 24;
    const uint8_t M_SRR_FILTER = 28;

    // Subregisters (Slave)
    const uint8_t S_CR_SEN = 0;
    const uint8_t S_CR_SMEN = 1;
    const uint8_t S_CR_SMATCH = 2;
    const uint8_t S_CR_GCMATCH = 3;
    const uint8_t S_CR_STREN = 4;
    const uint8_t S_CR_SWRST = 7;
    const uint8_t S_CR_SMDA = 9;
    const uint8_t S_CR_SMHH = 10;
    const uint8_t S_CR_PECEN = 11;
    const uint8_t S_CR_ACK = 12;
    const uint8_t S_CR_CUP = 13;
    const uint8_t S_CR_SOAM = 14;
    const uint8_t S_CR_SODR = 15;
    const uint8_t S_CR_ADR = 16;
    const uint8_t S_CR_TENBIT = 26;
    const uint8_t S_TR_TLOWS = 0;
    const uint8_t S_TR_TTOUT = 8;
    const uint8_t S_TR_SUDAT = 16;
    const uint8_t S_TR_EXP = 28;
    const uint8_t S_SR_RXRDY = 0;
    const uint8_t S_SR_TXRDY = 1;
    const uint8_t S_SR_SEN = 2;
    const uint8_t S_SR_TCOMP = 3;
    const uint8_t S_SR_TRA = 5;
    const uint8_t S_SR_URUN = 6;
    const uint8_t S_SR_ORUN = 7;
    const uint8_t S_SR_NAK = 8;
    const uint8_t S_SR_SMBTOUT = 12;
    const uint8_t S_SR_SMBPECERR = 13;
    const uint8_t S_SR_BUSERR = 14;
    const uint8_t S_SR_SAM = 16;
    const uint8_t S_SR_GCM = 17;
    const uint8_t S_SR_SMBHHM = 19;
    const uint8_t S_SR_SMBDAM = 20;
    const uint8_t S_SR_STO = 21;
    const uint8_t S_SR_REP = 22;
    const uint8_t S_SR_BTF = 23;
    const uint8_t S_SRR_DADRIVEL = 0;
    const uint8_t S_SRR_DASLEW = 8;
    const uint8_t S_SRR_FILTER = 28;
    
    // Ports
    const int N_PORTS_M = 4;
    const int N_PORTS_S = 2;
    enum class Port {
        I2C0,
        I2C1,
        I2C2,
        I2C3
    };

    enum Dir {
        READ,
        WRITE
    };

    enum class PinFunction {
        SDA,
        SCL
    };

    // Timeout for transfer operations
    const int TIMEOUT = 1000; // ms

    // Interrupts
    const int N_INTERRUPTS = 2;
    enum class Interrupt {
        ASYNC_READ_FINISHED,
        ASYNC_WRITE_FINISHED,
    };

    // Error codes
    const Error::Code WARN_PORT_ALREADY_INITIALIZED = 1;
    const Error::Code WARN_ARBITRATION_LOST = 2;
    const Error::Code ERR_PORT_NOT_INITIALIZED = 3;
    const Error::Code ERR_TIMEOUT = 4;


    // Common functions
    void disable(Port port);
    uint32_t getStatus(Port port);
    void setPin(Port port, PinFunction function, GPIO::Pin pin);

    // Master-mode functions
    bool enableMaster(Port port, unsigned int frequency=100000);
    unsigned int read(Port port, uint8_t address, uint8_t* buffer, int n, bool* acked=nullptr);
    uint8_t read(Port port, uint8_t address, bool* acked=nullptr);
    bool write(Port port, uint8_t address, const uint8_t* buffer, int n);
    bool write(Port port, uint8_t address, uint8_t byte);
    unsigned int writeRead(Port port, uint8_t address, const uint8_t* txBuffer, int nTX, uint8_t* rxBuffer, int nRX, bool* acked=nullptr);
    unsigned int writeRead(Port port, uint8_t address, uint8_t byte, uint8_t* rxBuffer, int nRX, bool* acked=nullptr);
    bool testAddress(Port port, uint8_t address, Dir direction);

    // Slave-mode functions
    bool enableSlave(Port port, uint8_t address);
    int read(Port port, uint8_t* buffer, int n, bool async=false);
    bool write(Port port, const uint8_t* buffer, int n, bool async=false);
    bool isAsyncReadFinished(Port port);
    bool isAsyncWriteFinished(Port port);
    int getAsyncReadCounter(Port port);
    int getAsyncReadBytesSent(Port port);
    int getAsyncWriteCounter(Port port);
    void enableInterrupt(Port port, void (*handler)(), Interrupt interrupt);

}


#endif

Module

#include "i2c.h"
#include "core.h"
#include "pm.h"
#include "dma.h"
#include "error.h"

namespace I2C {

    // Channel parameters
    const int BUFFER_SIZE = 64;
    enum class Mode {
        NONE,
        MASTER,
        SLAVE
    };
    struct Channel {
        Mode mode = Mode::NONE;
        uint8_t buffer[BUFFER_SIZE];
        int rxDMAChannel = -1;
        int txDMAChannel = -1;
        unsigned int nBytesToRead = 0;
        unsigned int nBytesToWrite = 0;
    };

    // List of available ports
    struct Channel _ports[N_PORTS_M];

    // Interrupt handlers
    extern uint8_t INTERRUPT_PRIORITY;
    uint32_t _interruptHandlers[N_PORTS_M][N_INTERRUPTS];
    Core::Interrupt _interruptChannelsMaster[] = {Core::Interrupt::TWIM0, Core::Interrupt::TWIM1, Core::Interrupt::TWIM2, Core::Interrupt::TWIM3};
    Core::Interrupt _interruptChannelsSlave[] = {Core::Interrupt::TWIS0, Core::Interrupt::TWIS1};
    void interruptHandlerWrapper();

    // Clocks
    const int PM_CLK_M[] = {PM::CLK_I2CM0, PM::CLK_I2CM1, PM::CLK_I2CM2, PM::CLK_I2CM3}; // Master mode
    const int PM_CLK_S[] = {PM::CLK_I2CS0, PM::CLK_I2CS1}; // Slave mode

    // Package-dependant, defined in pins_sam4l_XX.cpp
    extern struct GPIO::Pin PINS_SDA[];
    extern struct GPIO::Pin PINS_SCL[];

    // Registers base address
    const uint32_t I2C_BASE[] = {0x40018000, 0x4001C000, 0x40078000, 0x4007C000};

    // I2C clock frequency
    unsigned int _frequency = 0;



    // MASTER MODE

    bool enableMaster(Port port, unsigned int frequency) {
        if (static_cast<int>(port) > N_PORTS_M) {
            return false;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        struct Channel* p = &(_ports[static_cast<int>(port)]);

        // If this port is already enabled in slave mode, disable it
        if (p->mode == Mode::SLAVE) {
            Error::happened(Error::Module::I2C, WARN_PORT_ALREADY_INITIALIZED, Error::Severity::WARNING);
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_CR)) = 0;
        }
        p->mode = Mode::MASTER;

        // Initialize the buffer
        for (int i = 0; i < BUFFER_SIZE; i++) {
            p->buffer[i] = 0;
        }

        // Initialize interrupt handlers
        for (int i = 0; i < N_INTERRUPTS; i++) {
            _interruptHandlers[static_cast<int>(port)][i] = (uint32_t)nullptr;
        }

        // Enable the clock
        PM::enablePeripheralClock(PM_CLK_M[static_cast<int>(port)]);

        // CR (Control Register) : enable the master interface
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_MEN;          // MEN : Master Enable

        // CR (Control Register) : reset the interface
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_SWRST;        // SWRST : software reset

        // CWGR (Clock Waveform Generator Register) : setup the SCL (clock) line
        _frequency = frequency;
        unsigned long fPrescaler = PM::getModuleClockFrequency(PM_CLK_M[static_cast<int>(port)]) / 2; // Prescaler at EXP=0 divides the freq. by 2
        unsigned int periods = fPrescaler / frequency;
        uint8_t t = periods / 2;
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CWGR))
            = t << M_CWGR_LOW      // LOW : low and buffer counter
            | t << M_CWGR_HIGH     // HIGH : high counter
            | t << M_CWGR_STASTO   // STASTO : start/stop counters
            | 1 << M_CWGR_DATA     // DATA : data time counter
            | 0 << M_CWGR_EXP;     // EXP : clock prescaler

        // SRR (Slew Rate Register) : setup the lines
        // See Electrical Characteristics in the datasheet for more details
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SRR))
            = 3 << M_SRR_DADRIVEL
            | 0 << M_SRR_DASLEW
            | 3 << M_SRR_CLDRIVEL
            | 0 << M_SRR_CLSLEW
            | 2 << M_SRR_FILTER;

        // Set up the DMA channels and related interrupts
        p->rxDMAChannel = DMA::setupChannel(p->rxDMAChannel, static_cast<DMA::Device>(static_cast<int>(DMA::Device::I2C0_M_RX) + static_cast<int>(port)), DMA::Size::BYTE);
        p->txDMAChannel = DMA::setupChannel(p->txDMAChannel, static_cast<DMA::Device>(static_cast<int>(DMA::Device::I2C0_M_TX) + static_cast<int>(port)), DMA::Size::BYTE);

        // Set the pins in peripheral mode
        GPIO::enablePeripheral(PINS_SDA[static_cast<int>(port)]);
        GPIO::enablePeripheral(PINS_SCL[static_cast<int>(port)]);

        return true;
    }

    // Internal function which checks if the controller has lost the bus arbitration
    // to another master. If no other master is present and this condition arises, 
    // this may be the sign of an electrical problem (short circuit or missing pull-ups).
    bool checkArbitrationLost(Port port) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::MASTER) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return false;
        }

        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        if ((*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_ARBLST)) {
            Error::happened(Error::Module::I2C, WARN_ARBITRATION_LOST, Error::Severity::WARNING);
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CMDR)) = 0;
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SCR)) = 1 << M_SR_ARBLST;
            return true;
        }
        return false;
    }

    // Try to send a read request to the specified address and return true if
    // a slave device has answered
    bool testAddress(Port port, uint8_t address, Dir direction) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::MASTER) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return false;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        if (checkArbitrationLost(port)) {
            return false;
        }

        // CR (Control Register) : reset the interface in case a failed previous
        // transfer is still pending
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_SWRST;
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_MEN;

        // CMDR (Command Register) : initiate a transfer
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CMDR))
            = !static_cast<int>(direction) << M_CMDR_READ
            | address << M_CMDR_SADR
            | 1 << M_CMDR_START
            | 1 << M_CMDR_STOP
            | 1 << M_CMDR_VALID
            | 0 << M_CMDR_NBYTES;

        // Wait for the transfer to complete or an Arbitration lost condition to happen
        Core::Time t0 = Core::time();
        while (!( (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_IDLE)
                || (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_ARBLST) )) {
            if (Core::time() > t0 + TIMEOUT) {
                Error::happened(Error::Module::I2C, ERR_TIMEOUT, Error::Severity::CRITICAL);
            }
            Core::sleep(1);
        }

        // Check for arbitration lost again now that the transfer is complete
        if (checkArbitrationLost(port)) {
            return false;
        }

        // If the ANAK status flag is set, no slave has answered
        if ((*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_ANAK)) {

            // Clear the ANAK status flag and the command register
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CMDR)) = 0;
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SCR)) = 1 << M_SR_ANAK;

            return false;
        }
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SCR)) = 1 << M_SR_CCOMP;
        return true;
    }

    // Master read
    unsigned int read(Port port, uint8_t address, uint8_t* buffer, int n, bool* acked) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::MASTER) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return 0;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        if (checkArbitrationLost(port)) {
            return 0;
        }

        // CR (Control Register) : reset the interface in case a failed previous
        // transfer is still pending
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_SWRST;
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_MEN;

        // Clear every status
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SCR)) = 0xFFFFFFFF;

        // Start the DMA RX channel
        DMA::startChannel(p->rxDMAChannel, (uint32_t)(buffer), n);

        // CMDR (Command Register) : initiate a read transfer
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CMDR))
            = 1 << M_CMDR_READ
            | address << M_CMDR_SADR
            | 1 << M_CMDR_START
            | 1 << M_CMDR_STOP
            | 1 << M_CMDR_VALID
            | n << M_CMDR_NBYTES;

        // Wait for the transfer to be finished
        Core::Time t0 = Core::time();
        while (!DMA::isFinished(p->rxDMAChannel)
                && !((*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_ANAK | 1 << M_SR_DNAK | 1 << M_SR_ARBLST))) {
            if (Core::time() > t0 + TIMEOUT) {
                Error::happened(Error::Module::I2C, ERR_TIMEOUT, Error::Severity::CRITICAL);
            }
            Core::sleep(1);
        }

        // Check for arbitration lost again now that the transfer is complete
        if (checkArbitrationLost(port)) {
            return false;
        }

        // If the slave has not responded, cancel the read
        if ((*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & 1 << M_SR_ANAK) {
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CMDR)) = 0;
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SCR)) = 1 << M_SR_ANAK;
            if (acked != nullptr) {
                *acked = false;
            }
            return 0;
        } else {
            if (acked != nullptr) {
                *acked = true;
            }
        }

        return n - DMA::getCounter(p->rxDMAChannel);
    }

    // Helper function to read a single byte
    uint8_t read(Port port, uint8_t address, bool* acked) {
        uint8_t buffer[] = {0x00};
        read(port, address, buffer, 1, acked);
        return buffer[0];
    }

    // Master write
    bool write(Port port, uint8_t address, const uint8_t* buffer, int n) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::MASTER) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return false;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        if (checkArbitrationLost(port)) {
            return false;
        }

        // CR (Control Register) : reset the interface in case a failed previous
        // transfer is still pending
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_SWRST;
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_MEN;

        // Write at most BUFFER_SIZE characters
        if (n > BUFFER_SIZE) {
            n = BUFFER_SIZE;
        }

        // Copy the user buffer into the port buffer
        for (int i = 0; i < n; i++) {
            p->buffer[i] = buffer[i];
        }

        // Clear every status
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SCR)) = 0xFFFFFFFF;

        // Copy the first byte to transmit
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_THR)) = p->buffer[0];

        // Start the DMA
        if (n >= 2) {
            DMA::startChannel(p->txDMAChannel, (uint32_t)(p->buffer + 1), n - 1);
        }

        // CMDR (Command Register) : configure the command to send
        uint32_t cmdr
            = 0 << M_CMDR_READ
            | address << M_CMDR_SADR
            | 1 << M_CMDR_START
            | 1 << M_CMDR_STOP
            | n << M_CMDR_NBYTES;
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CMDR)) = cmdr;

        // CMDR (Command Register) : execute the command
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CMDR))
            = cmdr
            | 1 << M_CMDR_VALID;

        // Wait for the transfer to be finished
        if (n >= 2) {
            Core::Time t0 = Core::time();
            while (!(DMA::isFinished(p->txDMAChannel) && (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_BUSFREE))
                    && !((*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_ANAK | 1 << M_SR_DNAK | 1 << M_SR_ARBLST))) {
                if (Core::time() > t0 + TIMEOUT) {
                    Error::happened(Error::Module::I2C, ERR_TIMEOUT, Error::Severity::CRITICAL);
                }
                Core::sleep(1);
            }

        } else {
            Core::Time t0 = Core::time();
            while (!((*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_IDLE))) {
                if (Core::time() > t0 + TIMEOUT) {
                    Error::happened(Error::Module::I2C, ERR_TIMEOUT, Error::Severity::CRITICAL);
                }
                Core::sleep(1);
            }
        }

        // Check for arbitration lost again now that the transfer is complete
        if (checkArbitrationLost(port)) {
            return false;
        }

        // Return true if the transfer was completed successfully
        return !((*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_ANAK | 1 << M_SR_DNAK));
    }

    // Helper function to write a single byte
    bool write(Port port, uint8_t address, uint8_t byte) {
        return write(port, address, &byte, 1);
    }

    // Write then immediately read on the bus on the same transfer, with a Repeated Start condition.
    // This is especially useful for reading registers on devices by writing the register address
    // then reading the value
    unsigned int writeRead(Port port, uint8_t address, const uint8_t* txBuffer, int nTX, uint8_t* rxBuffer, int nRX, bool* acked) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::MASTER) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return false;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        if (checkArbitrationLost(port)) {
            return false;
        }

        // CR (Control Register) : reset the interface in case a failed previous
        // transfer is still pending
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_SWRST;
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
            = 1 << M_CR_MEN;

        // Write at most BUFFER_SIZE characters
        if (nTX >  BUFFER_SIZE) {
            nTX = BUFFER_SIZE;
        }

        // Copy the user TX buffer into the port buffer
        for (int i = 0; i < nTX; i++) {
            p->buffer[i] = txBuffer[i];
        }

        // Clear every status
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SCR)) = 0xFFFFFFFF;

        // Copy the first byte to transmit
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_THR)) = p->buffer[0];

        // Start the DMA TX channel
        if (nTX >= 2) {
            DMA::startChannel(p->txDMAChannel, (uint32_t)(p->buffer + 1), nTX - 1);
        }

        // Start the DMA RX channel
        DMA::startChannel(p->rxDMAChannel, (uint32_t)(rxBuffer), nRX);

        // CMDR (Command Register) : initiate a write transfer without STOP
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CMDR))
            = 0 << M_CMDR_READ
            | address << M_CMDR_SADR
            | 1 << M_CMDR_START
            | 0 << M_CMDR_STOP
            | 1 << M_CMDR_VALID
            | nTX << M_CMDR_NBYTES;

        // NCMDR (Next Command Register) : initiate a read transfer to follow
        (*(volatile uint32_t*)(REG_BASE + OFFSET_M_NCMDR))
            = 1 << M_CMDR_READ
            | address << M_CMDR_SADR
            | 1 << M_CMDR_START
            | 1 << M_CMDR_STOP
            | 1 << M_CMDR_VALID
            | nRX << M_CMDR_NBYTES;

        // Wait for the transfer to be finished
        Core::Time t0 = Core::time();
        while (!DMA::isFinished(p->rxDMAChannel)
                && !((*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & (1 << M_SR_ANAK | 1 << M_SR_DNAK | 1 << M_SR_ARBLST))) {
            if (Core::time() > t0 + TIMEOUT) {
                Error::happened(Error::Module::I2C, ERR_TIMEOUT, Error::Severity::CRITICAL);
            }
            Core::sleep(1);
        }

        // Check for arbitration lost again now that the transfer is complete
        if (checkArbitrationLost(port)) {
            return false;
        }

        // If the slave has not responded, cancel the read
        if ((*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR)) & 1 << M_SR_ANAK) {
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CMDR)) = 0;
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SCR)) = 1 << M_SR_ANAK;
            if (acked != nullptr) {
                *acked = false;
            }
            return 0;
        } else {
            if (acked != nullptr) {
                *acked = true;
            }
        }

        return nRX - DMA::getCounter(p->rxDMAChannel);
    }

    // Helper function which writes a single byte then reads the result
    unsigned int writeRead(Port port, uint8_t address, uint8_t byte, uint8_t* rxBuffer, int nRX, bool* acked) {
        return writeRead(port, address, &byte, 1, rxBuffer, nRX, acked);
    }







    // SLAVE MODE

    bool enableSlave(Port port, uint8_t address) {
        if (static_cast<int>(port) > N_PORTS_S) {
            return false;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        struct Channel* p = &(_ports[static_cast<int>(port)]);

        // If this port is already enabled in master mode, disable it
        if (p->mode == Mode::MASTER) {
            Error::happened(Error::Module::I2C, WARN_PORT_ALREADY_INITIALIZED, Error::Severity::WARNING);
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR))
                = 1 << M_CR_MDIS
                | 1 << M_CR_STOP;
        }
        p->mode = Mode::SLAVE;

        // Initialize the buffer
        for (int i = 0; i < BUFFER_SIZE; i++) {
            p->buffer[i] = 0;
        }

        // Initialize interrupt handlers
        for (int i = 0; i < N_INTERRUPTS; i++) {
            _interruptHandlers[static_cast<int>(port)][i] = (uint32_t)nullptr;
        }

        // Enable the clock
        PM::enablePeripheralClock(PM_CLK_S[static_cast<int>(port)]);

        // CR (Control Register) : reset the interface
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_CR))
            = 1 << S_CR_SWRST;        // SWRST : software reset

        // CR (Control Register) : enable the slave interface
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_CR))
            = 1 << S_CR_SEN;          // SEN : Slave Enable

        // CR (Control Register) : configure the interface
        address &= 0x7F;
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_CR))
            = 1 << S_CR_SEN           // SEN : Slave Enable
            | 1 << S_CR_SMATCH        // SMATCH : Acknowledge the slave address
            | 0 << S_CR_STREN         // STREN : Clock stretch disabled
            | 1 << S_CR_CUP           // CUP : NBYTES Count Up
            | address << S_CR_ADR;    // ADDR : Slave Address

        // TR (Timing Register) : setup bus timings
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_TR))
            = 1 << S_TR_SUDAT;        // SUDAT : Data setup cycles

        // NBYTES : reset the bytes counter
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_NBYTES)) = 0;

        // Set up the DMA channels
        p->rxDMAChannel = DMA::setupChannel(p->rxDMAChannel, static_cast<DMA::Device>(static_cast<int>(DMA::Device::I2C0_S_RX) + static_cast<int>(port)), DMA::Size::BYTE);
        p->txDMAChannel = DMA::setupChannel(p->txDMAChannel, static_cast<DMA::Device>(static_cast<int>(DMA::Device::I2C0_S_TX) + static_cast<int>(port)), DMA::Size::BYTE);

        // SRR (Slew Rate Register) : setup the lines
        // See Electrical Characteristics in the datasheet for more details
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_SRR))
            = 3 << S_SRR_DADRIVEL
            | 0 << S_SRR_DASLEW
            | 2 << S_SRR_FILTER;

        // IER (Interrupt Enable Register) : enable the Transfer Complete, Overrun and Underrun interrupts
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_IER))
                = 1 << S_SR_TCOMP
                | 1 << S_SR_URUN
                | 1 << S_SR_ORUN;

        // Enable the interrupt in the NVIC
        Core::Interrupt interruptChannel = _interruptChannelsSlave[static_cast<int>(port)];
        Core::setInterruptHandler(interruptChannel, interruptHandlerWrapper);
        Core::enableInterrupt(interruptChannel, INTERRUPT_PRIORITY);

        // Initialize the slave with an empty write
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_THR)) = 0xFF;

        // Set the pins in peripheral mode
        GPIO::enablePeripheral(PINS_SDA[static_cast<int>(port)]);
        GPIO::enablePeripheral(PINS_SCL[static_cast<int>(port)]);

        return true;
    }

    // Slave read
    int read(Port port, uint8_t* buffer, int n, bool async) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::SLAVE) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return 0;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];

        // Clear every status
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_SCR)) = 0xFFFFFFFF;

        // Dummy read to clear RHR in case of overrun
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_RHR));

        // NBYTES : reset the bytes counter
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_NBYTES)) = 0;

        // Save the number of bytes to read
        p->nBytesToRead = n;

        // Start the DMA RX channel
        DMA::startChannel(p->rxDMAChannel, (uint32_t)(buffer), n);

        if (async) {
            // In async mode, do not wait for the read to complete, it will be managed in background
            // by the DMA
            return 0;

        } else {
            // Wait for the transfer to be finished
            Core::Time t0 = Core::time();
            while (!((*(volatile uint32_t*)(REG_BASE + OFFSET_S_SR)) & (1 << S_SR_TCOMP | 1 << S_SR_BUSERR | 1 << S_SR_NAK))) {
                if (Core::time() > t0 + TIMEOUT) {
                    Error::happened(Error::Module::I2C, ERR_TIMEOUT, Error::Severity::CRITICAL);
                }
                Core::sleep(1);
            }

            return n - DMA::getCounter(p->rxDMAChannel);
        }
    }

    // Slave write
    bool write(Port port, const uint8_t* buffer, int n, bool async) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::SLAVE) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return false;
        }
        bool ret = true;
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];

        // Stop any previous transfer
        DMA::stopChannel(p->txDMAChannel);

        // NBYTES : reset the bytes counter
        (*(volatile uint32_t*)(REG_BASE + OFFSET_S_NBYTES)) = 0;

        // Empty transfer
        if (n == 0 || buffer == nullptr) {
            // Copy a dummy byte to THR to overwrite any previous byte waiting to be sent
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_THR)) = 0xFF;

            // Clear every status
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_SCR)) = 0xFFFFFFFF;

        } else {
            // Write at most BUFFER_SIZE - 1 + 1 = BUFFER_SIZE characters into the port buffer : 
            // - 1 to allow room for the 0xFF terminating byte (see below)
            // + 1 because the first byte in the user buffer is written directly to THR (see below)
            if (n >= BUFFER_SIZE) {
                n = BUFFER_SIZE;
                ret = false;
            }

            // Copy the first byte to THR to overwrite any previous byte waiting to be sent
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_THR)) = buffer[0];

            // Copy the rest of the user buffer into the port buffer
            for (int i = 0; i < n - 1; i++) {
                p->buffer[i] = buffer[i + 1];
            }

            // Save the number of bytes to write
            p->nBytesToWrite = n - 1;

            // Write a 0xFF at the end of the buffer
            // Since the last byte is repeated indefinitely when the master attempts to read
            // more bytes than were available, this forces the hardware to send only 0xFF bytes.
            p->buffer[n - 1] = 0xFF;
            n++;

            // Clear every status
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_SCR)) = 0xFFFFFFFF;

            // Start the DMA
            DMA::startChannel(p->txDMAChannel, (uint32_t)(p->buffer), n - 1);
        }

        if (async) {
            // In async mode, do not wait for the read to complete, it will be managed in background
            // by the DMA
            return ret;

        } else {
            // Wait for the transfer to be finished
            Core::Time t0 = Core::time();
            while (!((*(volatile uint32_t*)(REG_BASE + OFFSET_S_SR)) & (1 << S_SR_TCOMP | 1 << S_SR_BUSERR | 1 << S_SR_NAK))) {
                if (Core::time() > t0 + TIMEOUT) {
                    Error::happened(Error::Module::I2C, ERR_TIMEOUT, Error::Severity::CRITICAL);
                }
                Core::sleep(1);
            }

            return DMA::isFinished(p->txDMAChannel);
        }
    }

    bool isAsyncReadFinished(Port port) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::SLAVE) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return false;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        uint32_t status = *(volatile uint32_t*)(REG_BASE + OFFSET_S_SR);
        return !(status & 1 << S_SR_TRA) && (status & (1 << S_SR_TCOMP | 1 << S_SR_BUSERR | 1 << S_SR_NAK));
    }

    bool isAsyncWriteFinished(Port port) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::SLAVE) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return false;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        uint32_t status = *(volatile uint32_t*)(REG_BASE + OFFSET_S_SR);
        return (status & 1 << S_SR_TRA) && (status & (1 << S_SR_TCOMP | 1 << S_SR_BUSERR | 1 << S_SR_NAK));
    }

    // Get the number of bytes which have been read in the current async transfer
    int getAsyncReadCounter(Port port) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::SLAVE) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return 0;
        }
        if (p->nBytesToRead > 0) {
            return p->nBytesToRead - DMA::getCounter(p->rxDMAChannel);
        }
        return 0;
    }

    // Get the number of bytes which have been sent by the master during a read transfer.
    // This will usually be equal to getAsyncReadCounter(), but may be greater in case the
    // master sent more bytes than the buffer allowed. Comparing the two values allows
    // to determine if data was lost during the transfer.
    int getAsyncReadCounterSent(Port port) {
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        return *(volatile uint32_t*)(REG_BASE + OFFSET_S_NBYTES) & 0xFF;
    }

    // Get the number of bytes which have been written in the current async transfer
    int getAsyncWriteCounter(Port port) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::SLAVE) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return 0;
        }
        if (p->nBytesToWrite > 0) {
            return p->nBytesToWrite - DMA::getCounter(p->txDMAChannel);
        }
        return 0;
    }

    void enableInterrupt(Port port, void (*handler)(), Interrupt interrupt) {
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode != Mode::SLAVE) {
            Error::happened(Error::Module::I2C, ERR_PORT_NOT_INITIALIZED, Error::Severity::CRITICAL);
            return;
        }

        // Save the user handler
        _interruptHandlers[static_cast<int>(port)][static_cast<int>(interrupt)] = (uint32_t)handler;
    }

    void interruptHandlerWrapper() {
        // Get the port through the current interrupt number
        Port port;
        Core::Interrupt currentInterrupt = Core::currentInterrupt();
        if (currentInterrupt == Core::Interrupt::TWIS0) {
            port = Port::I2C0;
        } else if (currentInterrupt == Core::Interrupt::TWIS1) {
            port = Port::I2C1;
        } else {
            return;
        }
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];

        // URUN : underrun
        if ((*(volatile uint32_t*)(REG_BASE + OFFSET_S_SR)) & (1 << S_SR_URUN)) {
            // Write a dummy byte in THR to make sure the peripheral will not block
            // the bus waiting for data to transmit
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_THR)) = 0xFB;

            // SCR (Status Clear Register) : clear the interrupt
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_SCR))
                = 1 << S_SR_URUN;
        }

        // ORUN : overrun
        if ((*(volatile uint32_t*)(REG_BASE + OFFSET_S_SR)) & (1 << S_SR_ORUN)) {
            // Dummy read from RHR to make sure the peripheral will not block
            // the bus waiting for the received data to be handled
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_RHR));

            // SCR (Status Clear Register) : clear the interrupt
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_SCR))
                = 1 << S_SR_ORUN;
        }

        // TCOMP : Transfer Complete
        if ((*(volatile uint32_t*)(REG_BASE + OFFSET_S_SR)) & (1 << S_SR_TCOMP)) {
            // Call the user handler corresponding to this interrupt
            void (*handler)() = nullptr;
            if ((*(volatile uint32_t*)(REG_BASE + OFFSET_S_SR)) & (1 << S_SR_TRA)) {
                // Write finished
                handler = (void (*)())_interruptHandlers[static_cast<int>(port)][static_cast<int>(Interrupt::ASYNC_WRITE_FINISHED)];
                if (handler != nullptr) {
                    handler();
                }

            } else {
                // Read finished
                handler = (void (*)())_interruptHandlers[static_cast<int>(port)][static_cast<int>(Interrupt::ASYNC_READ_FINISHED)];
                if (handler != nullptr) {
                    handler();
                }
            }
            // SCR (Status Clear Register) : clear the interrupt
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_SCR))
                = 1 << S_SR_TCOMP;
        }
    }


    // COMMON TO MASTER AND SLAVE MODE

    // Disable the port and release its ressources
    void disable(Port port) {
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        
        // Free the pins in peripheral mode
        GPIO::disablePeripheral(PINS_SDA[static_cast<int>(port)]);
        GPIO::disablePeripheral(PINS_SCL[static_cast<int>(port)]);

        // Stop the DMA channels
        if (p->txDMAChannel > -1) {
            DMA::stopChannel(p->txDMAChannel);
        }
        if (p->rxDMAChannel > -1) {
            DMA::stopChannel(p->rxDMAChannel);
        }

        if (p->mode == Mode::MASTER) {
            // Disable the interrupt in the NVIC
            Core::Interrupt interruptChannel = _interruptChannelsMaster[static_cast<int>(port)];
            Core::disableInterrupt(interruptChannel);

            // CR (Control Register) : disable the master interface
            (*(volatile uint32_t*)(REG_BASE + OFFSET_M_CR)) = 0;

            // Disable the clock
            PM::disablePeripheralClock(PM_CLK_M[static_cast<int>(port)]);

        } else if (p->mode == Mode::SLAVE) {
            // Disable the interrupt in the NVIC
            Core::Interrupt interruptChannel = _interruptChannelsSlave[static_cast<int>(port)];
            Core::disableInterrupt(interruptChannel);

            // CR (Control Register) : disable the slave interface
            (*(volatile uint32_t*)(REG_BASE + OFFSET_S_CR)) = 0;

            // Disable the clock
            PM::disablePeripheralClock(PM_CLK_S[static_cast<int>(port)]);
        }

        p->mode = Mode::NONE;
    }

    // Advanced function which returns the raw Status Register.
    // See the datasheet §27.9.8 for more details.
    uint32_t getStatus(Port port) {
        const uint32_t REG_BASE = I2C_BASE[static_cast<int>(port)];
        struct Channel* p = &(_ports[static_cast<int>(port)]);
        if (p->mode == Mode::MASTER) {
            return (*(volatile uint32_t*)(REG_BASE + OFFSET_M_SR));
        } else if (p->mode == Mode::SLAVE) {
            return (*(volatile uint32_t*)(REG_BASE + OFFSET_S_SR));
        }
        return 0;
    }

    void setPin(Port port, PinFunction function, GPIO::Pin pin) {
        switch (function) {
            case PinFunction::SDA:
                PINS_SDA[static_cast<int>(port)] = pin;
                break;

            case PinFunction::SCL:
                PINS_SCL[static_cast<int>(port)] = pin;
                break;
        }
    }

}