ORG     $FFFF0000
    LDMD    #$31
    LDS     #$FFFE0000
    LDU     #$FFFDF000
* Direct mode: address is built with DS:DP:8 bits offset
* i2c controller is at $FFFEB000-$FFFEB003
    LDDS    #$FFFE
    LDDP    #$B0
* TWI0 control register
    LDA     #%00000000  ; polling mode
    STA     <TWI0+2
* Set TWI0 clock divisor
    LDD     #0
    STD     <TWI0+4
* Set slave address
    LDD     #$50
    STD     <TWI0+6

* Write into $20
    LDA     #$20
    STA     <TWI0
    LDA     #%00000001
    STA     <TWI0+1
    LBSR    TX_EMPTY    ; wait until TX register is loaded in buffer
    LDA     #$4E        ; data written at address $20
    STA     <TWI0
    LDA     #%00001001
    STA     <TWI0+1
    LDB     #%00000011  ; wait until STOP state
    LBSR    POLL

* Write into $10 and $11
    LDA     #$10
    STA     <TWI0
    LDA     #%00000001
    STA     <TWI0+1
    LDB     #%00010001  ; wait until SLAVE ACK after address
    LBSR    POLL
    LDA     #$3F        ; data written at address $10
    STA     <TWI0
    LDA     #%00000001
    STA     <TWI0+1
    LDB     #%00000101  ; wait until WRITE is started
    LBSR    POLL
    LDA     #$24        ; data written at address $11
    STA     <TWI0
    LDA     #%00001001
    STA     <TWI0+1
    LDB     #%00000011  ; wait until STOP state
    LBSR    POLL

* Read addresses $10 (3F) and $11 (24)
    LDA     #$10
    STA     <TWI0
* Send data (write mode)
    LDA     #%001
    STA     <TWI0+1
    LDB     #%00000101  ; wait until WRITE is started
    BSR     POLL
* Receive data (read mode with MACK)
    LDA     #%111
    STA     <TWI0+1
    LDB     #%00001000  ; wait until MASTER_ACK state
    BSR     POLL

* Wait
    LDW     #$0200
WAIT:
    DECW
    BNE     WAIT

    LDF     <TWI0       ; read data (#$3F)

* TWI0 control register ; IRQ mode
    LDA     #%00100000
    STA     <TWI0+2

* Receive data (read mode without MACK)
    LDA     #%011
    STA     <TWI0+1
    LDB     #%00001001  ; wait until NO_MASTER_ACK state
    BSR     POLL

* Read addresses $20 (4E)
    LDA     #$20
    STA     <TWI0
* Send data (write mode)
    LDA     #%001
    STA     <TWI0+1
    BSR     WAIT_ACK
* Receive data (read mode without MACK)
    LDA     #%011
    STA     <TWI0+1
    LDB     #%00000011  ; wait until STOP state
    BSR     POLL

* Read addresses $11 (24)
    LDA     #$11
    STA     <TWI0
* Send data (write mode)
    LDA     #%001
    STA     <TWI0+1
    BSR     WAIT_ACK
* Receive data (read mode without MACK)
    LDA     #%011
    STA     <TWI0+1
    LDB     #%00000011  ; wait until STOP state
    BSR     POLL

ABORT:
    FCB     $CF

TX_EMPTY:
    LDA     <TWI0+2
    ANDA    #$40
    BEQ     TX_EMPTY
    RTS

WAIT_ACK:
    LDA     <TWI0+2
    ANDA    #$08
    BEQ     WAIT_ACK
    RTS

POLL:
    * Wait for READY state
    LDA     <TWI0+3
    CMPR    A,B
    BNE     POLL
    CLR     <TWI0
    RTS

* Interruptions
    ORG     $FFFFFF00
    LDE     <TWI0
    RTI

TWI0    EQU $FFFEB000

SPI0    EQU $FFFEC000

PTM0    EQU $FFFED000
PTM1    EQU $FFFED008

ACIA0   EQU $FFFEE000
ACIA1   EQU $FFFEE004

PIA0    EQU $FFFEF000
PIA1    EQU $FFFEF002
PIA2    EQU $FFFEF004
PIA3    EQU $FFFEF006
PIA4    EQU $FFFEF008

    * TWI0 interrupt
    ORG     $FFFFFFD8
    FDB     $FF00

    ORG     $FFFFFFF0
    FDB     $0000
    FDB     $0000
    FDB     $0000
    FDB     $0000
    FDB     $0000
    FDB     $0000
    FDB     $0000
    FDB     $0000

    END