feat: implement handler for distance and led

RP2040_I2C_Registers.py

@@ -0,0 +1,302 @@
+I2C_OFFSET = {
+    "I2C_IC_CON":            0x00000000,
+    "I2C_IC_TAR":            0x00000004,
+    "I2C_IC_SAR":            0x00000008,
+    "I2C_IC_DATA_CMD":       0x00000010,
+    "I2C_IC_SS_SCL_HCNT":    0x00000014,
+    "I2C_IC_SS_SCL_LCNT":    0x00000018,
+    "I2C_IC_FS_SCL_HCNT":    0x0000001c,
+    "I2C_IC_FS_SCL_LCNT":    0x00000020,
+    "I2C_IC_INTR_STAT":      0x0000002c,
+    "I2C_IC_INTR_MASK":      0x00000030,
+    "I2C_IC_RAW_INTR_STAT":  0x00000034,
+    "I2C_IC_RX_TL":          0x00000038,
+    "I2C_IC_TX_TL":          0x0000003c,
+    "I2C_IC_CLR_INTR":       0x00000040,
+    "I2C_IC_CLR_RX_UNDER":   0x00000044,
+    "I2C_IC_CLR_RX_OVER":    0x00000048,
+    "I2C_IC_CLR_TX_OVER":    0x0000004c,
+    "I2C_IC_CLR_RD_REQ":     0x00000050,
+    "I2C_IC_CLR_TX_ABRT":    0x00000054,
+    "I2C_IC_CLR_RX_DONE":    0x00000058,
+    "I2C_IC_CLR_ACTIVITY":   0x0000005c,
+    "I2C_IC_CLR_STOP_DET":   0x00000060,
+    "I2C_IC_CLR_START_DET":  0x00000064,
+    "I2C_IC_CLR_GEN_CALL":   0x00000068,
+    "I2C_IC_ENABLE":         0x0000006c,
+    "I2C_IC_STATUS":         0x00000070,
+    "I2C_IC_TXFLR":          0x00000074,
+    "I2C_IC_RXFLR":          0x00000078,
+    "I2C_IC_SDA_HOLD":       0x0000007c,
+    "I2C_IC_TX_ABRT_SOURCE": 0x00000080,
+    "I2C_IC_SLV_DATA_NACK_ONLY": 0x00000084,
+    "I2C_IC_DMA_CR":             0x00000088,
+    "I2C_IC_DMA_TDLR":           0x0000008c,
+    "I2C_IC_DMA_RDLR":           0x00000090,
+    "I2C_IC_SDA_SETUP":          0x00000094,
+    "I2C_IC_ACK_GENERAL_CALL":   0x00000098,
+    "I2C_IC_ENABLE_STATUS":      0x0000009c,
+    "I2C_IC_FS_SPKLEN":          0x000000a0,
+    "I2C_IC_CLR_RESTART_DET":    0x000000a8,
+    "I2C_IC_COMP_PARAM_1":       0x000000f4,
+    "I2C_IC_COMP_VERSION":       0x000000f8,
+    "I2C_IC_COMP_TYPE":          0x000000fc,
+}
+
+
+
+
+I2C_IC_CON = {
+    0x00000400: "STOP_DET_IF_MASTER_ACTIVE",    # Master issues the STOP_DET interrupt irrespective of whether...
+    0x00000200: "RX_FIFO_FULL_HLD_CTRL",        # This bit controls whether DW_apb_i2c should hold the bus when the Rx...
+    0x00000100: "TX_EMPTY_CTRL",                # This bit controls the generation of the TX_EMPTY interrupt, as described in...
+    0x00000080: "STOP_DET_IFADDRESSED",         # In slave mode: - 1'b1: issues the STOP_DET interrupt only when it is...
+    0x00000040: "IC_SLAVE_DISABLE",             # This bit controls whether I2C has its slave disabled, which means once...
+    0x00000020: "IC_RESTART_EN",                # Determines whether RESTART conditions may be sent when acting as a master.
+    0x00000010: "IC_10BITADDR_MASTER",          # Controls whether the DW_apb_i2c starts its transfers in 7- or 10-bit...
+    0x00000008: "IC_10BITADDR_SLAVE",           # When acting as a slave, this bit controls whether the DW_apb_i2c...
+    0x00000006: "SPEED",                        # These bits control at which speed the DW_apb_i2c operates; its setting is relevant...
+    0x00000001: "MASTER_MODE",                  # This bit controls whether the DW_apb_i2c master is enabled.
+}
+
+I2C_IC_TAR = {
+    0x00000800: "SPECIAL",                      # This bit indicates whether software performs a Device-ID or General Call or START...
+    0x00000400: "GC_OR_START",                  # If bit 11 (SPECIAL) is set to 1 and bit 13(Device-ID) is set to 0, then this...
+    0x000003FF: "IC_TAR",                       # This is the target address for any master transaction
+}
+
+I2C_IC_SAR = {
+    0x000003FF: "IC_SAR",                       # The IC_SAR holds the slave address when the I2C is operating as a slave
+}
+
+I2C_IC_DATA_CMD = {
+    0x00000800: "FIRST_DATA_BYTE",              # Indicates the first data byte received after the address phase for receive...
+    0x00000400: "RESTART",                      # This bit controls whether a RESTART is issued before the byte is sent or received
+    0x00000200: "STOP",                         # This bit controls whether a STOP is issued after the byte is sent or received
+    0x00000100: "CMD",                          # This bit controls whether a read or a write is performed
+    0x000000FF: "DAT",                          # This register contains the data to be transmitted or received on the I2C bus
+}
+
+I2C_IC_SS_SCL_HCNT = {
+    0x0000FFFF: "IC_SS_SCL_HCNT",               # This register must be set before any I2C bus transaction can take place...
+}
+
+I2C_IC_SS_SCL_LCNT = {
+    0x0000FFFF: "IC_SS_SCL_LCNT",               # This register must be set before any I2C bus transaction can take place...
+}
+
+I2C_IC_FS_SCL_HCNT = {
+    0x0000FFFF: "IC_FS_SCL_HCNT",               # This register must be set before any I2C bus transaction can take place...
+}
+
+I2C_IC_FS_SCL_LCNT = {
+    0x0000FFFF: "IC_FS_SCL_LCNT",               # This register must be set before any I2C bus transaction can take place...
+}
+
+I2C_IC_INTR_STAT = {
+    0x00001000: "R_RESTART_DET",                # See IC_RAW_INTR_STAT for a detailed description of R_RESTART_DET bit
+    0x00000800: "R_GEN_CALL",                   # See IC_RAW_INTR_STAT for a detailed description of R_GEN_CALL bit
+    0x00000400: "R_START_DET",                  # See IC_RAW_INTR_STAT for a detailed description of R_START_DET bit
+    0x00000200: "R_STOP_DET",                   # See IC_RAW_INTR_STAT for a detailed description of R_STOP_DET bit
+    0x00000100: "R_ACTIVITY",                   # See IC_RAW_INTR_STAT for a detailed description of R_ACTIVITY bit
+    0x00000080: "R_RX_DONE",                    # See IC_RAW_INTR_STAT for a detailed description of R_RX_DONE bit
+    0x00000040: "R_TX_ABRT",                    # See IC_RAW_INTR_STAT for a detailed description of R_TX_ABRT bit
+    0x00000020: "R_RD_REQ",                     # See IC_RAW_INTR_STAT for a detailed description of R_RD_REQ bit
+    0x00000010: "R_TX_EMPTY",                   # See IC_RAW_INTR_STAT for a detailed description of R_TX_EMPTY bit
+    0x00000008: "R_TX_OVER",                    # See IC_RAW_INTR_STAT for a detailed description of R_TX_OVER bit
+    0x00000004: "R_RX_FULL",                    # See IC_RAW_INTR_STAT for a detailed description of R_RX_FULL bit
+    0x00000002: "R_RX_OVER",                    # See IC_RAW_INTR_STAT for a detailed description of R_RX_OVER bit
+    0x00000001: "R_RX_UNDER",                   # See IC_RAW_INTR_STAT for a detailed description of R_RX_UNDER bit
+}
+
+I2C_IC_INTR_MASK = {
+    0x00001000: "M_RESTART_DET",                # This bit masks the R_RESTART_DET interrupt in IC_INTR_STAT register
+    0x00000800: "M_GEN_CALL",                   # This bit masks the R_GEN_CALL interrupt in IC_INTR_STAT register
+    0x00000400: "M_START_DET",                  # This bit masks the R_START_DET interrupt in IC_INTR_STAT register
+    0x00000200: "M_STOP_DET",                   # This bit masks the R_STOP_DET interrupt in IC_INTR_STAT register
+    0x00000100: "M_ACTIVITY",                   # This bit masks the R_ACTIVITY interrupt in IC_INTR_STAT register
+    0x00000080: "M_RX_DONE",                    # This bit masks the R_RX_DONE interrupt in IC_INTR_STAT register
+    0x00000040: "M_TX_ABRT",                    # This bit masks the R_TX_ABRT interrupt in IC_INTR_STAT register
+    0x00000020: "M_RD_REQ",                     # This bit masks the R_RD_REQ interrupt in IC_INTR_STAT register
+    0x00000010: "M_TX_EMPTY",                   # This bit masks the R_TX_EMPTY interrupt in IC_INTR_STAT register
+    0x00000008: "M_TX_OVER",                    # This bit masks the R_TX_OVER interrupt in IC_INTR_STAT register
+    0x00000004: "M_RX_FULL",                    # This bit masks the R_RX_FULL interrupt in IC_INTR_STAT register
+    0x00000002: "M_RX_OVER",                    # This bit masks the R_RX_OVER interrupt in IC_INTR_STAT register
+    0x00000001: "M_RX_UNDER",                   # This bit masks the R_RX_UNDER interrupt in IC_INTR_STAT register
+}
+
+I2C_IC_RAW_INTR_STAT = {
+    0x00001000: "RESTART_DET",                  # Indicates whether a RESTART condition has occurred on the I2C interface when...
+    0x00000800: "GEN_CALL",                     # Set only when a General Call address is received and it is acknowledged
+    0x00000400: "START_DET",                    # Indicates whether a START or RESTART condition has occurred on the I2C interface...
+    0x00000200: "STOP_DET",                     # Indicates whether a STOP condition has occurred on the I2C interface regardless...
+    0x00000100: "ACTIVITY",                     # This bit captures DW_apb_i2c activity and stays set until it is cleared
+    0x00000080: "RX_DONE",                      # When the DW_apb_i2c is acting as a slave-transmitter, this bit is set to 1 if the...
+    0x00000040: "TX_ABRT",                      # This bit indicates if DW_apb_i2c, as an I2C transmitter, is unable to complete the...
+    0x00000020: "RD_REQ",                       # This bit is set to 1 when DW_apb_i2c is acting as a slave and another I2C master is...
+    0x00000010: "TX_EMPTY",                     # The behavior of the TX_EMPTY interrupt status differs based on the TX_EMPTY_CTRL...
+    0x00000008: "TX_OVER",                      # Set during transmit if the transmit buffer is filled to IC_TX_BUFFER_DEPTH and the...
+    0x00000004: "RX_FULL",                      # Set when the receive buffer reaches or goes above the RX_TL threshold in the...
+    0x00000002: "RX_OVER",                      # Set if the receive buffer is completely filled to IC_RX_BUFFER_DEPTH and an...
+    0x00000001: "RX_UNDER",                     # Set if the processor attempts to read the receive buffer when it is empty by...
+}
+
+I2C_IC_RX_TL = {
+    0x000000FF: "RX_TL",                        # Receive FIFO Threshold Level
+}
+
+I2C_IC_TX_TL = {
+    0x000000FF: "TX_TL",                        # Transmit FIFO Threshold Level
+}
+
+I2C_IC_CLR_INTR = {
+    0x00000001: "CLR_INTR",                     # Read this register to clear the combined interrupt, all individual interrupts,...
+}
+
+I2C_IC_CLR_RX_UNDER = {
+    0x00000001: "CLR_RX_UNDER",                 # Read this register to clear the RX_UNDER interrupt (bit 0) of the...
+}
+
+I2C_IC_CLR_RX_OVER = {
+    0x00000001: "CLR_RX_OVER",                  # Read this register to clear the RX_OVER interrupt (bit 1) of the...
+}
+
+I2C_IC_CLR_TX_OVER = {
+    0x00000001: "CLR_TX_OVER",                  # Read this register to clear the TX_OVER interrupt (bit 3) of the...
+}
+
+I2C_IC_CLR_RD_REQ = {
+    0x00000001: "CLR_RD_REQ",                   # Read this register to clear the RD_REQ interrupt (bit 5) of the...
+}
+
+I2C_IC_CLR_TX_ABRT = {
+    0x00000001: "CLR_TX_ABRT",                  # Read this register to clear the TX_ABRT interrupt (bit 6) of the...
+}
+
+I2C_IC_CLR_RX_DONE = {
+    0x00000001: "CLR_RX_DONE",                  # Read this register to clear the RX_DONE interrupt (bit 7) of the...
+}
+
+I2C_IC_CLR_ACTIVITY = {
+    0x00000001: "CLR_ACTIVITY",                 # Reading this register clears the ACTIVITY interrupt if the I2C is not active anymore
+}
+
+I2C_IC_CLR_STOP_DET = {
+    0x00000001: "CLR_STOP_DET",                 # Read this register to clear the STOP_DET interrupt (bit 9) of the...
+}
+
+I2C_IC_CLR_START_DET = {
+    0x00000001: "CLR_START_DET",                # Read this register to clear the START_DET interrupt (bit 10) of the...
+}
+
+I2C_IC_CLR_GEN_CALL = {
+    0x00000001: "CLR_GEN_CALL",                 # Read this register to clear the GEN_CALL interrupt (bit 11) of...
+}
+
+I2C_IC_ENABLE = {
+    0x00000004: "TX_CMD_BLOCK",                 # In Master mode: - 1'b1: Blocks the transmission of data on I2C bus even if Tx...
+    0x00000002: "ABORT",                        # When set, the controller initiates the transfer abort
+    0x00000001: "ENABLE",                       # Controls whether the DW_apb_i2c is enabled
+}
+
+I2C_IC_STATUS = {
+    0x00000040: "SLV_ACTIVITY",                 # Slave FSM Activity Status
+    0x00000020: "MST_ACTIVITY",                 # Master FSM Activity Status
+    0x00000010: "RFF",                          # Receive FIFO Completely Full
+    0x00000008: "RFNE",                         # Receive FIFO Not Empty
+    0x00000004: "TFE",                          # Transmit FIFO Completely Empty
+    0x00000002: "TFNF",                         # Transmit FIFO Not Full
+    0x00000001: "ACTIVITY",                     # I2C Activity Status
+}
+
+I2C_IC_TXFLR = {
+    0x0000001F: "TXFLR",                        # Transmit FIFO Level
+}
+
+I2C_IC_RXFLR = {
+    0x0000001F: "RXFLR",                        # Receive FIFO Level
+}
+
+I2C_IC_SDA_HOLD = {
+    0x00FF0000: "IC_SDA_RX_HOLD",               # Sets the required SDA hold time in units of ic_clk period, when DW_apb_i2c...
+    0x0000FFFF: "IC_SDA_TX_HOLD",               # Sets the required SDA hold time in units of ic_clk period, when DW_apb_i2c...
+}
+
+I2C_IC_TX_ABRT_SOURCE = {
+    0xFF800000: "TX_FLUSH_CNT",                 # This field indicates the number of Tx FIFO Data Commands which are flushed...
+    0x00010000: "ABRT_USER_ABRT",               # This is a master-mode-only bit
+    0x00008000: "ABRT_SLVRD_INTX",              # 1: When the processor side responds to a slave mode request for data to be...
+    0x00004000: "ABRT_SLV_ARBLOST",             # This field indicates that a Slave has lost the bus while transmitting...
+    0x00002000: "ABRT_SLVFLUSH_TXFIFO",         # This field specifies that the Slave has received a read command and...
+    0x00001000: "ARB_LOST",                     # This field specifies that the Master has lost arbitration, or if...
+    0x00000800: "ABRT_MASTER_DIS",              # This field indicates that the User tries to initiate a Master operation...
+    0x00000400: "ABRT_10B_RD_NORSTRT",          # This field indicates that the restart is disabled (IC_RESTART_EN bit...
+    0x00000200: "ABRT_SBYTE_NORSTRT",           # To clear Bit 9, the source of the ABRT_SBYTE_NORSTRT must be fixed...
+    0x00000100: "ABRT_HS_NORSTRT",              # This field indicates that the restart is disabled (IC_RESTART_EN bit...
+    0x00000080: "ABRT_SBYTE_ACKDET",            # This field indicates that the Master has sent a START Byte and the START...
+    0x00000040: "ABRT_HS_ACKDET",               # This field indicates that the Master is in High Speed mode and the High...
+    0x00000020: "ABRT_GCALL_READ",              # This field indicates that DW_apb_i2c in the master mode has sent a General...
+    0x00000010: "ABRT_GCALL_NOACK",             # This field indicates that DW_apb_i2c in master mode has sent a General...
+    0x00000008: "ABRT_TXDATA_NOACK",            # This field indicates the master-mode only bit
+    0x00000004: "ABRT_10ADDR2_NOACK",           # This field indicates that the Master is in 10-bit address mode and that...
+    0x00000002: "ABRT_10ADDR1_NOACK",           # This field indicates that the Master is in 10-bit address mode and the...
+    0x00000001: "ABRT_7B_ADDR_NOACK",           # This field indicates that the Master is in 7-bit addressing mode and...
+}
+
+I2C_IC_SLV_DATA_NACK_ONLY = {
+    0x00000001: "NACK",                         # Generate NACK
+}
+
+I2C_IC_DMA_CR = {
+    0x00000002: "TDMAE",                        # Transmit DMA Enable
+    0x00000001: "RDMAE",                        # Receive DMA Enable
+}
+
+I2C_IC_DMA_TDLR = {
+    0x0000000F: "DMATDL",                       # Transmit Data Level
+}
+
+I2C_IC_DMA_RDLR = {
+    0x0000000F: "DMARDL",                       # Receive Data Level
+}
+
+I2C_IC_SDA_SETUP = {
+    0x000000FF: "SDA_SETUP",                    # SDA Setup
+}
+
+I2C_IC_ACK_GENERAL_CALL = {
+    0x00000001: "ACK_GEN_CALL",                 # ACK General Call
+}
+
+I2C_IC_ENABLE_STATUS = {
+    0x00000004: "SLV_RX_DATA_LOST",             # Slave Received Data Lost
+    0x00000002: "SLV_DISABLED_WHILE_BUSY",      # Slave Disabled While Busy (Transmit, Receive)
+    0x00000001: "IC_EN",                        # ic_en Status
+}
+
+I2C_IC_FS_SPKLEN = {
+    0x000000FF: "IC_FS_SPKLEN",                 # I2C SS, FS or FM+ spike suppression limit
+}
+
+I2C_IC_CLR_RESTART_DET = {
+    0x00000001: "CLR_RESTART_DET",              # Clear RESTART_DET Interrupt Register
+}
+
+I2C_IC_COMP_PARAM_1 = {
+    0x00FF0000: "TX_BUFFER_DEPTH",              # TX Buffer Depth = 16
+    0x0000FF00: "RX_BUFFER_DEPTH",              # RX Buffer Depth = 16
+    0x00000080: "ADD_ENCODED_PARAMS",           # Encoded parameters not visible
+    0x00000040: "HAS_DMA",                      # DMA handshaking signals are enabled
+    0x00000020: "INTR_IO",                      # COMBINED Interrupt outputs
+    0x00000010: "HC_COUNT_VALUES",              # Programmable count values for each mode
+    0x0000000C: "MAX_SPEED_MODE",               # MAX SPEED MODE = FAST MODE
+    0x00000003: "APB_DATA_WIDTH",               # APB data bus width is 32 bits
+}
+
+I2C_IC_COMP_VERSION = {
+    0xFFFFFFFF: "IC_COMP_VERSION",              # IC_COMP_VERSION = 0x3230312a
+}
+
+I2C_IC_COMP_TYPE = {
+    0xFFFFFFFF: "IC_COMP_TYPE",                 # IC_COMP_TYPE = 0x44570140 (Designware Component Type number = 0x44_57_01_40)
+}

