;http://www.rjhcoding.com/avr-asm-assemblers.php
;http://www.rjhcoding.com/avr-asm-getting-started-with-avra.php
;http://avra.sourceforge.net/README.html
; https://avr-asm.tripod.com/
; https://www.dexterindustries.com/howto/working-with-avr/any-port-any-pin-a-twi-master-for-attiny-atmega/
; also see http://www.qrp-labs.com/synth/
; and https://github.com/riyas-org/tiny5351
; and https://www.rfzero.net/tutorials/si5351a/
; and http://www.wa5bdu.com/programming-the-si5351a-synthesizer/
; https://www.gadgetronicx.com/attiny85-i2c-protocol-tutorial/


; ATtiny13.Si5351.asm
; version 25 Sep 2021
; author: W8DIZ
;
; This code is based on original code written by Ram Sankar Pillai, VU3XVR, 25-June-2018
; The code has been modified for easy testing and education.

; Set up the working directory for avra ==> cd ~/Desktop/freddieneil.com/kitsandparts.com/PCBs/porkalyzer/ATtiny13A/

; Assemble the code ==> avra ATtiny13.Si5351.asm

; avrdude -P usb -p t13 -c avrispv2 -U ATtiny13.Si5351.hex
; avrdude -P usb -p t13 -c avrispv2 -U lfuse:w:0x7a:m -U hfuse:w:0xff:m



; Microcontroller Clock Speed = 1.2MHz using internal oscillator 9.6MHz/8 = 1.2MHz
; 8bit Timer0 overflow interrupt set to occur every 0.012 milliSeconds = 12millis
; I2C data transfer using BIT-BANG method
       
;********************************************************************************************

.nolist
.include "tn13def.inc"
.list

;**** Ports ****

.equ	SDAPORT	= PORTB
.equ	SDABIT	= PORTB0
.equ	SDADDR	= DDRB
.equ	SDAPIN	= PINB

.equ	SCLPORT	= PORTB
.equ	SCLBIT	= PORTB1
.equ	SCLDDR	= DDRB
.equ	SCLPIN	= PINB

.equ	txPORT	= PORTB
.equ	txBIT		= PORTB2
.equ	txDDR		= DDRB
.equ	txPIN		= PINB

.equ	keyPORT	= PORTB
.equ	keyBIT	= PORTB4
.equ	keyDDR	= DDRB
.equ	keyPIN	= PINB

.equ	b_dir	= 0            ; transfer direction bit in i2cadr
.equ	txFlagBit	= 0        ; BIT0 of flagRegister SRAM # 128
.equ	txDelayTimeFlagBit = 1 ; BIT1 of RX/TX switching delay time

;****  Registers  ****
.def	i2cdata	= r17            ; I2C data transfer register
.def	i2cadr	= r18            ; I2C address and direction register

.def	regNum	= r20
.def	regVal	= r21

.def    A            = r16

.def    tempFreqByte1    = R25
.def    tempFreqByte2    = R26
.def    tempFreqByte3    = R27   

.def    counter            = R30
;********** Division def *****************
.def    remainder1     = R0
.def    remainder2     = R1
.def    remainder3     = R2
.def    remainder4     = R3
.def    remainder5     = R4
.def    remainder6     = R5

.def    dividend1      = r16
.def    dividend2      = r17
.def    dividend3      = r18
.def    dividend4      = r19
.def    dividend5      = r20
.def    dividend6      = r21

.def    divisor1       = r24
.def    divisor2       = r25
.def    divisor3       = r26
.def    divisor4       = r27
.def    divisor5       = r28
.def    divisor6       = r29

;********** Multiply def ******************
.def    ANS1 = R0           ;64-Bit Answer
.def    ANS2 = R1           ;
.def  ANS3 = R2           ;
.def  ANS4 = R3           ;
.def  ANS5 = R4           ;
.def  ANS6 = R5           ;
.def  ANS7 = R6           ;
.def  ANS8 = R7           ;

.def    A1 = R16          ;Multiplicand
.def  A2 = R17          ;
.def  A3 = R18          ;
.def  A4 = R19          ;
.def  B1 = R20          ;Multiplier
.def  B2 = R21          ;
.def  B3 = R22          ;
.def  B4 = R23          ;

