;*********************BIOSTIMULATOR 1.0 FALL 1999 Prof.Land and Brian Wong *****************************

.include "c:\biostim\4414def.inc"

.device AT90S4414 ; specifies to the assembler which chip we are using

.def	temp	=r0	;temporary register
.def    save    =r1     ;SREG temp reg
.def	inthi	=r2	;hi8 for interval
.def	intlo	=r3	;lo8 for interval
.def	del1hi	=r4	;hi8 for delay1
.def	del1lo	=r5	;lo8 for delay1
.def	del2hi	=r6	;hi8 for delay2
.def	del2lo	=r7	;lo8 for delay2
.def	durhi	=r8	;hi8 for duration
.def	durlo	=r9	;lo8 for duration
.def	add2l	=r10	;u16bit add/cmp
.def	beats	=r11	;number of stims

.def	add1l	=r12	;addlow1
.def	add1h	=r13	;addhi1
.def	add2h	=r14	;addhi2
.def	extrl	=r15	;extra stuff... use carefully

.def 	mc16uL	=r16	;multiply low1
.def 	mc16uH	=r17	;multiply high1
.def 	status	=r18	;unit op stauts
.def 	m16u1	=r19	;result byte 1

.def	RXchar	=r20	;new character 
.def	mp16uH	=r21	;multiply high2
.def	TXbusy	=r22	;transmit busy flag
.def	m16u3	=r23	;result byte 3
.def	m16u2	=r24	;result byte 2

.def	m16u0	=r25	;result byte 0 (LSB)
.def	mp16uL	=r26	;multiply lo2
.def	mcnt16u	=r27	;loo0p counter
.def	newchar	=r28	;new char rx flag
.def	TXflash	=r29	;text to be sent is in flash if <>0
	


.def 	wreg	=r30	;another temp reg
.def    templo  =r31    ;temporary register


 
;***** Other equates 
.equ	baud96	=25	;9600 baud constant for 4Mhz crystal
.equ	beats	='n' 	; ascii 'N'
.equ	inter	='i'	; ascii 'I'
.equ	dur	='d'	; ascii 'D'
.equ	del1	='s'	; ascii 's'
.equ	del2	='p'	; ascii 'P'
.equ	musi	='m'	; ascii m  
.equ	go	='g' 	;0x67 ascii 'G' go 
.equ	stop	='x'	;ascii 'x' stop
.equ	return	='='	; number entry



.dseg			;memory addresses
betmem:	.byte	7	;beats memory 7 bytes format (zero,ascii*5,zero)
intmem:	.byte	7	;interval memory 7 bytes
del1mem:.byte	7	;delay 1 memory 7 bytes
del2mem:.byte	7	;delay 2 memory 7 bytes
durmem:	.byte	7	;duration memory 7 bytes


;***** Initialization

.cseg
.org $0000
        rjmp RESET      ;reset entry vector
        reti		;IRQ0
	reti		;irq1
	reti		;timer1 capture
	rjmp intover	;timer1 comparea handler
        reti		;timer1 compareb handler
        reti		;timer1 overflow
	rjmp synctic	;timer0 overflow
	reti		;SPI handler
	rjmp	RXdone	;UART receive done
	rjmp	TXempty	;UART buffer empty	
	rjmp 	TXdone	;UART transmit done
	reti		;ADC




;define fixed strings to be tranmitted from flash- zero terminated
	
	notes:	.db	"All times are entered in ***100 X MICROseconds***.",0x00
	text:	.db	"N)um pulses? ",0x00 		;messages to be displayed
	txt2:	.db	"I)nterval? ", 0x00
	txt3:	.db	"D)uration? ",0x00
	txt4:	.db	"S)ync Delay? ", 0x00
	txt5:	.db	"P)ulse delay? ", 0x00
	txt6:	.db	"M)ulti press'2' single press'1')? ", 0x00
	txt7:	.db	"Single", 0x00
	txt8:	.db	"Multi",0x00
	txt9:	.db	"Running...Press 'x' to stop", 0x00
	txt10:	.db	"Stopped",0x00
	txt11:	.db	"Multiburst",0x00
	txt12:	.db	"Single",0x00
	crlf:	.db	0x0d, 0x0a, 0x00	;carrage return/line feed




;;;********************BOOT MICRO********************



