#include <avr/io.h>

#define IO(x) _SFR_IO_ADDR(x)

#define COLSTEP  IO(GPIOR0)
#define COLUMN   IO(GPIOR1)

.extern frameptr

.section .data
frm:
.WORD frameptr

.section .bss
time_frac:
.BYTE 0

.section .text
.global TIMER0_COMPA_vect
.func TIMER0_COMPA_vect
TIMER0_COMPA_vect:
    /* prologue */
    push r0
    push r30
    push r31
    in r30, IO(SREG)
    push r30
    clt
    /* ===  10 cycles === */

    in r30, COLSTEP
    subi r30, -(4)
    cpi r30, 8*4
    brne colstep_out
    in r30, COLUMN
    subi r30, -(4)
    cpi r30, 18*4
    brne column_out
    ldi r30, 0
column_out:
    out COLUMN, r30
    ldi r30, 0
    set    /* T flag: remember that colstep == 0 */
colstep_out:
    out COLSTEP, r30
    /* === 6 od. 13 cycles === */

    /* update OCR0A for the next timeslice and
     * figure out bitindex (used as pointer into framebuffer) */
    /* r30 here is colstep, which already is multiplied * 4 for the switch() */
    clr r31
    subi r30, lo8(-(pm(startbitindex)))
    sbci r31, hi8(-(pm(startbitindex)))
    ijmp
    /* ===  5 cycles === */

startbitindex:
    /* switch for new time slice and bitindex */
    ldi r30, 0x07 * 4  $  out IO(OCR0A), r30  $  ldi r30, 2  $  rjmp endbitindex
    ldi r30, 0x07 * 4  $  out IO(OCR0A), r30  $  ldi r30, 4  $  rjmp endbitindex
    ldi r30, 0x07 * 2  $  out IO(OCR0A), r30  $  ldi r30, 1  $  rjmp endbitindex
    ldi r30, 0x07 * 4  $  out IO(OCR0A), r30  $  ldi r30, 3  $  rjmp endbitindex
    ldi r30, 0x07 * 4  $  out IO(OCR0A), r30  $  ldi r30, 4  $  rjmp endbitindex
    ldi r30, 0x07 * 4  $  out IO(OCR0A), r30  $  ldi r30, 3  $  rjmp endbitindex
    ldi r30, 0x07 * 8  $  out IO(OCR0A), r30  $  ldi r30, 4  $  rjmp endbitindex
    ldi r30, 0x07 * 1  $  out IO(OCR0A), r30  $  ldi r30, 0
    /* ===  5 cycles (3 for colstep == 7) === */
endbitindex:

    /* r30 += 5 * column */
    in r0, COLUMN
    add r30, r0
    lsr r0
    lsr r0
    add r30, r0
    clr r31
    lds r0, frm
    add r30, r0
    lds r0, frm+1
    adc r31, r0
    /* === 12 cycles === */
    /* Z is now &frm[column * 5 + bitindex[colstep]] */

    /* prepare new data for PORTC */
    ld r0, Z

    /* use remembered T flag */
    brts enable_next_column     /* if colstep == 0 goto enable_next_column */
    out IO(PORTC), r0
    rjmp return


enable_next_column:
    /* activate new column: start: calculate target address for ijmp*/
    in r30, COLUMN   /* already multiplied by 4 */
    clr r31
    subi r30, lo8(-(pm(startsetbit)))
    sbci r31, hi8(-(pm(startsetbit)))
    ijmp
    /* ===  6 cycles === */