.def  C = R24          ;Loop Counter  
 
.def    freqByte1 = R8 ; do not change as it is not deleted by CLEAR ALL
.def    freqByte2 = R9
.def    freqByte3 = R10
;.def    freqByte4 = R11

;**** add16 Register Variables
.def add1l = r16
.def add1h = r17
.def add2l = r18
.def add2h = r19

;sub32 Register Variables
.DEF  sub1byte1 = R16         ;To hold low byte of value1
.DEF  sub1byte2 = R17         ;To hold second byte of value1
.DEF  sub1byte3 = R18         ;To hold third byte of value1
.DEF  sub1byte4 = R19         ;To hold fourth byte of value1
.DEF  sub2byte1 = R20         ;To hold low byte of value2
.DEF  sub2byte2 = R21         ;To hold second byte of value2
.DEF  sub2byte3 = R22         ;To hold third byte of value2
.DEF  sub2byte4 = R23         ;To hold fourth byte of value2

;* "sub16" - Subtracting 16-bit registers
.def sub1l = r16
.def sub1h = r17
.def sub2l = r18
.def sub2h = r19

;___________________ START OF SRAM ADDRESS  _____________________________________
.DSEG
aByte1:    .byte 1
aByte2:    .byte 1

bByte1:    .byte 1
bByte2:    .byte 1
bByte3:    .byte 1
bByte4:    .byte 1
b48bit1:    .byte 1
b48bit2:    .byte 1
b48bit3:    .byte 1
b48bit4:    .byte 1
b48bit5:    .byte 1
b48bit6:    .byte 1
p1Byte1:    .byte 1
p1Byte2:    .byte 1

p2Byte1:    .byte 1
p2Byte2:    .byte 1
p2Byte3:    .byte 1

aTimes128B1:    .byte 1
aTimes128B2:    .byte 1

bTimes128b1:    .byte 1
bTimes128b2:    .byte 1
bTimes128b3:    .byte 1
bTimes128b4:    .byte 1

bTimes128byC:    .byte 1

cTimes128bbyC1:    .byte 1
cTimes128bbyC2:    .byte 1
cTimes128bbyC3:    .byte 1
cTimes128bbyC4:    .byte 1

synth:        .byte 1
flagRegister:    .byte 1
lastFreqVal:        .byte 1
currentFreqVal:    .byte 1

temp1: .byte 1
temp2: .byte 1

.equ    pllFreq    = 810000000
.equ    fr1    = 2970000+700
.equ    fr2    = 7022300

;___________________________________________________________________________

.cseg
.ORG    0x000    
RJMP onReset ;RESET VECTOR
.ORG    0x003 
RJMP    TIMER0_OVF

;___________________________________________________________________________
;*************************************
; TIMER 0 OVERFLOW INTERUPT ROUTINE     *
;*************************************
TIMER0_OVF: ;  0.012 seconds; @1.2MHz clk; 12 milli second
    PUSH    A
    INC    counter
    LDI    A, 0b00100011 ; select ADC 03 = Tunning
    RCALL    readADC
    STS    currentFreqVal, A
    LDI    A,0x8B ;0x1F ;SET THE COMPARE REGISTER
    OUT    TCNT0,A
    POP    A
RETI

;___________________________________________________________________________
onReset:
    LDI    A, LOW(RAMEND)
    OUT    SPL, A   

;    LDI    A,0b00000011 ; CLK 1/64
    LDI    A,0b00000101 ; CLK 1/1024
    OUT    TCCR0B, A
    LDI    A,0b00000010 ;ENABLE TIMER1 OVF, TIMER0_OVF
    OUT    TIMSK0, A
    LDI    A1,0x1F ;SET THE COMPARE REGISTER
    OUT    TCNT0, A

    LDI    A, 0b10000011    ; ADC clock should be 100 to 200KHz,     Processor Clock Speed / 100k = x(round up to next division factor)
    OUT    ADCSRA, A        ; ADC enable, ADC prescaler division factor 10

    SEI
    SBI    txDDR, txBIT
    CBI    keyDDR, keyBIT
    SBI    keyPORT, keyBIT
;************ I2C INIT *********************
    SBI    SDADDR, SDABIT ; as output
    SBI    SCLDDR, SCLBIT ; as output