RP2040_Slave.py

@@ -0,0 +1,306 @@
+### i2cSlave.py
+from machine import mem32
+from RP2040_I2C_Registers import*
+
+
+class i2c_slave:
+    """
+    RP2040 I2C Slave implementation using direct register access.
+    
+    This class implements an I2C slave interface for the RP2040 microcontroller,
+    allowing it to receive and transmit data as an I2C peripheral device.
+    """
+
+    I2C0_BASE = 0x40044000
+    I2C1_BASE = 0x40048000
+    IO_BANK0_BASE = 0x40014000
+
+    # Atomic Register Access 
+    mem_rw = 0x0000     # Normal read/write access
+    mem_xor = 0x1000    # XOR on write
+    mem_set = 0x2000    # Bitmask set on write
+    mem_clr = 0x3000    # Bitmask clear on write
+
+
+    def get_Bits_Mask(self, bits, register):
+        """ This function return the bit mask based on bit name """
+        bits_to_clear = bits
+        bit_mask = sum([key for key, value in register.items() if value in bits_to_clear])
+        return bit_mask
+
+    def RP2040_Write_32b_i2c_Reg(self, register, data, atr=0):
+        """ Write RP2040 I2C 32bits register """
+        # < Base Addr > | < Atomic Register Access > | < Register > 
+        mem32[self.i2c_base | atr | register] = data
+
+    def RP2040_Set_32b_i2c_Reg(self, register, data):
+        """ Set bits in RP2040 I2C 32bits register """
+        # < Base Addr > | 0x2000 | < Register > 
+        self.RP2040_Write_32b_i2c_Reg(register, data, atr=self.mem_set)
+
+    def RP2040_Clear_32b_i2c_Reg(self, register, data):
+        """ Clear bits in RP2040 I2C 32bits register """
+        # < Base Addr > | 0x3000 | < Register > 
+        self.RP2040_Write_32b_i2c_Reg(register, data, atr=self.mem_clr)
+    
+    def RP2040_Read_32b_i2c_Reg(self, offset):
+        """ Read RP2040 I2C 32bits register """
+        return mem32[self.i2c_base | offset]
+    
+    def RP2040_Get_32b_i2c_Bits(self, offset, bit_mask):
+        return mem32[self.i2c_base | offset] & bit_mask
+
+    def __init__(self, i2cID=0, sda=0, scl=1, slaveAddress=0x41, enable_clock_stretch=True):
+        self.scl = scl
+        self.sda = sda
+        self.slaveAddress = slaveAddress
+        self.i2c_ID = i2cID
+        if self.i2c_ID == 0:
+            self.i2c_base = self.I2C0_BASE
+        else:
+            self.i2c_base = self.I2C1_BASE
+
+        """
+          I2C Slave Mode Intructions
+          https://datasheets.raspberrypi.com/rp2040/rp2040-datasheet.pdf
+        
+        """
+
+        # 1. Disable the DW_apb_i2c by writing a ‘0’ to IC_ENABLE.ENABLE
+        self.RP2040_Clear_32b_i2c_Reg(I2C_OFFSET["I2C_IC_ENABLE"], 
+                                  self.get_Bits_Mask("ENABLE", I2C_IC_ENABLE))
+
+        # 2. Write to the IC_SAR register (bits 9:0) to set the slave address. 
+        # This is the address to which the DW_apb_i2c responds.
+        self.RP2040_Clear_32b_i2c_Reg(I2C_OFFSET["I2C_IC_SAR"], 
+                                  self.get_Bits_Mask("IC_SAR", I2C_IC_SAR))
+
+        self.RP2040_Set_32b_i2c_Reg(I2C_OFFSET["I2C_IC_SAR"], 
+                                self.slaveAddress & self.get_Bits_Mask("IC_SAR", I2C_IC_SAR))
+
+        # 3. Write to the IC_CON register to specify which type of addressing is supported (7-bit or 10-bit by setting bit 3).
+        # Enable the DW_apb_i2c in slave-only mode by writing a ‘0’ into bit six (IC_SLAVE_DISABLE) and a ‘0’ to bit zero 
+        # (MASTER_MODE).
+        
+        # Disable Master mode
+        self.RP2040_Clear_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CON"], 
+                                      self.get_Bits_Mask("MASTER_MODE", I2C_IC_CON))
+        
+        # Enable slave mode 
+        self.RP2040_Clear_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CON"], 
+                                      self.get_Bits_Mask("IC_SLAVE_DISABLE", I2C_IC_CON))
+        
+        # Enable clock strech 
+        if enable_clock_stretch:
+            self.RP2040_Set_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CON"], 
+                                      self.get_Bits_Mask("RX_FIFO_FULL_HLD_CTRL", I2C_IC_CON))
+
+        
+        # 4. Enable the DW_apb_i2c by writing a ‘1’ to IC_ENABLE.ENABLE.
+        self.RP2040_Set_32b_i2c_Reg(I2C_OFFSET["I2C_IC_ENABLE"], 
+                                self.get_Bits_Mask("IC_ENABLE", I2C_IC_ENABLE))
+        
+        # Reset GPIO0 function 
+        mem32[ self.IO_BANK0_BASE | self.mem_clr | ( 4 + 8 * self.sda) ] = 0x1f
+        # Set GPIO0 as IC0_SDA function 
+        mem32[ self.IO_BANK0_BASE | self.mem_set | ( 4 + 8 * self.sda) ] = 0x03
+
+        # Reset GPIO1 function
+        mem32[ self.IO_BANK0_BASE | self.mem_clr | ( 4 + 8 * self.scl) ] = 0x1f
+        # Set GPIO1 as IC0_SCL function 
+        mem32[ self.IO_BANK0_BASE | self.mem_set | ( 4 + 8 * self.scl) ] = 3
+
+    class I2CStateMachine:
+        I2C_RECEIVE = 0
+        I2C_REQUEST = 1
+        I2C_FINISH  = 2
+        I2C_START   = 3
+        I2C_IDLE    = 4
+
+   
+    class I2CTransaction:
+
+        def __init__(self, address: int, data_byte: list):
+            self.address = address  
+            self.data_byte = data_byte
+
+    
+    
+    def handle_event(self):
+        """Optimized event detection by reading interrupt status register once"""
+        # Read entire interrupt status register in one operation
+        intr_stat = self.RP2040_Read_32b_i2c_Reg(I2C_OFFSET["I2C_IC_INTR_STAT"])
+        
+        # Check for restart condition
+        if intr_stat & self.get_Bits_Mask("R_RESTART_DET", I2C_IC_INTR_STAT):
+            self.RP2040_Read_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CLR_RESTART_DET"])
+            # Handle restart logic here
+            
+        # Check other conditions using the same intr_stat value
+        # I2C Master has abort the transactions
+        if (intr_stat & self.get_Bits_Mask("R_TX_ABRT", I2C_IC_INTR_STAT)):
+            # Clear int
+            self.RP2040_Read_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CLR_TX_ABRT"])
+            return i2c_slave.I2CStateMachine.I2C_FINISH
+        
+        # Last byte transmitted by I2C Slave but NACK from I2C Master 
+        if (intr_stat & self.get_Bits_Mask("R_RX_DONE", I2C_IC_INTR_STAT)):
+            # Clear int
+            self.RP2040_Read_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CLR_RX_DONE"])
+            return i2c_slave.I2CStateMachine.I2C_FINISH
+
+        # Start condition detected by I2C Slave
+        if (intr_stat & self.get_Bits_Mask("R_START_DET", I2C_IC_INTR_STAT)):
+            # Clear start detection 
+            self.RP2040_Read_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CLR_START_DET"])
+            return i2c_slave.I2CStateMachine.I2C_START
+
+        # Stop condition detected by I2C Slave
+        if (intr_stat & self.get_Bits_Mask("R_STOP_DET", I2C_IC_INTR_STAT)):
+            # Clear stop detection
+            self.RP2040_Read_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CLR_STOP_DET"])
+            return i2c_slave.I2CStateMachine.I2C_FINISH
+        
+        # Check if RX FIFO is not empty
+        if (self.RP2040_Get_32b_i2c_Bits(I2C_OFFSET["I2C_IC_STATUS"],
+                                 self.get_Bits_Mask("RFNE", I2C_IC_STATUS))):
+            
+            return i2c_slave.I2CStateMachine.I2C_RECEIVE
+        
+        # Check if Master is requesting data 
+        if (intr_stat & self.get_Bits_Mask("R_RD_REQ", I2C_IC_INTR_STAT)):
+            
+            # Shall Wait until transfer is done, timing recommended 10 * fastest SCL clock period
+            # for 100 Khz = (1/100E3) * 10 = 100 uS
+            # for 400 Khz = (1/400E3) * 10 = 25 uS
+                
+            return i2c_slave.I2CStateMachine.I2C_REQUEST
+
+        # Add at the end
+        return i2c_slave.I2CStateMachine.I2C_IDLE
+
+    def is_Master_Req_Read(self):
+        """ Return status if I2C Master is requesting a read sequence """
+        
+        # Check RD_REQ Interrupt bit (master wants to read data from the slave)
+        status = self.RP2040_Get_32b_i2c_Bits(I2C_OFFSET["I2C_IC_RAW_INTR_STAT"],
+                                 self.get_Bits_Mask("RD_REQ", I2C_IC_RAW_INTR_STAT))
+
+        if status :
+            return True
+        return False
+    
+    """
+    def is_Master_Req_Seq_Write(self):
+        # Return true if I2C Master is requesting a sequential data writing 
+   
+        # Check whether is FIRST_DATA_BYTE bit is active in IC_DATA_CMD. 
+        first_data_byte_stat = self.RP2040_Get_32b_i2c_Bits(I2C_OFFSET["I2C_IC_DATA_CMD"],
+                                 self.get_Bits_Mask("FIRST_DATA_BYTE", I2C_IC_DATA_CMD))
+                
+        # Check whether is STOP_DET_IFADDRESSED bit is active in IC_CON.
+        stop_stat = self.RP2040_Get_32b_i2c_Bits(I2C_OFFSET["I2C_IC_CON"],
+                                 self.get_Bits_Mask("STOP_DET_IFADDRESSED", I2C_IC_CON))
+        
+        if (stop_stat):
+            # Clear stop bit int 
+            self.RP2040_Read_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CLR_STOP_DET"])
+        
+        # Master sequential write true if FIRST_DATA_BYTE bit is active and no stop condition detected. 
+        if first_data_byte_stat and not(stop_stat):
+            return True
+        return False
+    """
+
+    def Slave_Write_Data(self, data):
+        """ Write 8bits of data at destination of I2C Master """
+    
+        # Send data
+        self.RP2040_Write_32b_i2c_Reg(I2C_OFFSET["I2C_IC_DATA_CMD"], data & 
+                                  self.get_Bits_Mask("DAT", I2C_IC_DATA_CMD))
+        
+        self.RP2040_Read_32b_i2c_Reg(I2C_OFFSET["I2C_IC_CLR_RD_REQ"]) 
+        
+        
+    def Available(self):
+        """ Return true if data has been received from I2C Master """
+
+        # Get RFNE Bit (Receive FIFO Not Empty)
+        return self.RP2040_Get_32b_i2c_Bits(I2C_OFFSET["I2C_IC_STATUS"],
+                                self.get_Bits_Mask("RFNE", I2C_IC_STATUS))
+        
+
+    def Read_Data_Received(self):
+        """ Return data from I2C Master """
+              
+        return self.RP2040_Read_32b_i2c_Reg(I2C_OFFSET["I2C_IC_DATA_CMD"]) &  self.get_Bits_Mask("DAT", I2C_IC_DATA_CMD)
+
+    def deinit(self):
+        """Disable the I2C slave and release pins"""
+        # Disable I2C interface
+        self.RP2040_Clear_32b_i2c_Reg(I2C_OFFSET["I2C_IC_ENABLE"], 
+                              self.get_Bits_Mask("ENABLE", I2C_IC_ENABLE))
+        
+        # Reset GPIO pins back to default state
+        mem32[self.IO_BANK0_BASE | self.mem_clr | (4 + 8 * self.sda)] = 0x1f
+        mem32[self.IO_BANK0_BASE | self.mem_clr | (4 + 8 * self.scl)] = 0x1f
+
+
+if __name__ == "__main__":
+    import machine
+    from machine import mem32
+    
+    def main():
+        data_buf = []
+        addr = 0x00
+        
+        # Create I2C slave instance
+        s_i2c = i2c_slave(0, sda=0, scl=1, slaveAddress=0x41, enable_clock_stretch=True)
+        state = i2c_slave.I2CStateMachine.I2C_IDLE
+        currentTransaction = i2c_slave.I2CTransaction(addr, data_buf)
+        
+        counter = 0
+        
+        print("I2C Slave test")
+        try:
+            while True:
+                state = s_i2c.handle_event()
+
+                if state == s_i2c.I2CStateMachine.I2C_START:
+                    pass
+    
+                if state == s_i2c.I2CStateMachine.I2C_RECEIVE:
+                    if currentTransaction.address == 0x00:
+                        # First byte received is the register address
+                        currentTransaction.address = s_i2c.Read_Data_Received() 
+
+                    # Read all data byte received until RX FIFO is empty
+                    while (s_i2c.Available()):
+                        currentTransaction.data_byte.append(s_i2c.Read_Data_Received())
+                        # Virtually Increase register address
+                        # s_i2c.I2CTransaction.address += 1
+            
+                if state == s_i2c.I2CStateMachine.I2C_REQUEST:
+                    # Send some dummy data back 
+                    while (s_i2c.is_Master_Req_Read()):
+                        counter += 1
+                        s_i2c.Slave_Write_Data(counter)
+
+                        # Virtually Increase register address
+                        # s_i2c.I2CTransaction.address += 1
+                        print ("Sendind data : ", counter)
+            
+                if state == s_i2c.I2CStateMachine.I2C_FINISH:
+                    print ("Register : ", currentTransaction.address ,"Received : ", currentTransaction.data_byte)
+                
+                    currentTransaction.address = 0x00
+                    currentTransaction.data_byte = []
+                    state= s_i2c.I2CStateMachine.I2C_IDLE
+        
+
+
+        except KeyboardInterrupt:
+            s_i2c.deinit()  # Clean up when done
+            print("I2C slave stopped")
+    
+    main()

