; PROGRAM OVERVIEW
;
; midi data is recieved over UART1 and buffered in a FIFO buffer.
; these data are then sent over the RF link at 10ms intervals.
; if a packet was not recieved, any new data is concatenated to
; the old data, and both are sent out.  the RF data is given a
; sequence number so redundant data can be removed on the recieve
; side.


; CODE OPTIONS

.equ low_power = 1 ; low_power=1 turns the transmitter off between
; transmissions. low_power=0 keeps it on.  the latter is more stable,
; but consumes more power, and can not be used with phantom power.

.equ keep_alive = 0 ; keep_alive=1 allows keep-alive packets to be
; sent. keep_alive=0 suppresses keep-alive packets to save on
; transmission bandwidth.


.include "m1281def.inc" ; which micro is being used


; FUSES - fuse settings for micro
;
; BOD Level = 2.7V
; JTAG programming ON
; SPI porgramming ON
; Bootloader is OFF
; External Oscillator


; RF unit settings
.equ throttle = $0fa0 ; (4ms) delay time for rf failure to keep power consumption down
; this throttles the transmission rate. delay time in us.
.equ timeout = $3245 ; (13s) continuous rf failure timeout, 1024us per unit
.equ delay = $2710 ; (10ms) delay time in us between data packets
.equ des_addr = $6666 ; place unique destination address here
.equ src_addr = $4444 ; place unique source address here
.equ csma_seed = low(src_addr)*$08 ; random backoff exponent - must be unique,<$0800 
.equ pan_id = $a4c3 ; system pan_id
.equ retries = $0e ; high nibble = frame retries,low nibble = 2x(csma retries)
.equ channel = $33 ; tx/rx channel, $2b -> $3a valid


; INTERRUPT VECTORS
.org 0 ; RESET vector
rjmp start
.org int5addr ; RF UNIT interrupt
rjmp clearirq

.if low_power == 1 ; enable T3 interrupt if in low power mode
.org OC3Aaddr ; T3 compare match
rjmp timer3_int
.endif

.org URXC1addr ; USART1 - Rx Complete
rjmp getdata
.org OC4Aaddr ; T4 compare match
rjmp timer4_int
.org OC5Aaddr ; T5 compare match
rjmp timer5_int


; PIN ASSIGNMENTS:
;
; PD2 = USART1 RX for MIDI in


; REGISTER ASSIGNMENTS:
;
; r0  
; r1  
; r2  failure counter
; r3  
; r4  
; r5  
; r6  
; r7  
; r8  
; r9  
; r10 
; r11 
; r12 
; r13 
; r14 
; r15 sequence byte
; r16 temporary register
; r17 
; r18 data read pointer temp storage
; r19 data read byte counter
; r20 wait timer
; r21 
; r22 
; r23 
; r24 
; r25 
; xl  data write pointer lsb
; xh  data write pointer msb
; yl  data read pointer lsb
; yh  data read pointer msb
; zl  
; zh  

start: ; configure microcontroller registers
; using external 1MHz oscillator from RF unit
; this gives better frequency accuracy for the USARTs
ldi r16,high(RAMEND)
out SPH,r16 ; Set Stack Pointer to top of RAM
ldi r16,low(RAMEND)
out SPL,r16
ldi r16,$17
out ddrb,r16 ; set ss,sclk,mosi,slp_tr as output
ldi r16,$08
out portb,r16 ; enable pullup for miso
ldi r16,$01 ; spi to 2x speed
out spsr,r16 ; set up spi
ldi r16,$50 ; spi on,msb first,master,mode 0,ck=f/2=500kbps
out spcr,r16 ; set up spi
sbi portb,portb0 ; set ss pin high
cbi portb,portb4 ; set slp_tr low
sbi ddra,dda7 ; set reset pin as output
cbi porta,porta7 ; initiate a reset of rf unit
nop ; wait appropriate hold time (>625ns)
sbi porta,porta7 ; pull reset high on rf unit
ldi r16,$0c
sts eicrb,r16 ; configure int5 for rising edge
ldi r16,$20
out eimsk,r16 ; enable int5
clr r2 ; initialize failure counter

;wait for rf unit to stabilize
ldi r20,$80 ; load the wait timer to 128 cycles (x10/ck)
rcall wait2 ; wait ~(1280/1MHz = 1.2ms) for rf unit to stabilize

;go to trx_off state
rcall txoff ; go to trx_off state on rf unit