RESET:  ldi     templo, LOW(RAMEND) 	;setup stack pointer
        out     SPL, templo			;for uart input
        ldi     templo, HIGH(RAMEND)
        out     SPH, templo

	ldi	templo,0b00001000	;makes sure cloks are off on startup or reset
	out	TCCR1B,templo		;shut off 16 bit clok
	ldi	templo,0b00000000
	out	TCCR0,templo		;shut off tick clok


;setup UART -- enable TXempty & RXdone int, and RX, TX pins	

	ldi 	templo, 0b10111000	
	out 	UCR, templo	;set baud rate to 9600	
	ldi	templo, baud96
	out	UBRR, templo
	

	sei		; leave this sei in here
	ser	templo		;can debug on portc if so desired
	out	DDRC,templo
	;ldi	status, 0b00000000

	

;**************************LOAD IN DEFAULTS**************************
	rcall	carr		;carriage return
	rcall	carr
	rcall	nots		;headers
	rcall	carr
	rcall	carr
	clr	del2lo
	clr 	del2hi
	ldi	m16u0,0x01
	mov	beats,m16u0	;load in value to beats (1)
	ldi	ZL, LOW(betmem)  ;ptr to beats RAM	ldi	ZH, HIGH(betmem)
	st	Z+,del2lo	;load in ascii values into RAM, primary zero
	ori	m16u0,0x30	;ascii 1
	st	Z+,m16u0	;
	st	Z+,del2lo	;trailing zero

	ldi	m16u0,0x64	;load value to interval (100)
	mov	intlo,m16u0	
	clr	inthi
	ldi	ZL, LOW(intmem)  ;ptr to interval RAM	ldi	ZH, HIGH(intmem)
	st	Z+,del2lo		;primary zero
	ldi	m16u0,0x31
	;ori	m16u0,0x30		;one, remember to stick on a 0x30 to conform to ascii
	st	Z+,m16u0
	ldi	m16u0,0x30
	;ori	m16u0,0x30
	st	Z+,m16u0		;zero
	ldi	m16u0,0x30
	;ori	m16u0,0x30
	st	Z+,m16u0		;zero
	st	Z+,del2lo		;trailing zero
	

	ldi	m16u0,50
	mov	durlo,m16u0
	clr	durhi
	ldi	ZL, LOW(durmem)  ;ptr to RAm for duration	ldi	ZH, HIGH(durmem)
	st	Z+,del2lo
	ldi	m16u0,0x35
	;ori	m16u0,0x30		;5
	st	Z+,m16u0
	ldi	m16u0,0x30
	;ori	m16u0,0x30
	st	Z+,m16u0		;0
	st	Z+,del2lo

	clr	del1lo
	clr	del1hi
	ldi	ZL, LOW(del1mem)  ;ptr to RAM	delay1	ldi	ZH, HIGH(del1mem)
	st	Z+,del2lo
	ldi	m16u0,0x30
	;ori	m16u0,0x30		;0
	st	Z+,m16u0
	st	Z+,del2lo

	
	ldi	ZL, LOW(del2mem)  ;ptr to RAM	delay 2	ldi	ZH, HIGH(del2mem)
	st	Z+,del2lo
	ldi	m16u0,0x30
	;ori	m16u0,0x30		;0
	st	Z+,m16u0
	st	Z+,del2lo

	ldi	status,0x01;

		
	
	;ldi	ZL, LOW(del2mem)  ;ptr to RAM	;ldi	ZH, HIGH(del2mem)

;;;********************LIST PARAMETERS****************************************

dingle:	rcall	carr
	rcall 	carr			;list options
	rcall 	betreq			;beats
	ldi	ZL, LOW(betmem)  ;ptr to RAM to read off numbers in memory
	ldi	ZH, HIGH(betmem)
	rcall 	rmwr
	rcall 	carr			;carr return
	rcall 	ireq			;interval
	ldi	ZL, LOW(intmem)  ;ptr to RAM	ldi	ZH, HIGH(intmem)
	rcall 	rmwr
	rcall	carr
	rcall	durreq			;duration
	ldi	ZL, LOW(durmem)  ;ptr to RAM	ldi	ZH, HIGH(durmem)
	rcall 	rmwr
	rcall 	carr
	rcall	del1req			;delay 1
	ldi	ZL, LOW(del1mem)  ;ptr to RAM	ldi	ZH, HIGH(del1mem)
	rcall 	rmwr
	rcall 	carr
	rcall	del2req			;delay 2
	ldi	ZL, LOW(del2mem)  ;ptr to RAM	ldi	ZH, HIGH(del2mem)
	rcall 	rmwr
	rcall	carr	
	rcall	multi
	rcall	carr
	rcall	carr
	