distance.py

@@ -0,0 +1,84 @@
+import machine
+import time
+from i2c_handler import I2CSlaveHandler, Packet
+
+class DistanceHandler(I2CSlaveHandler):
+    """
+    I2C handler that responds to master requests by sending distance values
+    from an HC-SR04 ultrasonic sensor.
+    """
+
+    def __init__(self, i2c_id, sda, scl, slave_addr, trig_pin, echo_pin):
+        super().__init__(i2c_id, sda, scl, slave_addr)
+        self.trig = machine.Pin(trig_pin, machine.Pin.OUT)
+        self.echo = machine.Pin(echo_pin, machine.Pin.IN)
+
+    def read_distance_mm(self):
+        """
+        Measure distance using the HC-SR04 sensor.
+        Returns distance in millimeters as an integer.
+        """
+        # Send a 10us pulse to trigger
+        self.trig.low()
+        time.sleep_us(2)
+        self.trig.high()
+        time.sleep_us(10)
+        self.trig.low()
+
+        # Wait for echo high (timeout after 25ms)
+        timeout = 25000
+        start = time.ticks_us()
+        while not self.echo.value():
+            if time.ticks_diff(time.ticks_us(), start) > timeout:
+                return 0  # sensor error
+        t1 = time.ticks_us()
+        while self.echo.value():
+            if time.ticks_diff(time.ticks_us(), t1) > timeout:
+                return 0  # sensor error
+        t2 = time.ticks_us()
+
+        # Calculate duration and distance
+        duration = time.ticks_diff(t2, t1)
+        distance_mm = (duration * 100) // 582
+        # HC-SR04: distance (mm) = duration (us) / 5.82
+
+        # Clamp to 0..65535
+        return max(0, min(distance_mm, 65535))
+
+    def process_request(self):
+        """
+        Called when master requests data. Responds with a packet containing distance.
+        Packet format:
+        Byte 0: 0x01 (distance command)
+        Byte 1: reserved
+        Byte 2-3: distance (uint16, mm, little-endian)
+        Byte 4-6: reserved
+        Byte 7: checksum (xor of bytes 0-6)
+        """
+        #dist = self.read_distance_mm()
+        dist = 155
+        data = [0] * Packet.LENGTH
+        data[0] = 0x01  # Command: distance
+        data[2] = dist & 0xFF  # Low byte
+        data[3] = (dist >> 8) & 0xFF  # High byte
+
+        # Checksum: XOR of bytes 0-6
+        cs = 0
+        for b in data[:-1]:
+            cs ^= b
+        data[7] = cs
+        
+        print(Packet.from_bytes(data))
+        return Packet.from_bytes(data)
+    
+def main():
+    handler = DistanceHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41, trig_pin=3, echo_pin=2)
+    print("Distance handler I2C slave started")
+    try:
+        handler.handle()
+    except KeyboardInterrupt:
+        handler.deinit()
+        print("I2C slave stopped")
+
+if __name__ == "__main__":
+    main()

