SSD1306 TWI AVR Assembly

;Program is made for the Atmega328p for connecting with the SSD1306 OLED
;Made by int 21h

.def temp1 = r16
.def temp2 = r17
.def temp3 = r18

.equ SLAVE_ADDRESS_W = 0b01111010

;******************************
;           INIT
;******************************

    clr   temp2
    clr   temp3

    ;Init Stackpointer
    ldi   temp1, HIGH(RAMEND)
    out   SPH, temp1
    ldi   temp1, LOW(RAMEND)
    out   SPL, temp1

    ldi   temp1, 0xFF
    out   DDRD, temp1
    out   DDRC, temp1 ;Don't know if needed or not

;******************************
;           MAIN
;******************************

    ldi   temp1, 0b00110011
    ;8*8 Dotmatrix
    rcall oled_send_data
    rcall oled_send_data
    rcall oled_send_data
    rcall oled_send_data
    rcall oled_send_data
    rcall oled_send_data
    rcall oled_send_data
    rcall oled_send_data

end:
    rjmp  end

;******************************
;           ROUTINEN
;******************************

oled_send_command:
    ;Not worked on this yet
    ret

oled_send_data:
    push  temp1
    rcall oled_start

    ;Send Slave Addres
    ldi   temp1, SLAVE_ADDRESS_W
    sts   TWDR, temp1
    ldi   temp1, (1 << TWINT)|(1 << TWEN)
    sts   TWCR, temp1
oled_send_data_0:
    lds   temp1, TWCR
    sbrs  temp1, TWINT
    rjmp  oled_send_data_0

    ;Send Control Byte
    ldi   temp1, 0x40
    sts   TWDR, temp1
    ldi   temp1, (1 << TWINT)|(1 << TWEN)
    sts   TWCR, temp1
oled_send_data_1:
    lds   temp1, TWCR
    sbrs  temp1, TWINT
    rjmp  oled_send_data_1

    ;Send Byte
    pop   temp1
    push  temp1
    sts   TWDR, temp1
    ldi   temp1, (1 << TWINT)|(1 << TWEN)
    sts   TWCR, temp1
oled_send_data_2:
    lds   temp1, TWCR
    sbrs  temp1, TWINT
    rjmp  oled_send_data_2

    rcall oled_stop
    pop   temp1
    ret

oled_start:
    push  temp1
    ldi   temp1, (1 << TWINT)|(1 << TWSTA)|(1 << TWEN)
    sts   TWCR, temp1
oled_start_0:
    lds   temp1, TWCR
    sbrs  temp1, TWINT
    rjmp  oled_start_0
    ;Check Status Code
    lds   temp1, TWSR
    andi  temp1, 0x08
    sbrs  temp1, 3
    rcall oled_error
    pop   temp1
    ret

oled_stop:
    push  temp1
    ldi   temp1, (1 << TWINT)|(1 << TWSTO)|(1 << TWEN)
    sts   TWCR, temp1
oled_stop_0:
    lds   temp1, TWCR
    sbrs  temp1, TWINT
    rjmp  oled_start_0
    pop   temp1
    ret

;If an error occurs, toggle the LED
oled_error:
    push  temp1
    ldi   temp1, 0x01
    out   PORTD, temp1
    pop   temp1
    ret