;setup clkm - only driver strength changed
ldi r16,$c3 ; load first data byte - write trx_ctrl_0 register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,$01 ; load second data byte - lowest driver strength, clkm = 1MHz
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high
nop ; idle for a bit as clock changes
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
nop
ldi r21,$ff ; load wait timer to 255 cycles (x10/ck)
rcall wait2 ; wait ~2.5ms to make sure clock is stable

;setup power reduction registers
ldi r16,$eb
sts prr0,r16 ; turn off adc,usart0,t0,t1,t2,twi

.if low_power == 0 ; turn peripherals if not in low power mode
ldi r16,$0e
sts prr1,r16 ; turn off t3,usart3,usart2
.elif low_power == 1 ; leave t3 on if in low power mode 
ldi r16,$06
sts prr1,r16 ; turn off usart3,usart2
.endif

ldi r16,$90
out acsr,r16 ; turn off comparator


;configure rf unit registers

;setup short_addr_0
ldi r16,$e0 ; load first data byte - write short_addr_0 register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,low(src_addr) ; load second data byte - src_addr lsb
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high

;setup short_addr_1
ldi r16,$e1 ; load first data byte - write short_addr_1 register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,high(src_addr) ; load second data byte - src_addr msb
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high

;setup pan_id_0
ldi r16,$e2 ; load first data byte - write pan_id_0 register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,low(pan_id) ; load second data byte - pan_id lsb
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high

;setup pan_id_1
ldi r16,$e3 ; load first data byte - write pan_id_1 register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,high(pan_id) ; load second data byte - pan_id msb
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high

;setup phy_cc_ca - set channel id
ldi r16,$c8 ; load byte - write phy_cc_ca register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,channel ; load data byte - 
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high

;setup phy_tx_pwr - turn crc on
ldi r16,$c5 ; load byte - write phy_tx_pwr register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,$80 ; load data byte - auto_crc=on,pwr=+3dbm
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high

;setup xah_ctrl - set frame and csma retries
ldi r16,$ec ; load byte - write xah_ctrl register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,retries ; load data byte - frame and csma retries
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high

;setup csma_seed_1
ldi r16,$ee ; load byte - write csma_seed_1 register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,high(csma_seed) ; load data byte - min_be=0,aack_set=0,i_am_coord=0,csma(10:8)
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high

;setup csma_seed_0
ldi r16,$ed ; load first data byte - write csma_seed_1 register
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send data
ldi r16,low(csma_seed) ; load data byte - csma(7:0)
rcall spiwrite ; send data
sbi portb,portb0 ; pull ss high

;clear pending irqs on rf unit 
ldi r16,$8f ; load first data byte - read register irq_status command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte and clear pending irqs
sbi portb,portb0 ; pull ss high

;enable irqs on rf unit
ldi r16,$ce ; load first data byte - write register irq_mask command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$08 ; load second data byte - trx_end only
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high


;setup usart1 - assumes rest of registers are in default mode
ldi r16,$98
sts ucsr1b,r16 ; turn on usart1 rx, enable rx interrupt
ldi r16,$01
sts ubrr1l,r16 ; set usart1 to f/32=31.25kbps
ldi xh,$02 ;  set data write pointer to bottom of sram buffer
clr xl
movw y,x ; set data read pointer to same point as write

;setup t5 for RF packet delay interrupt
clr r16
sts tccr5b,r16 ; make sure counter is off
sts tcnt5h,r16 ; clear counter
sts tcnt5l,r16 ; clear counter
ldi r16,high(delay)
sts ocr5ah,r16 ; set msb of delay time
ldi r16,low(delay)
sts ocr5al,r16 ; set lsb of delay time
ldi r16,$02
sts tifr5,r16 ; clear any flags
sts timsk5,r16 ; enable t5 interrupt

;setup t4 for RF failure timeout interrupt
clr r16
sts tccr4b,r16 ; make sure counter is off
sts tcnt4h,r16 ; clear counter
sts tcnt4l,r16 ; clear counter
ldi r16,high(timeout)
sts ocr4ah,r16 ; set msb of timeout time
ldi r16,low(timeout)
sts ocr4al,r16 ; set lsb of timeout time

.if low_power == 1 ; setup T3 if in low power mode
; delays packet transmissions to throttle power consumption
clr r16
sts tccr3b,r16 ; make sure counter is off
sts tcnt3h,r16 ; clear counter
sts tcnt3l,r16 ; clear counter
ldi r16,high(throttle)
sts ocr3ah,r16 ; set msb of timeout time
ldi r16,low(throttle)
sts ocr3al,r16 ; set lsb of timeout time
.endif