i2c_handler.py

@@ -0,0 +1,124 @@
+import machine
+from RP2040_Slave import i2c_slave
+
+# === Packet Definition ===
+class Packet:
+    """Represents an 8-byte I2C packet."""
+    LENGTH = 8
+
+    def __init__(self, data=None):
+        if data is None:
+            data = [0] * self.LENGTH
+        if len(data) != self.LENGTH:
+            raise ValueError(f"Packet must be {self.LENGTH} bytes")
+        self.bytes = data
+
+    @classmethod
+    def from_bytes(cls, byte_list):
+        """Create Packet from list of bytes."""
+        return cls(byte_list[:cls.LENGTH])
+
+    def __getitem__(self, idx):
+        return self.bytes[idx]
+
+    def __setitem__(self, idx, value):
+        self.bytes[idx] = value
+
+    def __repr__(self):
+        fields = [f"Byte {i}: 0x{b:02X}" for i, b in enumerate(self.bytes)]
+        return "\n".join(fields)
+
+    def as_list(self):
+        return self.bytes
+
+    def is_valid(self):
+        """Verify checksum (XOR of bytes 0-6 equals byte 7)."""
+        cs = 0
+        for b in self.bytes[:-1]:
+            cs ^= b
+        return cs == self.bytes[-1]
+
+# === I2C Slave Handler ===
+class I2CSlaveHandler:
+    def __init__(self, i2c_id, sda, scl, slave_addr):
+        self.s_i2c = i2c_slave(i2c_id, sda=sda, scl=scl, slaveAddress=slave_addr)
+        self.packet_buf = []
+        self.response_buf = None
+        self.response_idx = 0
+
+    def handle(self):
+        state = self.s_i2c.I2CStateMachine.I2C_IDLE
+        while True:
+            state = self.s_i2c.handle_event()
+
+            if state == self.s_i2c.I2CStateMachine.I2C_RECEIVE:
+                while self.s_i2c.Available():
+                    byte = self.s_i2c.Read_Data_Received()
+                    self.packet_buf.append(byte)
+                    #print(f"Received byte: 0x{byte:02X}")
+
+                # Only process when full packet is received
+                if len(self.packet_buf) == Packet.LENGTH:
+                    packet = Packet.from_bytes(self.packet_buf)
+                    print("Received 8-byte packet:")
+                    print(packet)
+                    print("Packet valid:", packet.is_valid())
+                    self.process_packet(packet)
+                    self.packet_buf = []  # Clear buffer after processing
+
+            if state == self.s_i2c.I2CStateMachine.I2C_FINISH:
+                if self.packet_buf and len(self.packet_buf) < Packet.LENGTH:
+                    #print(f"Incomplete packet ({len(self.packet_buf)} bytes): {[f'0x{b:02X}' for b in self.packet_buf]}")
+                    print(f"Incomplete packet ({len(self.packet_buf)} bytes): {self.packet_buf}")
+                    self.packet_buf = []  # Clear buffer on incomplete packet
+                state = self.s_i2c.I2CStateMachine.I2C_IDLE
+
+            if state == self.s_i2c.I2CStateMachine.I2C_REQUEST:
+                print("Master requested data, sending response packet:")
+                #self.s_i2c.Slave_Write_Data(self.process_request().as_list())
+                # Prepare response buffer if not set
+                if self.response_buf is None:
+                    self.response_buf = self.process_request().as_list()
+                    self.response_idx = 0
+                    print("Prepared response packet:", self.response_buf)
+                
+                # Send next byte
+                if self.response_idx < Packet.LENGTH:
+                    self.s_i2c.Slave_Write_Data(self.response_buf[self.response_idx])
+                    print("write")
+                    self.response_idx += 1
+                else:
+                    # Reset response for next transaction
+                    self.response_buf = None
+                    self.response_idx = 0
+
+    def process_packet(self, packet):
+        """Override this method to process packets."""
+        # Example: Print command/type and target/channel
+        cmd = packet[0]
+        target = packet[1]
+        print(f"Command/Type: 0x{cmd:02X}, Target/Channel: 0x{target:02X}")
+        # Add custom logic for your robot here
+
+    def process_request(self):
+        """Override this method to process requests"""
+        return Packet.from_bytes([0x12, 0, 0, 0, 0, 0, 0, 0])
+
+    def deinit(self):
+        self.s_i2c.deinit()
+
+# === Example usage for RP2040 ===
+def main():
+    # Change slaveAddress as needed for each RP2040
+    SLAVE_ADDR = 0x41
+
+    handler = I2CSlaveHandler(i2c_id=0, sda=0, scl=1, slave_addr=SLAVE_ADDR)
+    print(f"I2C Slave started at address 0x{SLAVE_ADDR:02X}")
+    try:
+        handler.handle()
+    except KeyboardInterrupt:
+        handler.deinit()
+        print("I2C slave stopped")
+
+if __name__ == "__main__":
+    main()