;************ PLL SETTINGS *******************       
; Except PLL parameter #29, 32 all are ZERO only.
; 810MHZ DIV 27 MHz, then a=30; b=0; c=0
; becomes P1 = 3328 0b0000_1101_0000_0000
    LDI    regNum, 29
    LDI    regVal, 0b00001101
    RCALL    i2cSend

;    LDI    regNum, 32
;    LDI    regVal, 0b00001101
;    RCALL    i2cSend

; ******* One Time Settings *****************
    LDI    regNum, 16 ; CLK0 enable
    LDI    regVal, 0b01001101
    RCALL    i2cSend

;    LDI    regNum, 17 ; CLK1 disable
;    LDI    regVal, 0b01001101
;    RCALL    i2cSend

    LDI    regNum, 18 ; CLK2 enable
    LDI    regVal, 0b01001101
    RCALL    i2cSend

    LDI    regNum, 177 ; PLL Reset
    LDI    regVal, 0xAC
    RCALL    i2cSend

    LDI    regNum, 183 ; capacitor value
    LDI    regVal, 0b11010010
    RCALL    i2cSend

    LDI    regNum, 03 ; clock disable
    LDI    regVal, 0b11111110
    RCALL    i2cSend
;___________________ MAIN PROGRAM STARTS HERE ________________________________
main:
    SBIS    keyPIN, keyBIT
    RJMP    keyPressed
    RJMP    keyNotPressed

keyPressed:
    LDS    A,  flagRegister
    SBRC    A,  txFlagBIT ; skip next if txFlagBIT is 0. ie was in RX mode
    RJMP    checkFreqChange    ; Do nothin, go to main loop
    SBI    txPORT, txBIT
    LDI    regNum, 03 ; clock enable
    LDI    regVal, 0b11111010
    RCALL    i2cSend
    LDS    A,  flagRegister
    SBR    A,  (1<<txFLagBIT) | (1<<txDelayTimeFlagBit)
    STS    flagRegister, A
    RJMP    checkFreqChange

keyNotPressed:
    LDS    A,  flagRegister
    SBRS    A,  txFlagBIT ; skip next if txFlagBIT is 1. ie was in TX mode
    RJMP    checkDelayTimeFlag    ; Do nothing, go to main loop
    LDI    regNum, 03 ; clock enable
    LDI    regVal, 0b11111110
    RCALL    i2cSend
    LDS    A,  flagRegister
    CBR    A,  1<<txFLagBIT
    STS    flagRegister, A
    CLR    counter
    RJMP    checkFreqChange

checkDelayTimeFlag:
    LDS    A,  flagRegister
    SBRS    A,  txDelayTimeFlagBit
    RJMP    checkFreqChange
    CPI    counter, 4 ; 350 millis delay
    BRLO    main ; keyChecked
    CBI    txPORT, txBIT
    LDS    A,  flagRegister
    CBR    A,  1<<txDelayTimeFlagBit
    STS    flagRegister, A
;    RJMP    main

checkFreqChange:
    LDS    A, currentFreqVal
    LDS    R17, lastFreqVal
    CP    R17, A
    BREQ    main
    STS    lastFreqVal, A   
    CLI
    LDS    R17, currentFreqVal
    CLR    XL
    CLR    XH
mapStart:
    INC    XH
    SUBI    XL, -10
    CPI    XH, 25
    BRLT    ok
    RJMP    mapEnd
OK:
    CP    R17, XL
    BRSH    mapStart
mapEnd:

multiply8x8:
.def    ansL    =    R0
.def    ansH    =    R1
.def    multiply8a = R26 ; A
.def    multiply8b = R27 ; B
    MOV    multiply8a, XL ;currentFreqVal
    LDI    multiply8b, 100
    LDI    R28, 8 ; counter for multiply ; C
    CLR    ansH
    MOV    ansL, multiply8b
    LSR    ANSL
loopMul:
    BRCC    skipMul
    ADD    ANSH, multiply8a
skipMul:
    ROR    ANSH
    ROR    ANSL
    DEC    R28
    BRNE    loopMul