estall:	clr	mp16uH		;key press menu
	cpi 	RXchar, go	;wait for go signal - a 'g' on the keyboard
	brne	ha		;go for it
	rjmp	EEload
ha:	cpi	RXchar, beats	;look for beat input
	breq	bstr
	cpi	RXchar, inter	;look for interval input
	breq	istr
	cpi	RXchar, dur	;look for duration input
	breq	dstr
	cpi	RXchar, del1	;look for delay 1 input
	breq	d1str
	cpi	RXchar, del2	;look for delay 2input
	breq	d2str
	cpi	RXchar, musi	;look for multiple/single input
	breq	mulstr	
	out	PORTC, intlo	;debug code....safety blanket
	rjmp	estall

bstr:	rcall 	betreq			;what to do when beat input
	ldi	templo,4
	mov	extrl,templo		;load tag for parser to figure out memory addresses
	rcall	ripper			;parse inputs
	mov	beats,add1l		;move to correct reg
	rjmp	dingle			;redisplay parameters
	
istr:	rcall	ireq			;what to do when interval input
	ldi	templo,3
	mov	extrl,templo
	rcall	ripper			;parse
	mov	intlo,add1l		;move to correct regs
	mov	inthi,add1h
	rjmp	dingle
	
dstr:	rcall	durreq			;what to do when duration inputs
	ldi	templo,2
	mov	extrl,templo
	rcall	ripper
	mov	durlo,add1l
	mov	durhi,add1h
	rjmp	dingle
	
d1str:	rcall	del1req			;what to do when delayy1 inputs
	ldi	templo,1
	mov	extrl,templo
	rcall	ripper
	mov	del1lo,add1l
	mov	del1hi,add1h
	rjmp	dingle
	
d2str:	rcall	del2req			;what to do whith delay 2 inputs
	ldi	templo,0
	mov	extrl,templo
	rcall	ripper
	mov	del2lo,add1l
	mov	del2hi,add1h
	rjmp	dingle
	
mulstr: rcall	multi			;what to do with multi/single inputs
	ser 	newchar
cat:	tst	newchar
	brne	cat
	cpi	RXchar, 0x31 	; stick in a '1'
	breq	sin
	cpi	RXchar, 0x32	;stick in  '2'
	breq	many
sin:	ldi	status,0x00	;set status to single fire
	rcall	sf
	rjmp	dingle
many:	ldi	status,0x01;	set status to multiburst
	rcall	mf
	rjmp	dingle
	
rmwr:	ld	r0,Z	
	out	UDR, r0		;fire off the UART transmit
	clr	TXflash		;the string is in RAM
	ser	TXbusy		;and set the TX busy flag
	sbi	UCR, UDRIE	;enable the TXempty interrupt
	rcall	TXwait
	ret


RESET1:	ldi 	templo, 0b10111000	
	out 	UCR, templo		;inserted because relative jump
	ldi	templo,0b00001000	;stop cloks when stop command given
	out	TCCR1B,templo		;stop 16 bit clok
	ldi	templo,0b00000000
	out	TCCR0,templo		;stop 8 bit tick clock
	rcall	carr
	rcall	stp			; give stop message
	rcall	carr
	rjmp	dingle			;redisplay parameters


;;****************************START CLOCK CORE
EEload:	rcall	ruti
	ldi 	templo, 0b10010000	;turn of uart transmit so can detect stop signal
	out 	UCR, templo	
	ldi	status, 0b00000000

	sei				;leave this in
	ldi	templo,206		;fill 100u clock
	out	TCNT0,templo		;
	ldi	templo,0b00010000	;set pulse edges
	out	TCCR1A,templo
	mov	mcnt16u,beats		;prefill beat counter
	out	OCR1AH,inthi		;prefill interval capture
	out	OCR1AL,intlo
	;ldi	templo,0b11111111	;output test pins D
	;out	DDRD,templo		;setup output pins
	ldi	templo,0b11111101
	out	DDRB,templo		;setup output pins
	ldi	templo,0b01000010
	out	TIMSK,templo