led.py

@@ -0,0 +1,36 @@
+from i2c_handler import I2CSlaveHandler, Packet
+import neopixel
+
+class LedDistanceHandler(I2CSlaveHandler):
+    def __init__(self, i2c_id, sda, scl, slave_addr):
+        super().__init__(i2c_id, sda, scl, slave_addr)
+        self.last_led_color = (0, 0, 0)  # RGB tuple
+        self.np = neopixel.NeoPixel(machine.Pin(23), 1)
+
+    def process_packet(self, packet: Packet):
+        cmd = packet[0]
+        # Assume LED color command is 0x10, RGB in bytes 2,3,4
+        if cmd == 0x10:  # Set LED Color
+            r = packet[2]
+            g = packet[3]
+            b = packet[4]
+            self.last_led_color = (r, g, b)
+            print(f"Set LED color to RGB({r},{g},{b})")
+            # TODO: Set the hardware LED to this color
+            self.np[0] = self.last_led_color
+            self.np.write()
+        else:
+            print(f"Unknown command 0x{cmd:02X}")
+            self.response_packet = None
+
+def main():
+    handler = LedDistanceHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41)
+    print("LED/Distance handler I2C slave started")
+    try:
+        handler.handle()
+    except KeyboardInterrupt:
+        handler.deinit()
+        print("I2C slave stopped")
+
+if __name__ == "__main__":
+    main()