;clear pending irqs on micro
ser r16
out eifr,r16 ; clear any interrupt flags that are set

;enable interrupts on micro
sei ; turn on interrupts

.if low_power == 0 ; turn on transmitter if not in low power mode
rcall txonaret ; put the rf unit in tx_aret_on state
.endif

idle: ; do nothing while waiting for data

nop
nop
nop
nop
rjmp idle


.if low_power == 1 ; enable T3 interrupt if in low power mode

timer3_int: ; interrupt routing for idle timer

push r16 ; save temp register
in r16,sreg ; get sreg
push r16 ; push sreg on stack
rcall txonaret ; turn on transmitter
mov yl,r18 ; restore data read pointer
dec r15 ; reset sequence number
rcall transmitpacket ; resend data, including any new data
clr r16
sts tccr5b,r16 ; turn off delay counter
sts tccr3b,r16 ; turn off throttle counter
out tifr5,r16 ; clear timer interrupt flag if set
pop r16 ; get sreg off stack
out sreg,r16 ; return sreg
pop r16 ; return temporary register
reti ; return to task

.endif


timer4_int: ; interrupt routine for timeout timer
; turns off RF unit if no reciever is present
; board requires reset to turn back on
push r16 ; save temp register
in r16,sreg ; get sreg
push r16 ; push sreg on stack

rcall forcetxoff ; turn off transmitter

;turn off the peripherals
clr r16
sts ucsr1b,r16 ; turn off rx usart
sts timsk5,r16 ; turn off delay timer
sts tccr5b,r16
sts timsk4,r16 ; turn off timeout timer
sts tccr4b,r16

.if low_power == 1 ; turn off T3 if in low power mode
sts timsk3,r16 ; turn off idle timer
sts tccr3b,r16
.endif

;clear any timer interrupt flags set
ldi r16,$02
out tifr5,r16 ; clear compare match interrupt flag
out tifr4,r16

.if low_power == 1 ; clear T3 if in low power mode
out tifr3,r16
.endif

;finish off
pop r16 ; get sreg off stack
out sreg,r16 ; return sreg
pop r16 ; return temporary register
reti ; return to task


timer5_int: ; interrupt routine for delay timer
; sends out packetized data at a fixed delay time
push r16 ; save temp register
in r16,sreg ; get sreg
push r16 ; push sreg on stack
clr r16
sts tccr5b,r16 ; turn off counter
sts timsk5,r16 ; turn off interrupt to block during transmission

.if low_power == 1 ; turn on transmitter if in low power mode
rcall txonaret ; turn on transmitter
.endif

rcall transmitpacket ; send data packet
pop r16 ; get sreg off stack
out sreg,r16 ; return sreg
pop r16 ; return temporary register
reti ; return to task


;RF UNIT COMMANDS

txoff: ; send trx_off command to rf unit and wait till stable

ldi r16,$c2 ; load first data byte - write register trx_state command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$08 ; load second data byte - data trx_off bit
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high

checktxoff: ; check current state to see if in tx_off state

ldi r16,$81 ; load first data byte - read register trx_status command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$00 ; load second data byte - blank for reading
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high
cpi r16,$08
brne checktxoff
ret ; return to previous duty

forcetxoff: ; send trx_off command to rf unit and wait till stable

ldi r16,$c2 ; load first data byte - write register trx_state command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$03 ; load second data byte - data force_trx_off bit
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high

checkforcetxoff: ; check current state to see if in tx_off state

nop ; delay for a bit to make sure the device is off
nop
nop
nop
ldi r16,$81 ; load first data byte - read register trx_status command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$00 ; load second data byte - blank for reading
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high
cpi r16,$08
brne forcetxoff ; redo if not off - only takes 1us to turn off (changed due to lock up bug of getting stuck in check loop)
ret ; return to previous duty

pllon: ; send pll_on command to rf unit

ldi r16,$c2 ; load first data byte - write register trx_state command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$09 ; load second data byte - data pll_on bit
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high

checkpllon: ; check current state to see if in pll_on state

ldi r16,$81 ; load first data byte - read register trx_status command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$00 ; load second data byte - blank for reading
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high
cpi r16,$09
brne checkpllon
ret ; return to previous duty

txonaret: ; send trx_aret_on command to rf unit and wait till stable