lpst:	cpi 	RXchar, stop		;wait for go signal - a 'x' on the keyboard
	breq	RESET1
	
	ldi	status, 0b00000000
	tst	del1hi			;check to see if sync and first
	brne	burst			;edge are together		
	tst	del1lo
	brne	burst
	sbr	status,0x10		;has zero=zero condition

burst:	sbrc	status,0		;burst or single: check status reg
	rjmp	noz
	


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;



puex:	ldi	templo,0b00000000
	out	TCCR0,templo
	sbis	PINA,5			;pulse /external hold state
	rjmp	noz			;waits for the external input
	sbic	PINA,6
	rjmp	noz
	rjmp 	puex

noz:	clr	add1h			;check for zero primary delay
	clr	add1l
	cbr	status,0b10000000
	sbrc	status,4
	rjmp	zon

	mov	add2h,del1hi		;delay 1 preload
	mov	add2l,del1lo
	ldi	templo,0b00000100
	out 	PORTB,templo
	sbr	status,0b00000010
	rjmp	doggie

zon:	mov	add2h,durhi		;zero delay / sync start
	mov	add2l,durlo
	ldi	templo,0b00001100
	out 	PORTB,templo
	cbr	status,0b00000010

doggie:	ldi	templo,0b00001111	;start the clocks
	out	TCCR1B,templo
	ldi	templo,0b00000010
	out	TCCR0,templo
	cbi	PORTB,2
	;cbr	status,0b00000010


nozl:	rcall	add16			;timer core DO NOT TOUCH..This means you
wait:	in	add2l,TCNT1L		;Brian
	in	add2h,TCNT1H
	rcall	cp16
	brne	wait
	
	sbic	PINB,3
	rjmp	on
	sbi	PORTB,3
	rjmp	doff
on:	cbi	PORTB,3			;compare until ready to stop
doff:	sbrc	status,1
	breq	durs
	mov	add2h,del2hi		;add for new target
	mov	add2l,del2lo
	ori	status,0b00000010
	dec	mcnt16u
	tst	mcnt16u
	breq	beatend
	rjmp	nozl

durs:	mov	add2h,durhi
	mov	add2l,durlo
	cbr	status,0b00000010
	rjmp	nozl
	
beatend: sbrs status,7
	 rjmp	beatend
	 rjmp	lpst


;******************** labels*******************



nots:	ldi	ZL, LOW(notes<<1)  ;shifted becuase pgm memory is words	
	ldi	ZH, HIGH(notes<<1) ;multiple/single label
	rcall	prompt
	ret

multi:	ldi	ZL, LOW(txt6<<1)  ;shifted becuase pgm memory is words	
	ldi	ZH, HIGH(txt6<<1) ;multiple/single label
	rcall	prompt
	ret

betreq: ;set up the transmit pointer for the first message	
	ldi	ZL, LOW(text<<1)  ;shifted becuase pgm memory is words	
	ldi	ZH, HIGH(text<<1) ;beat request
	rcall	prompt
	ret	

ireq:	;set up the transmit pointer for the first message	
	ldi	ZL, LOW(txt2<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt2<<1) ;interval request
	rcall	prompt
	ret
	
durreq:	sbr	status, 0b00001000
	;set up the transmit pointer for the first message	
	ldi	ZL, LOW(txt3<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt3<<1) ;duration request
	rcall	prompt
	ret	

del1req:;set up the transmit pointer for the first message
	ldi	ZL, LOW(txt4<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt4<<1) ;delay1 request
	rcall	prompt
	ret	

del2req:;set up the transmit pointer for the first message
	ldi	ZL, LOW(txt5<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt5<<1) ;delay 2 request
	rcall	prompt
	ret	

sing:	ldi	ZL, LOW(txt7<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt7<<1) ;delay 2 request
	rcall	prompt
	ret	


fre:	ldi	ZL, LOW(txt8<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt8<<1) ;delay 2 request
	rcall	prompt
	ret	

ruti:	ldi	ZL, LOW(txt9<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt9<<1) ;delay 2 request
	rcall	prompt
	ret

stp:	ldi	ZL, LOW(txt10<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt10<<1) ;delay 2 request
	rcall	prompt
	ret

mf:	ldi	ZL, LOW(txt11<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt11<<1) ;delay 2 request
	rcall	prompt
	ret