;************************ CLK 1 **************************   
    LDI    tempFreqByte1,   LOW(fr1)
    LDI    tempFreqByte2, byte2(fr1)
    LDI    tempFreqByte3, byte3(fr1)

    MOV    freqByte1, tempFreqByte1
    MOV    freqByte2, tempFreqByte2
    MOV    freqByte3, tempFreqByte3

    PUSH    ansL
    PUSH    ansH

    ADD    freqByte1, ansL
    ADC    freqByte2, ansH
    LDI    A, 0
    ADC    freqByte3, A

    LDI    A, 42    ; setting frequency on CLK0
    STS    synth, A
    RCALL    arithmatic

;********************* CLK 2 *******************************
    POP    ansH
    POP    ansL
    LDI    tempFreqByte1,   LOW(fr2)
    LDI    tempFreqByte2, byte2(fr2)
    LDI    tempFreqByte3, byte3(fr2)

    MOV    freqByte1, tempFreqByte1
    MOV    freqByte2, tempFreqByte2
    MOV    freqByte3, tempFreqByte3

    SUB    freqByte1, ansL
    SBC    freqByte2, ansH
    LDI    A, 0
    SBC    freqByte3, A

    LDI    A, 58 ; ; setting frequency on CLK2
    STS    synth, A
    RCALL     arithmatic

    SEI
    rjmp    main

;______________________ ENF OF MAIN PROGRAM ____________________________
;************************************************
;             Subprogram  delay        *
;************************************************
;delay:
;    CLR    counter
;delayNotFinished:
;    CP    counter, A
;    BRLO    delayNotFinished
;RET
;_________________________________________________________________________
readADC:
    OUT    ADMUX, A     ; AVcc, left adjust( we need just ADCH only), ADC0
    SBI    ADCSRA, ADSC ; start ADC conversion
keep_pooling:
    SBIS    ADCSRA, ADIF ; look for ADC conversion finish flag ADIF
    RJMP    keep_pooling
    SBI    ADCSRA, ADIF   
    IN    A, ADCH    ; load ADCH into A
RET

;************************************************
;        Set Frequencey subroutines        *
;************************************************
setFrequency:

; P1 Registers
;    #46          #45           #44
; MS0_P1[0:7], MS0_P1[15:8], MS0_P1[17:16]
; p1Byte1         p1Byte2,      p1Byte3
; when freq is > 2MHz the P1 will be less than 16bit, hence
; bit 17:16 will become zero.
; #44 also contains R divider value, which is Zero in our case
;  P2 Registers
;    #49          #48           #47
; MS0_P2[0:7], MS0_P2[15:8], MS0_P2[19:16] [3:0]
; p2Byte1         p2Byte2,      p2Byte3

; P3 registers
;    #43          #42           #47
; MS0_P3[0:7], MS0_P3[15:8], MS0_P3[19:16] [7:4]
; p1Byte1         p1Byte2,      p1Byte3

    LDS    regNum, synth ; #42, #50, #58
    LDI    regVal, 0b11111111
    RCALL    i2cSend

    INC    regNum    ; #43
    LDI    regVal, 0b11111111
    RCALL    i2cSend

    INC regNum        ; #44 this parameter is always ZERO
;    LDI    regNum, 44
;    LDI    regVal, 0x00
;    RCALL    i2cSend

    INC    regNum    ; #45
    LDS    regVal, p1Byte2
    RCALL    i2cSend

    INC    regNum    ; #46
    LDS     regVal, p1Byte1
    RCALL    i2cSend

    INC    regNum    ; #47
    LDS    A, p2Byte3
    ORI    A, 0b11110000
    MOV    regVal, A
    RCALL    i2cSend

    INC    regNum    ; #48
    LDS    regVal, p2Byte2
    RCALL    i2cSend

    INC    regNum    ; #49
    LDS    regVal, p2Byte1
    RCALL    i2cSend
RET


i2cSend: ;*****************************************************
	ldi		i2cadr,0xc0			;+i2cwr    ; Set device address and write
	rcall	i2c_start				; Send start condition and address
	MOV		i2cdata,regNum	; Write word address (0x00)
	rcall	i2c_do_transfer	; Execute transfer
	MOV		i2cdata,regVal	; Set write data to 01010101b
	rcall	i2c_do_transfer	; Execute transfer
	rcall	i2c_stop				; Send stop condition
	RET

