/* VectorAdd.asm
 *
 * Modified: 28 May 2024
 * Author: Dr. Schwartz
 **********************************************
 * Calls a subroutine, VADD, that adds two 
 * contiguous N-element vectors to form the 
 * resulting vector, VC = VA + VB.  The 
 * subroutine inputs and outputs are below.
 * Inputs:  Z = address of the first vector (VA)
 *          r16 = N, the number of elements 
 *          Z+N is address of the 2nd vector (VB)
 *			X = address of resulting vector sum
 * Outputs: Table pointed to by X (VC)
 **********************************************
*/ 

.include "ATxmega128A1Udef.inc"

.equ N = 6		;set the table size here
.def CTR_r17 = r17	;define r17 to hold a counter
; I suggest that you include the register name in your
;   .def assembler directives so that you don't accidently
;   overwrite them.  A smart assembler would warn you about
;   overwriting a defined register.  (Ours is not that smart!)

; SRAM Data Segment
.dseg
.org 0x2000	;Address 0x2000 is where the internal SRAM begins
VC:	.byte N

; Program Section For Code Segment
.cseg		
;Everything is .cseg by default, but since we used .dseg earlier, 
;  we must redeclare the following as a code segment

; Program Flash Data Section (in Code Memory Space)
.org 0x100
VA: .db 1,2,3,4,5,6
VB: .db 0xA0, 0xB0, 0xC0, 0xD0, 0xE0, 0xF0

.org 0x0000
	rjmp MAIN

.org 0x200
MAIN:
;Initialize the stack pointer. There is a default, but assume nothing!!!
	ldi r16, 0xFF
	out CPU_SPL, r16
	ldi r16, 0x3F
	out CPU_SPH, r16

;Initialize pointers to program memory table (VA) and data memory table (VC)
	ldi ZL, low(VA<<1)
	ldi ZH, high(VA<<1)
	ldi XL, low(VC)
	ldi XH, high(VC)
;Initialize r16 with the size of the tables
	ldi r16, N
;Call the subroutine
	rcall VADD

;Normal dog chasing his tail
DONE:
	rjmp DONE	

/****************************************
 * Subroutine VADD adds two contiguous
 * N-element vectors (VA and VB) to form the
 * resulting vector. VC = VA + VB
 * Inputs:   Z = address of the first vector in ROM (VA)
 *           r16 (=N), the number of elements 
 *           Z+N is address of the second vector (VB)
 *			 X = adddress of resulting vector sum in (VC)
 * Outputs:  Values in VC pointed to by X
 *           r16=0 if succesful, else non-zero
 * Affected: CTR_r17 (=r17), r18, r19, Y
 **************************************/
.org 0x300	;Do NOT need this, but good for debugging 
VADD:
; save most everything on the stack (in this order): X, Z, r16
	push XL
	push XH
	push ZL
	push ZH

; make a copy of X into Y
	movw Y, x
; make a copy of r16 into CTR_r17
	mov CTR_r17, r16

; Copy VA ROM tables to RAM
LOOP:
	lpm r19, Z+
	st Y+, r19
	dec CTR_r17
	brne LOOP

; Note that Z now points to VB table
; Do the addition (in a loop)
REPEAT:
	lpm r18, Z+
	ld r19, x
	add r18, r19
	st X+, r18
	dec r16
	brne REPEAT

; Finish up the program by clearing r16 and restoring the saved values
	pop ZH
	pop ZL
	pop XH
	pop XL
; go back to main routine
	ret