startsetbit:
    /*  0: */  sbi IO(PORTA), 7  $  out IO(PORTC), r0  $  cbi IO(PORTD), 4  $  rjmp endsetbit
    /*  1: */  sbi IO(PORTD), 4  $  out IO(PORTC), r0  $  cbi IO(PORTD), 5  $  rjmp endsetbit
    /*  2: */  sbi IO(PORTD), 5  $  out IO(PORTC), r0  $  cbi IO(PORTD), 6  $  rjmp endsetbit
    /*  3: */  sbi IO(PORTD), 6  $  out IO(PORTC), r0  $  cbi IO(PORTD), 7  $  rjmp endsetbit
    /*  4: */  sbi IO(PORTD), 7  $  out IO(PORTC), r0  $  cbi IO(PORTB), 2  $  rjmp endsetbit
    /*  5: */  sbi IO(PORTB), 2  $  out IO(PORTC), r0  $  cbi IO(PORTB), 3  $  rjmp endsetbit
    /*  6: */  sbi IO(PORTB), 3  $  out IO(PORTC), r0  $  cbi IO(PORTB), 4  $  rjmp endsetbit
    /*  7: */  sbi IO(PORTB), 4  $  out IO(PORTC), r0  $  cbi IO(PORTB), 5  $  rjmp endsetbit
    /*  8: */  sbi IO(PORTB), 5  $  out IO(PORTC), r0  $  cbi IO(PORTB), 6  $  rjmp endsetbit
    /*  9: */  sbi IO(PORTB), 6  $  out IO(PORTC), r0  $  cbi IO(PORTB), 7  $  rjmp endsetbit
    /* 10: */  sbi IO(PORTB), 7  $  out IO(PORTC), r0  $  cbi IO(PORTA), 0  $  rjmp endsetbit
    /* 11: */  sbi IO(PORTA), 0  $  out IO(PORTC), r0  $  cbi IO(PORTA), 1  $  rjmp endsetbit
    /* 12: */  sbi IO(PORTA), 1  $  out IO(PORTC), r0  $  cbi IO(PORTA), 2  $  rjmp endsetbit
    /* 13: */  sbi IO(PORTA), 2  $  out IO(PORTC), r0  $  cbi IO(PORTA), 3  $  rjmp endsetbit
    /* 14: */  sbi IO(PORTA), 3  $  out IO(PORTC), r0  $  cbi IO(PORTA), 4  $  rjmp endsetbit
    /* 15: */  sbi IO(PORTA), 4  $  out IO(PORTC), r0  $  cbi IO(PORTA), 5  $  rjmp endsetbit
    /* 16: */  sbi IO(PORTA), 5  $  out IO(PORTC), r0  $  cbi IO(PORTA), 6  $  rjmp endsetbit
    /* 17: */  sbi IO(PORTA), 6  $  out IO(PORTC), r0  $  cbi IO(PORTA), 7
    /* ===  7 cycles === */
endsetbit:

    /* check for column */
    in r30, COLUMN
    cpi r30, 17*4
    brne return   /* if not in the last column return */

    /* updating time_tick
     *   ---------------- exact would be this:
     *   time_frac += 0xf42;    // 18*sum (interval[]) //
     *   if (time_frac > 15000)  // 12MHz / 8 / 100     //
     *     {
     *       time_tick ++;
     *       time_frac -= 15000;
     *     }
     *
     *   we're approximating 0xf42 / 15000 by  25/96 which fits in 8 bits.
     */

    /* time_frac += 25; if time_frac <= 96 then -= 96 and increment time_tick */
    lds r30, time_frac
    subi r30, -(25)
    sts time_frac, r30
    cpi r30, 96+1
    brlo startframeupdate  /* branch if time_frac <= 96 */
    subi r30, 96
    sts time_frac, r30
    lds r30, time_tick
    lds r31, time_tick+1
    adiw r30, 1
    sts time_tick, r30
    sts time_tick+1, r31

startframeupdate:

    lds r30, frameptr
    lds r31, frameptr+1
    or r30,r31
    lds r30, frameptr    /* restore r30, destroyed by OR */
    breq return          /* branch if frameptr == 0 */
    sts frm, r30         /* frame available, take ownership */
    sts frm+1, r31
    clr r30
    sts frameptr, r30
    sts frameptr+1, r30


return:
    /* epilog */
    pop r30
    out IO(SREG),r30
    pop r31
    pop r30
    pop r0
    reti
    /* ===  13 cycles === */

.endfunc