i2c_start: ;*****************************************************
    mov    i2cdata,i2cadr        ; copy address to transmitt register
    sbi    SDADDR,SDABIT        ; force SDA low
    rcall    i2cDelay        ; quarter period delay
i2c_write:
    sec                ; set carry flag
    rol    i2cdata            ; shift in carry and out bit one
    rjmp    i2c_write_first
i2c_write_bit:
    lsl    i2cdata            ; if transmit register empty
i2c_write_first:
    breq    i2c_get_ack        ;    goto get acknowledge
    sbi    SCLDDR,SCLBIT        ; force SCL low
    brcc    i2c_write_low        ; if bit high
    nop                ;    (equalize number of cycles)
    cbi    SDADDR,SDABIT        ;    release SDA
    rjmp    i2c_write_high
i2c_write_low:                ; else
    sbi    SDADDR,SDABIT        ;    force SDA low
    rjmp    i2c_write_high        ;    (equalize number of cycles)
i2c_write_high:
    rcall    i2cDelay        ; half period delay
    cbi    SCLDDR,SCLBIT        ; release SCL
    rcall    i2cDelay        ; half period delay
    rjmp    i2c_write_bit
i2c_get_ack:
    sbi    SCLDDR,SCLBIT        ; force SCL low
    cbi    SDADDR,SDABIT        ; release SDA
    rcall    i2cDelay        ; half period delay
    cbi    SCLDDR,SCLBIT        ; release SCL
i2c_get_ack_wait:
    sbis    SCLPIN,SCLBIT        ; wait SCL high
                        ;(In case wait states are inserted)
    rjmp    i2c_get_ack_wait
    clc                ; clear carry flag
    sbic    SDAPIN,SDABIT        ; if SDA is high
    sec                ;    set carry flag
    rcall    i2cDelay        ; half period delay
    RET

i2c_do_transfer: ;*****************************************************
    sbrs    i2cadr,b_dir        ; if dir = write
    rjmp    i2c_write        ;    goto write data
i2c_stop:
    sbi    SCLDDR,SCLBIT        ; force SCL low
    sbi    SDADDR,SDABIT        ; force SDA low
    rcall    i2cDelay        ; half period delay
    cbi    SCLDDR,SCLBIT        ; release SCL
    rcall    i2cDelay        ; quarter period delay
    cbi    SDADDR,SDABIT        ; release SDA
    rcall    i2cDelay        ; half period delay

    RET

i2cDelay: ;*****************************************************
    NOP
    NOP
    NOP
    NOP
    RET

;******************************************************
;        P1, P2, P3 Arithmatic Functions        *
;******************************************************
arithmatic:   
    RCALL     clearAll
    ldi    dividend1,   low(pllFreq)    ;1 LS-Byte
    ldi    dividend2, byte2(pllFreq)    ;1
    ldi    dividend3, byte3(pllFreq)    ;1
    ldi    dividend4, byte4(pllFreq)    ;1 MS-Byte
    MOV    divisor1, freqByte1           ;1 LS-Byte
    MOV    divisor2, freqByte2           ;1
    MOV    divisor3, freqByte3           ;1
    CLR    divisor4;, freqByte4           ;1 MS-Byte
; this is a + b/c = PLLFreq / Freq
    rcall  divideU32byU32r                ;- result (dividend4-dividend1) should be 1000000 (1000000.49)
    STS    aByte1, dividend1 ; hold value of " a "
    STS    aByte2, dividend2
    MOV    r12, remainder1
    MOV    r13, remainder2
    MOV    r14, remainder3
    MOV    r15, remainder4
    RCALL    clearAll ; aByte & bByte both TESTED OK
; remainder multiply 0xFFFFF
    MOV    a1, r12
    MOV    a2, r13
    MOV    a3, r14
    MOV    a4, r15
    ldi    b1,   low(0xFFFFF)
    ldi    b2, byte2(0xFFFFF)
    ldi    b3, byte3(0xFFFFF)
    RCALL    multiply32x32
    MOV    r12, ans1
    MOV    r13, ans2
    MOV    r14, ans3
    MOV    r15, ans4
    STS    b48bit5, ans5
    STS    b48bit6, ans6
    RCALL    clearAll