sf:	ldi	ZL, LOW(txt12<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(txt12<<1) ;delay 2 request
	rcall	prompt
	ret

carr:	;setup ptr for <crlf> string
	ldi	ZL, LOW(crlf<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(crlf<<1)	; carrage return 
	rcall prompt
	ret



;******************subroutines

prompt:	lpm			  ;put the char in r0
	out	UDR, r0		;put the character in the UART buffer	
	ser	TXflash		;string is in flash memory
	ser	TXbusy		;and set the TX busy flag
	sbi	UCR, UDRIE	;enable the TXempty interrupt
	rcall	TXwait		;chill until done
	ret




;*************************RIPPER/ PARSER subroutine******************************

ripper:	ldi	templo,0b00001111	;parse those numbers
	mov	temp,templo	;save copy of stack pointer to flip stack
	in 	templo,SPH	;starting point
	in 	wreg,SPL
	mov	add1l,wreg	;saved stack pointer
	mov	add1h,templo
	clr 	add2h
	ldi	m16u0,1
	mov	add2l,m16u0
	rcall	add16		;add 1 to align stack correclty
	mov	wreg,add1l
	mov	templo,add1h
	ldi	mcnt16u,5	;prep	digit counter
	
		
	ser	newchar		;rip routine
	rcall	newcw		;wait for new numbers
noret:	subi	mcnt16u,1	;parse those numbers as the come in
	out	UDR,RXchar
	push	RXchar		;push into stack
	ser	newchar		
	rcall	newcw
	cpi	RXchar,return
	brne	noret
	
	ldi	m16u3,4		;figure out what memory addreses to use
	cp	extrl,m16u3	;to write into RAM
	breq	a1		
	ldi	m16u3,3
	cp	extrl,m16u3
	breq	a2
	ldi	m16u3,2
	cp	extrl,m16u3
	breq	a3
	ldi	m16u3,1
	cp	extrl,m16u3
	breq	a4
	tst	extrl
	breq	a5
	rjmp	dingle

a1:	ldi	YL, LOW(betmem)  ;ptr to RAM	now actually choose the right address
	ldi	YH, HIGH(betmem) 
	rjmp	parser
a2:	ldi	YL, LOW(intmem)  ;ptr to RAM
	ldi	YH, HIGH(intmem) 
	rjmp	parser
a3:	ldi	YL, LOW(durmem)  ;ptr to RAM
	ldi	YH, HIGH(durmem) 
	rjmp	parser
a4:	ldi	YL, LOW(del1mem)  ;ptr to RAM
	ldi	YH, HIGH(del1mem) 
	rjmp	parser
a5:	ldi	YL, LOW(del2mem)  ;ptr to RAM
	ldi	YH, HIGH(del2mem) 
	rjmp	parser
	
parser:	ldi	mp16uL,0		;lump in preceeding zero for RAM format
	st	Y+,mp16uL
	clr	add1l			;prep for add
	clr	add1h			;count how many were used
	clr	mp16uH			;flip from stack accordingly
	clr	extrl
	cpi	mcnt16u,4		;pick right place to start 
	breq	z
	cpi	mcnt16u,3
	breq	tens
	cpi	mcnt16u,2
	breq	hunds
	cpi	mcnt16u,1
	breq	thou
	tst	mcnt16u
	breq	a10k
	rjmp	estall


z:	rjmp	ones


a10k:	mov	status,mcnt16u		;do it for 10k
	ld	mp16uL,-Z		; read from stack.... remember 0xFF is the LOWEST part of the stack
	st	Y+,mp16uL		;store nums in ram
	and	mp16uL,temp
	ldi	RXchar,HIGH(10000)	;parse from ascii to binary
	mov	mc16uH,RXchar		;mux to correct place
	ldi	RXchar,low(10000)
	mov	mc16uL,RXchar
	rcall	mpy16u
	mov	add2h,m16u1		;add
	mov	add2l,m16u0
	rcall	add16
	mov	mcnt16u,status

thou:	mov	status,mcnt16u		;do it for 1k
	ld	mp16uL,-Z
	st	Y+,mp16uL		;read from stak
	and	mp16uL,temp		;stor nums in ram
	ldi	RXchar,HIGH(1000)
	mov	mc16uH,RXchar		;mux to correct place	
	ldi	RXchar,LOW(1000)
	mov	mc16uL,RXchar
	rcall	mpy16u
	mov	add2h,m16u1		;add
	mov	add2l,m16u0
	rcall	add16
	mov	mcnt16u,status
	
hunds:	mov	status,mcnt16u
	ld	mc16uL,-Z
	st	Y+,mc16uL
	clr	mc16uH
	and	mc16uL,temp
	ldi	mp16uL,0x64
	clr	mp16uH		; does it for 100
	rcall	mpy16u			;mux to correct place
	mov	add2h,m16u1
	mov	add2l,m16u0
	rcall	add16			;add
	mov	mcnt16u,status


tens:	mov	status,mcnt16u		
	ld	mc16uL,-Z
	st	Y+,mc16uL
	and	mc16uL,temp
	clr	mc16uH
	ldi	mp16uL,0x0a
	clr	mp16uH		; does it for tens
	rcall	mpy16u
	clr	add2h
	mov	add2l,m16u0
	rcall	add16
	mov	mcnt16u,status

ones:	mov	status,mcnt16u		;do it for ones
	ld	mp16uL,-Z
	st	Y+,mp16uL
	and	mp16uL,temp
	clr	add2h
	mov	add2l,mp16uL
	rcall	add16
	ldi	mp16uL,0		;trailing zero in Ram Format
	st	Y+,mp16uL
	

poplp:	cpi	status,5		;pop routine to realign stack
	breq	oute
	pop	extrl
	inc	status
	rjmp	poplp

oute:		ret

;*****************************;interrupt routines
; UART needs a character
TXempty:in	save, SREG	;save processor status
	tst	TXflash		;Is the string in flash memory?
	breq	TXram		;If not, it is in RAM
	inc	ZL		;get the next char from flash
	lpm			;and put it in r0
	rjmp	TXfls
TXram: 	inc	ZL		;get the next char from RAM
	ld	r0,Z
TXfls:	tst	r0		;if char is zero then exit
	breq	TXend
	out 	UDR, r0		;otherwise transmit it
	rjmp	TXexit		;exit until next char
TXend:	clr	TXbusy		;no more chars
	cbi	UCR, UDRIE	;clear the TXempty interrupt
TXexit:	out	SREG, save	;restore proc status
	reti			;back to pgm; TX done -- buffer is empty  -- unused here
TXdone:	in	save, SREG	;save processor status
	out	SREG, save	;restore proc status
	reti			;back to pgm; UART read a character
RXdone:	in	save, SREG	;save processor status
	in	RXchar, UDR	;get the character
	clr	newchar
	out	SREG, save	;restore proc status
	reti			;back to pgm;


;*****************************;subroutine
TXwait:	tst	TXbusy		;now wait for the tranmission to finish
	brne	TXwait
	ret
;***************************

newcw:	tst	newchar		;wait for new stuff to come in
	brne	newcw
	ret
;**************************

; Multiply routine modified from original ATMEL code
mpy16u:
	clr	m16u0		;clean out trash
	clr	m16u1
	clr	m16u2
	clr	m16u3		;clear 2 highest bytes of result
	ldi	mcnt16u,16	;init loop counter
	lsr	mp16uH
	

m16u_1:	ror	mp16uL
	brcc	noad8		;if bit 0 of multiplier set
	add	m16u2,mc16uL	;add multiplicand Low to byte 2 of res
	adc	m16u3,mc16uH	;add multiplicand high to byte 3 of res
noad8:	ror	m16u3		;shift right result byte 3
	ror	m16u2		;rotate right result byte 2
	ror	m16u1		;rotate result byte 1 and multiplier High
	ror	m16u0		;rotate result byte 0 and multiplier Low
	dec	mcnt16u		;decrement loop counter
	brne	m16u_1		;if not done, loop more
	ret

;************************************

add16:	add	add1l, add2l		;Add low bytes
	adc	add1h, add2h		;Add high bytes with carryn
	ret

;;*********************SUBROUTINES AND ISRs******************


synctic:in	save,SREG
	sbi	PORTB,0
	ldi	templo,206		;fill 100u clock
	out	TCNT0,templo
	ldi	templo,0b00000010	;set for clk/8
	out	TCCR0,templo
	cbi	PORTB,0
	out	SREG,save
	reti
	

intover:in	save,SREG		; interval is over
	mov	mcnt16u,beats
	sbr	status,0b10000000
	out	SREG,save
	reti

cp16:	cp	add1l,add2l	;Compare low byte
	cpc	add1h,add2h	;Compare high byte with carry from
				;previous operation
	ret