ldi r16,$c2 ; load first data byte - write register trx_state command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$19 ; load second data byte - data trx_aret_on bit
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high

checktxonaret: ; check current state to see if in tx_aret_on state

ldi r16,$81 ; load first data byte - read register trx_status command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$00 ; load second data byte - blank for reading
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high
cpi r16,$19
brne checktxonaret
ret ; return to previous duty

rxonaack: ; send rx_aack_on command to rf unit and wait till stable

ldi r16,$c2 ; load first data byte - write register trx_state command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$16 ; load second data byte - data rx_aack_on bit
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high

checkrxonaack: ; check current state to see if in rx_aack_on state

ldi r16,$81 ; load first data byte - read register trx_status command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$00 ; load second data byte - blank for reading
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high
cpi r16,$16
brne checkrxonaack
ret ; return to previous duty


;GENERAL FUNCTIONS

spiwrite: ; read and write data over spi

out spdr,r16 ; write transmitted byte to spi

wait: ; read spi register to check when done

in r16,spsr ; get spi status register
sbrs r16,spif ; check if done
rjmp wait ; keep checking if not done
in r16,spdr ; save recieved byte
ret ; return to previous duty

wait2: ; wait timer (~12 cylces per value in r20)

nop
nop
nop
nop
nop
nop
nop
nop
dec r20
brne wait2
ret ; return to previous duty


;RF UNIT INTERRUPT ROUTINES

clearirq: ; clear irq register and irq line

push r16 ; save temp register
in r16,sreg ; get sreg
push r16 ; push sreg on stack
ldi r16,$8f ; load first data byte - read register irq_status command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high

checkrf: ; check rf unit state

ldi r16,$81 ; load first data byte - read register trx_status command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$00 ; load second data byte - blank for reading
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high
cpi r16,$1f ; check if in state transition
breq checkrf ; keep checking till in a new state
cpi r16,$12 ; check if in busy_tx state
breq checkrf ; keep checking till in a new state
cpi r16,$19 ; check if in tx_aret_on state
brne wrongstate ; change back to tx_aret if not
;else check successful transmit
ldi r16,$82 ; load data byte - read trx_state to get trac_status
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte
ldi r16,$00 ; load data byte - blank byte for reading
rcall spiwrite ; send byte
sbi portb,portb0 ; pull ss high
andi r16,$e0 ; mask off trac_status
cpi r16,$30 ; check if success or data_pending success
brlo success ; skip if success
;else deal with failure

;failure routine
sbrc r2,$00 ; check if first failure
rjmp failure2 ; wait to send if not first failure
inc r2 ; set r2 to indicate first failure
mov yl,r18 ; restore data read pointer
dec r15 ; reset sequence number
rcall transmitpacket ; resend data, including any new data
clr r16
sts tccr5b,r16 ; turn off timer counter
ldi r16,$02
out tifr5,r16 ; clear timer interrupt flag if set

;setup timeout timer
clr r16
sts tcnt4h,r16 ; clear the counter
sts tcnt4l,r16
ldi r16,$05 ; turn on timeout timer
sts tccr4b,r16 ; set to ck/1024
ldi r16,$02
out tifr4,r16 ; clear any interrupt flags
sts timsk4,r16 ; turn on t4 compare match interrupt

;setup throttle timer
.if low_power == 1 ; throttle data rate if in low power mode
clr r16
sts tcnt3h,r16 ; clear the counter
sts tcnt3l,r16
ldi r16,$01 ; turn on timeout timer
sts tccr3b,r16 ; set to ck/1
ldi r16,$02
out tifr3,r16 ; clear any interrupt flags
sts timsk3,r16 ; turn on t3 compare match interrupt
.endif

rjmp irqdone ;  finish off


failure2: ; turn off and wait to transmit

.if low_power == 1 ; turn off transmitter if in low power mode
rcall forcetxoff ; turn off transmitter
clr r16
sts tcnt3h,r16 ; clear the counter
sts tcnt3l,r16
ldi r16,$01 ; turn on timeout timer
sts tccr3b,r16 ; set to ck/1
rjmp irqdone ;  finish off

.elif low_power == 0 ; continue to transmit if not in low power mode
mov yl,r18 ; restore data read pointer
dec r15 ; reset sequence number
rcall transmitpacket ; resend data, including any new data
clr r16
sts tccr5b,r16 ; turn off timer counter
ldi r16,$02
out tifr5,r16 ; clear timer interrupt flag if set
rjmp irqdone ; finish off
.endif