;______________________________________________________________________________
; divide by frequency
    MOV    dividend1, r12
    MOV    dividend2, r13
    MOV    dividend3, r14
    MOV    dividend4, r15
    LDS    dividend5, b48bit5
    LDS    dividend6, b48bit6

    MOV    divisor1, freqByte1
    MOV    divisor2, freqByte2
    MOV    divisor3, freqByte3
    CLR    divisor4;, freqByte4

    rcall    divideU32byU32r

    STS    bByte1, dividend1 ; hold value of " b "
    STS    bByte2, dividend2
    STS    bByte3, dividend3
    STS    bByte4, dividend4 ; holds b final value
    RCALL    clearAll

;______________________________________________________________________________
; b times 128
    LDS    A1, bByte1
    LDS    A2, bByte2
    LDS    A3, bByte3
    LDS    A4, bByte4
    LDI   B1,  128;LOW(128)
    rcall    multiply32x32 ; this is = b * 128 ; 32bit answer
    STS    bTimes128b1, ANS1
    STS    bTimes128b2, ANS2
    STS    bTimes128b3, ANS3
    STS    bTimes128b4, ANS4
    RCALL    clearAll
;______________________________________________________________________________
    LDS    dividend1, bTimes128b1
    LDS    dividend2, bTimes128b2
    LDS    dividend3, bTimes128b3
    LDS    dividend4, bTimes128b4

    ldi    divisor1,   low(0xFFFFF)           ;1 LS-Byte
    ldi    divisor2, byte2(0xFFFFF)           ;1
    ldi    divisor3, byte3(0xFFFFF)           ;1
    rcall    divideU32byU32r  ; this result is = 128 * b / 1048575

    STS    bTimes128byC, dividend1
    RCALL    clearAll
;______________________________________________________________________________
; now it = 128 * a
    LDS    A1, aByte1
    LDS    A2, aByte2
    LDI   B1,  128;LOW(128)
    RCALL    multiply32x32 ; i'm getting 14720     ; we are interested in ans1 & ans2 only = 16bit
    MOV    r12, ans1
    MOV    r13, ans2
    RCALL    clearAll
;______________________________________________________________________________
    MOV    add1L, r12
    MOV    add1H, r13
    LDS    add2L, bTimes128byC
    CLR    add2H
    add    add1l, add2l        ;Add low bytes
    adc    add1h, add2h
    STS    temp1, add1L
    STS    temp2, add1H
    RCALL    clearAll
;______________________________________________________________________________
    LDS    sub1L, temp1
    LDS    sub1H, temp2
    LDI    sub2L,   LOW(512)
    LDI    sub2H, BYTE2(512)
    sub    sub1l,sub2l        ;Subtract low bytes
    sbc    sub1h,sub2h        ;Add high byte with carry
    STS    p1Byte1, sub1L
    STS    p1Byte2, sub1H ;  this hold P1 Value.
    RCALL    clearAll
;______________________________________________________________________________
; now it = 128 * b
    LDS    A1,     bByte1
    LDS    A2,    bByte2
    LDS    A3,    bByte3
    LDS    A4,    bByte4
    LDI   B1,  LOW(128)
;    LDI   B2, BYTE2(128)
    RCALL    multiply32x32
    STS    bTimes128b1, ANS1
    STS    bTimes128b2, ANS2
    STS    bTimes128b3, ANS3
    STS    bTimes128b4, ANS4
    RCALL    clearAll
;______________________________________________________________________________
; now it = C *( 128 * b/c )
    LDS    A1,     bTimes128byC
    LDI   B1,   LOW(0xFFFFF)
    LDI   B2, BYTE2(0xFFFFF)
    LDI   B3, BYTE3(0xFFFFF)
    RCALL    multiply32x32
    MOV    r12, ans1
    MOV    r13, ans2
    MOV    r14, ans3
    MOV    r15, ans4
    RCALL    clearAll