usage.py

@@ -0,0 +1,40 @@
+from i2c_handler import I2CSlaveHandler, Packet
+
+class MyRobotHandler(I2CSlaveHandler):
+    def process_packet(self, packet: Packet):
+        """
+        Example: Override process_packet to perform actions based on packet command/type.
+        """
+        cmd = packet[0]
+        target = packet[1]
+        speed = (packet[2] << 8) | packet[3]
+        # Convert speed from unsigned to signed if needed
+        if speed & 0x8000:
+            speed -= 0x10000
+
+        print(f"Custom handler: Command 0x{cmd:02X}, Target 0x{target:02X}, Speed {speed}")
+
+        # Example: Perform a motor action for command 0x01
+        if cmd == 0x01:  # Set Motor Speed
+            print(f"Setting motor {target} speed to {speed}")
+            # TODO: Add your motor control code here
+
+        elif cmd == 0xFF:  # Emergency Stop
+            print("Emergency stop received!")
+            # TODO: Add your emergency stop code here
+
+        else:
+            print("Unknown command")
+
+# Usage example
+def main():
+    handler = MyRobotHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41)
+    print("MyRobotHandler I2C slave started")
+    try:
+        handler.handle()
+    except KeyboardInterrupt:
+        handler.deinit()
+        print("I2C slave stopped")
+
+if __name__ == "__main__":
+    main()