import mmap
import os
import struct
import time

LITEI2C_BASE = 0x40000000
LITEI2C_SIZE = 0x2000

I2C_SLAVE_ADDR = 0x0A
I2C_SLAVE_MEMORY_ADDR = 0x60

REG_ADDR = {
    "phy_speed_mode": 0x1800,
    "master_active": 0x1804,
    "master_settings": 0x1808,
    "master_addr": 0x180C,
    "master_rxtx": 0x1810,
    "master_status": 0x1814,
}

def write_reg(mem, offset, value):
    mem.seek(offset)
    mem.write(struct.pack("<I", value))

def read_reg(mem, offset):
    mem.seek(offset)
    return struct.unpack("<I", mem.read(4))[0]

fd = os.open("/dev/mem", os.O_RDWR | os.O_SYNC)
mem = mmap.mmap(fd, LITEI2C_SIZE, mmap.MAP_SHARED, mmap.PROT_READ | mmap.PROT_WRITE, offset=LITEI2C_BASE)

write_reg(mem, REG_ADDR["phy_speed_mode"], 0) # Using 100kHz speed
write_reg(mem, REG_ADDR["master_active"], 1)
while not (read_reg(mem, REG_ADDR["master_status"]) & 1):
    print("Wait for tx_ready with master_status:", read_reg(mem, REG_ADDR["master_status"]))
    time.sleep(0.001)

write_reg(mem, REG_ADDR["master_settings"], 0x0501) # rx_len=5, tx_len=1
write_reg(mem, REG_ADDR["master_addr"], I2C_SLAVE_ADDR)
write_reg(mem, REG_ADDR["master_rxtx"], I2C_SLAVE_MEMORY_ADDR)

write_reg(mem, REG_ADDR["master_rxtx"], I2C_SLAVE_MEMORY_ADDR)
write_reg(mem, REG_ADDR["master_rxtx"], I2C_SLAVE_MEMORY_ADDR)
write_reg(mem, REG_ADDR["master_settings"], 0x0401) # rx_len=4, tx_len=1
write_reg(mem, REG_ADDR["master_rxtx"], I2C_SLAVE_MEMORY_ADDR)

print("I2C read sequence complete.")

mem.close()
os.close(fd)