;______________________________________________________________________________
; Now subtraction of (128*b) - (c* 128*b/c)
    LDS    sub1byte1, bTimes128b1
    LDS    sub1byte2, bTimes128b2
    LDS    sub1byte3, bTimes128b3
    LDS    sub1byte4, bTimes128b4
    MOV    sub2Byte1, r12
    MOV    sub2Byte2, r13
    MOV    sub2Byte3, r14
    MOV    sub2Byte4, r15
    SUB    sub1byte1,sub2byte1         ;Add the lower bytes together
    SBC    sub1byte2,sub2byte2         ;Add the second bytes and include carry bit
    SBC    sub1byte3,sub2byte3         ;Add the third bytes and include carry bit
    SBC    sub1byte4,sub2byte4         ;Add the fourth bytes and include carry bit
    STS    p2Byte1, sub1byte1 ; this on holds P2 values.
    STS    p2Byte2, sub1byte2
    STS    p2Byte3, sub1byte3
    RCALL    setFrequency
    RET
;______________________________________________________________________________

;************************************************************
;         CLEAR ALL REGISTERS                    *
;************************************************************
clearAll: ; These are registers used in arithmatic functions.
    CLR    R0
    CLR    R1
    CLR    R2
    CLR    R3
    CLR    R4
    CLR    R5
    CLR    R6
    CLR    R7
       
    CLR    R16
    CLR    R17
    CLR    R18
    CLR    R19
    CLR    R20
    CLR    R21
    CLR    R22
    CLR    R23
    CLR    R24
    CLR    R25
    CLR    R26
    CLR    R27
    CLR    R28
    CLR    R29

RET


;************************************************************
;         Start of 32 % 32 bit division             *
;   http://www.avrfreaks.net/comment/4819#comment-4819    *
;************************************************************
 ; Dividend will hold the integer result
divideU32byU32r:
    ldi    counter,49                      ;1   init loop counter
divideU32byU32ra:
    sec                                    ;1   set carry
divideU32byU32rb:
    rol    dividend1                       ;1   shift left dividend
    rol    dividend2                       ;1
    rol    dividend3                       ;1
    rol    dividend4                       ;1
    rol    dividend5
    rol    dividend6
    dec    counter                         ;1
    brne   divideU32byU32rc                ;1/2
    ret                                    ;4

divideU32byU32rc:
    rol    remainder1                      ;1
    rol    remainder2                      ;1
    rol    remainder3                      ;1
    rol    remainder4                      ;1
    rol    remainder5
    rol    remainder6
    sub    remainder1,divisor1             ;1   rem -= divisor
    sbc    remainder2,divisor2             ;1
    sbc    remainder3,divisor3             ;1
    sbc    remainder4,divisor4             ;1
    sbc    remainder5,divisor5
    sbc    remainder6,divisor6
    brcc   divideU32byU32ra                ;1/2 if res < 0
    add    remainder1,divisor1             ;1   restore remainder
    adc    remainder2,divisor2             ;1
    adc    remainder3,divisor3             ;1
    adc    remainder4,divisor4             ;1
    adc    remainder5,divisor5
    adc    remainder6,divisor6
    clc                                    ;1   clear carry
    rjmp   divideU32byU32rb                ;2

;************************************************************
;         Start of 32 x 32 bit multiplication        *
;************************************************************
multiply32x32:
; https://sites.google.com/site/avrasmintro/home/2b-basic-math
MUL3232:
    SUB   ANS8,ANS8    ;Clear ANS8 and Carry Flag
    MOV   ANS1,B1      ;Copy Multiplier to Answer
    MOV   ANS2,B2      ;
    MOV   ANS3,B3      ;
    MOV   ANS4,B4      ;
    LDI   C,33         ;Set Loop Counter to 33
LOOP:
    ROR   ANS4         ;Shift Multiplier to right
    ROR   ANS3         ;
    ROR   ANS2         ;
    ROR   ANS1         ;
    DEC   C            ;Decrement Loop Counter
    BREQ    DONE         ;Check if all bits processed
    BRCC    SKIP         ;If Carry Clear skip addition
    ADD   ANS5,A1      ;Add Multipicand into Answer
    ADC   ANS6,A2      ;
    ADC   ANS7,A3      ;
    ADC   ANS8,A4      ;
SKIP:
    ROR   ANS8         ;Shift high bytes of Answer
    ROR   ANS7         ;
    ROR   ANS6         ;
    ROR   ANS5         ;
    RJMP LOOP
DONE:
    RET
;************************ END of 32 x 32 bit Multiplication ************************