wrongstate: ; change state in case of error

rcall forcetxoff ; switch to a known state

.if low_power == 0 ; turn on transmitter if not in low power mode
rcall txonaret ; put device back to transmit
.endif

rjmp irqdone ; finish off

success: ; tx success

sbrs r2,$00 ; check if previous tx was a failure
rjmp success1 ; skip counter shut off
clr r2 ; reset failure counter
sts tccr4b,r2 ; turn off timeout timer
sts timsk4,r2 ; turn off timeout interrupt

.if low_power == 1 ; turn off throttle timer if in low power mode
sts tccr3b,r2 ; turn off throttle timer
sts timsk3,r2 ; turn off throttle interrupt
.endif

success1: ; reset for next transmission

ldi r16,$02
sts timsk5,r16 ; enable t5 interrupt

.if low_power == 1 ; turn off transmitter if in low power mode
rcall forcetxoff ; turn off transmitter
.endif


irqdone: ; finish interrupt and return

pop r16 ; get off stack
out sreg,r16 ; return sreg
pop r16 ; return temporary register
reti ; clear interrupt and return to previous duty


;RF UNIT TRANSMISSION ROUTINES

transmitpacket: ; transmit a packet

sbi portb,portb4 ; pull slp_tr high to begin transmit
nop ; delay to make sure the pin is high long enough
cbi portb,portb4 ; pull slp_tr low

ldi r16,$60 ; load data byte - write buffer command
cbi portb,portb0 ; pull ss low
rcall spiwrite ; send byte

;calculate packet size
mov r18,yl ; store read register
mov r19,xl ; store write register
sub r19,r18 ; find number of packets to send
ldi r16,$0b ; prepare frame length byte
add r16,r19 ; total data bytes +11

;send out transmission
rcall spiwrite ; send byte - frame length
ldi r16,$61 ; load fcf(7:0) byte - $61 for data packet
rcall spiwrite ; send byte
ldi r16,$88 ; load fcf(15:8) byte - $88 for data packet
rcall spiwrite ; send byte
inc r15 ; increment sequence byte
mov r16,r15 ; load sequence byte
rcall spiwrite ; send byte
ldi r16,low(pan_id) ; load destination panid(7:0) byte
rcall spiwrite ; send byte
ldi r16,high(pan_id) ; load destination panid(15:8) byte
rcall spiwrite ; send byte
ldi r16,low(des_addr) ; load destination address(7:0) byte
rcall spiwrite ; send byte
ldi r16,high(des_addr) ; load destination address(15:8) byte
rcall spiwrite ; send byte
ldi r16,low(src_addr) ; load source address(7:0) byte
rcall spiwrite ; send byte
ldi r16,high(src_addr) ; load source address(15:8) byte
rcall spiwrite ; send byte

dataload: ; load data to rf unit

ld r16,y+ ; load data byte - data byte
andi yl,$3f ; mask off lower 64B to keep fifo within RF transmit buffer size
;andi yh,$02 ; mask off data pointer to stay in lower 256
rcall spiwrite ; send byte
dec r19 ; check byte in sequence
brne dataload ; branch if not last byte

;finish off transmission
sbi portb,portb0 ; pull ss high
ret ; return to previous duty


;USART RX ROUTINE

getdata: ; process incoming bytes on usart1

push r16 ; store temporary register
in r16,sreg ; get sreg
push r16 ;  push sreg on stack
lds r16,udr1 ; load recieved byte

.if keep_alive == 0 ; remove unwanted packets if desired
cpi r16,$fe ; check for keep-alive packet
breq getdatadone ; finish off
.endif

;else continue to store data
st x+,r16 ; store recieved byte to sram
andi xl,$3f ; mask off lower 64B to keep fifo within RF transmit buffer size
;andi xh,$02 ; mask off top byte to keep fifo at lower 256

;check if timer already set
lds r16,tccr5b ; get t5 control register
andi r16,$07 ; mask off clock select bits
cpi r16,$01 ; check if clock is off
brsh getdatadone ; finish off if t5 running

;else turn on t5 to start data delay aggregation
clr r16 ; reset t5 counter
sts tcnt5h,r16
sts tcnt5l,r16
ldi r16,$01
sts tccr5b,r16 ; turn on t5, normal mode, f=ck/1

getdatadone: ; finish off

pop r16 ; get sreg off stack
out sreg,r16 ; return sreg
pop r16 ; restore temporary register
reti ; return to previous task






