// ================================
// calculation of pi for MCOC115 (Moscovium)
//		(c) 2021	1YEN Toru
//
//
//		2023/10/28	ver.1.10
//			brushed up
//			Nh1: align sp to long word boundary
//
//		2022/04/23	ver.1.08
//			Bug fixed: dual core cpu communication (interrupt) error.
//
//		2022/04/16	ver.1.06
//			corresponding to Nihonium
//
//		2021/09/04	ver.1.04
//			change the formula from Machin to Klingenstierna:
//				8*atan (1/10) - atan (1/239) - 4*atan (1/515)=M_PI/4
//			for dual core cpu edition
//				cpu1: 4*(-1)*atan (1/239) - 4*4*atan (1/515)
//				cpu2: 4*8*atan (1/10)
//
//		2021/07/31	ver.1.02
//			for dual core cpu edition (MCOC115DC0416)
//				cpu1: 4*(-1)*atan (1/239)
//				cpu2: 4*4*atan (1/5)
//
//		2021/06/26	ver.1.00
//			Machin: 4*atan (1/5) - atan (1/239)=M_PI/4
//
// ================================
asm		"mcoc_irom.mem"
#asm		"mcoc_irom.dat2",bit16,list
#asm		"mcoc_irom.mif"
incl	"mcoc115.incl"
# ================================
# string macros
def		prog_name,"calc_pi"
def		prog_vers,"1.10"
def		s,"#"							// simulation="" / real fpga="#"
def		k,""							// use Klingenstierna if ""
def		m,"#"							// use Machin if ""
# Moscovium:		mc="" 	n0="#"	n1="#"
# Nihonium (NH=0):	mc="" 	n0=""	n1="#"
# Nihonium (NH=1):	mc="#" 	n0="#"	n1=""
def		mc,""							// for Moscovium if ""
def		n0,"#"							// for Nihonium (NH=0) if ""
def		n1,"#"							// for Nihonium (NH=1) if ""
# macros
$(n0)# These macros are a simple emulation
$(n0)#		of the divlu instruction for Nihonium.
$(n0)macro	divlu
$(n0)// divlu; r0:r1/r2:r3=r0:r1...r2:r3
$(n0)sesr	sreg_b_nh
$(n0)lsli	r0,16
$(n0)exzrl	r1
$(n0)or		r0,r1
$(n0)lsli	r2,16
$(n0)exzrl	r3
$(n0)or		r2,r3
$(n0)divu	r0,r2
$(n0)mov		r1,r0
$(n0)lsri	r0,16
$(n0)mov		r3,r2
$(n0)lsri	r2,16
$(n0)clsr	sreg_b_nh
$(n0)endm
$(n0)macro	divlq
$(n0)// divlq; empty
$(n0)endm
# constants
equ		max_lbuf,64						// size of lbuf
equ		baud,9600						// baud rate
equ		uart_baud,fcpu/baud-1			// uartbaud setting
equ		chr_lf,0x0a						// lf code
equ		chr_cr,0x0d						// cr code
equ		chr_cma,0x2c					// comma ',' code
equ		lfp_max,1280					// maximum size of long fixed point
equ		base,10000						// base
equ		digt,4							// base=10000 is 4 digits
# variables
equ		va,ramtop
equ		lfp_siz,va						// size of long fixed point
equ		va,va+2
equ		cpi_dgt,va						// digits to calculate pi
equ		va,va+2
equ		cpi_usec,va						// elapsed time [us]
equ		va,va+4
equ		static_va,va
# array variables
equ		lbuf,va							// receive data buffer
equ		va,va+max_lbuf
equ		cpi_a,va						// long fixed point array
equ		va,va+lfp_max*2
equ		cpi_b,va						//	B=m/(x^(2*k + 1));
equ		va,va+lfp_max*2
equ		cpi_a2,va						// for CPU2
equ		va,va+lfp_max*2
equ		cpi_b2,va
equ		va,va+lfp_max*2
equ		buffer_va,va
equ		array_va,va
# auto variables
# m_atan_1_x
equ		atn_zero,0
equ		va,atn_zero						// must not be a number but a variable
equ		atn_m,va						// m_atan_1_x: parameter m
equ		va,va+2
equ		atn_x,va						// m_atan_1_x: parameter x
equ		va,va+2
equ		atn_a,va
equ		va,va+2
equ		atn_b,va
equ		va,va+2
equ		atn_allz,va
equ		va,va+2
equ		atn_sgnp,va
equ		va,va+2
equ		atn_idx,va
equ		va,va+2
equ		atn_idxk,va
equ		va,va+2
equ		atn_quo,va						// quotient
equ		va,va+2
equ		atn_quo2,va
equ		va,va+2
equ		va,(va+3)/4*4					// align 4
equ		atn_rem,va						// remainder
equ		va,va+4
equ		atn_rem2,va
equ		va,va+4
equ		atn_xx,va						// sq(x)
equ		va,va+4
equ		atn_kk,va						// 2*k + 1
equ		va,va+4
equ		atn_auto_va,va
# ================================


// ================================
// setup
// ================================

// uart setting: RXE, TXE, baud rate
ldwi	r7,uartbaud
ldwi	r0,uart_baud
stw		[r7],r0
ldwi	r7,uartctl
ldbiu	r0,uart_rxe|uart_txe
stw		[r7],r0

$(n1)// nh=1
$(n1)sesr	sreg_b_nh

// set sp to the bottom of ram area
ldwi	r7,idrgramt						// idrg: ram top addr.
ldw		r7,[r7]
ldwi	r6,idrgrams						// idrg: ram size
ldw		r6,[r6]
add		r7,r6
movtc	sp,r7

// branch CPU ID
movfc	r0,sr
lsri	r0,sreg_b_id0
andi	r0,sreg_id_3>>sreg_b_id0
beq		cpu_id_0
cmpi	r0,1
beq		cpu_id_1
cmpi	r0,2
beq		cpu_id_2
cpu_id_0:
cpu_id_1:

// clear variables
ldwi	r7,ramtop
ldwi	r1,static_va
ldbiu	r0,0
st_clrvar:
stw		[r7],r0
addi	r7,2
cmp		r7,r1
blo		st_clrvar

// port setting
ldbiu	r0,led_builtin
ldwi	r7,porset
stw		[r7],r0
ldwi	r7,pordir
ldw		r0,[r7]
andi	r0,~led_builtin
stw		[r7],r0

// check mulc16 and divc32
ldbiu	r0,1
mov		r1,r0
mulu	r0,r1
cmpi	r0,1
beq		chk_mul_pass
ldwi	r1,lab_str_err_no_mul
jalwr7	print
chk_mul_pass:
ldbiu	r0,1
mov		r1,r0
divu	r0,r1
cmpi	r0,1
beq		chk_div_pass
ldwi	r1,lab_str_err_no_div
jalwr7	print
chk_div_pass:

// send RDY
ldwi	r1,lab_str_msg_rdy
jalwr7	print

$(s)// ********************************
$(s)// for simulation
#0$(s)// cancel time out
#0$(s)ldwi	r7,simctrl
#0$(s)ldbiu	r0,simc_noto
#0$(s)stw		[r7],r0
$(s)// command test
$(s)ldwi	r6,lab_sim_str_cmnd
$(s)sim_cmnd_loop2:
$(s)ldwi	r7,lbuf
$(s)sim_cmnd_loop:
$(s)ldb		r0,[r6]
$(s)addi	r6,1
$(s)stb		[r7],r0
$(s)addi	r7,1
$(s)cmpi	r0,0
$(s)bne		sim_cmnd_loop
$(s)pushw	r6
$(s)jalwr7	sim_cmnd
$(s)// skip nil, check end mark (*)
$(s)popw	r6
$(s)sim_loop_1:
$(s)ldb		r0,[r6]
$(s)addi	r6,1
$(s)cmpi	r0,0
$(s)beq		sim_loop_1
$(s)subi	r6,1
$(s)cmpi	r0,0x2a						// asterisk(*)
$(s)bne		sim_cmnd_loop2
$(s)bra		sim_cmnd_end
$(s)sim_str_cmnd:
$(s)dats	"who\0"
$(s)dats	"help\0"
$(s)dats	"pi,12\0"
$(s)dats	"*"							// end mark
$(s)sim_cmnd_end:
$(s)ldbiu	r0,0
$(s)// simulation finish
$(s)cmpi	r0,0
$(s)beq		pcnt+6
$(s)ldwi	r0,simfail
$(s)bra		pcnt+4
$(s)ldwi	r0,simpass
$(s)ldwi	r7,simfnsh
$(s)stw		[r7],r0
$(s)bra		pcnt-2
$(s)// ********************************


// ================================
// loop
// ================================
loop:

// ================================
// serial monitor handling
// check uartctl.RAVL
ldwi	r7,uartctl
ldw		r0,[r7]
andi	r0,uart_ravl
beq		no_data
jalwr7	get_line
$(s)// ********************************
$(s)sim_cmnd:
$(s)// ********************************

// check "pi," command
ldwi	r1,lbuf
ldwi	r2,lab_str_cmd_pi
ldbiu	r5,3
jalwr7	strncmp
cmpi	r0,0
bne		pi_notid
// ================================
// pi,<n_digit>
ldwi	r1,lbuf
// <n_digit>
addi	r1,3
jalwr7	atoi
ldwi	r7,cpi_dgt
stw		[r7],r0

// set on board led (green)
ldbiu	r0,led_builtin_g
ldwi	r7,porclr
stw		[r7],r0

// calculation
ldwi	r1,lab_str_msg_start
jalwr7	print_lst
// dual core cpu?
movfc	r0,sr
lsri	r0,sreg_b_id0
andi	r0,sreg_id_3>>sreg_b_id0
beq		pi_notdual
ldwi	r1,lab_str_msg_dual
jalwr7	print
pi_notdual:

// pi calculation
ldwi	r7,cpi_dgt
ldw		r1,[r7]
jalwr7	calc_pi

// clear on board led
ldbiu	r0,led_builtin
ldwi	r7,porset
stw		[r7],r0

// response
ldwi	r1,lab_str_msg_time
jalwr7	print
pushw	r1
ldwi	r1,cpi_usec
ldw		r1,[r1]
jalwr7	print_hex
ldwi	r1,cpi_usec+2
ldw		r1,[r1]
jalwr7	print_hex
popw	r1
jalwr7	print
bra		no_data
pi_notid:

// check "who" command
ldwi	r1,lbuf
ldwi	r2,lab_str_cmd_who
jalwr7	strcmp
cmpi	r0,0
bne		who_notid
// ================================
// who
// response
ldwi	r1,lab_str_msg_who
jalwr7	print
bra		no_data
who_notid:

// check "help" command
ldwi	r1,lbuf
ldwi	r2,lab_str_cmd_help
jalwr7	strcmp
cmpi	r0,0
bne		help_notid
// ================================
// help
// response
ldwi	r1,lab_str_msg_help
jalwr7	print_lst
bra		no_data
help_notid:

// ================================
// ignore empty line
ldwi	r1,lbuf
ldb		r0,[r1]
cmpi	r0,0
beq		no_data
// ================================
// ERROR: unsupported command
ldwi	r1,lab_str_err_unspt
jalwr7	print
pushw	r1
ldwi	r1,lbuf
jalwr7	print
popw	r1
jalwr7	print
bra		no_data

// ERROR: syntax
err_syntx:
ldwi	r1,lab_str_err_syntx
jalwr7	print
bra		no_data

// end of command interpretation
no_data:


// ================================
// end of loop
$(s)// ********************************
$(s)ldwi	r1,lab_sim_str_delim
$(s)jalwr7	print
$(s)rtnw
$(s)sim_str_delim:
$(s)dats	"--------\r\n\0"
$(s)// ********************************
bra		loop


// ================================
// A += m*atan(1/x); disturbed r0~r7,sr[DR],sr[ML]
//		parameter: r0=m, r1=x, r2=cpi_a, r3=cpi_b
//	m*atan(1/x)=m/x - m/(3*x^3) + m/(5*x^5) ...
//		+ ((-1)^k)*m/((2*k + 1)*x^(2*k + 1))
// ================================
m_atan_1_x:
$(n1)// align sp to long word boundary
$(n1)movfc	r7,sp
$(n1)andi	r7,0x02
$(n1)beq	pcnt+6
$(n1)link	2
$(n1)bra	pcnt+4
$(n1)link	0
# allocate auto variable area
movfc	r7,sp
subi	r7,atn_auto_va
movtc	sp,r7
# initialize
sesr	sreg_b_dr
ldbiu	r7,atn_m
stwsp	[r7],r0
ldbiu	r7,atn_x
stwsp	[r7],r1
ldbiu	r7,atn_a
stwsp	[r7],r2
ldbiu	r7,atn_b
stwsp	[r7],r3

// k=0
//	B[0]=(m<0)? -m: m;
cmpi	r0,0
bpl		pcnt+2
neg		r0
ldbiu	r7,atn_b
ldwsp	r7,[r7]
stw		[r7],r0
//	for (idx=1; idx<lfp_siz; idx++)
//		B[idx]=0;
ldbiu	r0,0
ldwi	r1,lfp_siz
ldw		r1,[r1]
lsli	r1,1
add		r1,r7
addi	r7,2
cmp		r7,r1
bhs		pcnt+4
stw		[r7],r0
bra		pcnt-10

//	rem=0;
ldbiu	r0,0
ldbiu	r7,atn_rem
$(mc)stwsp	[r7],r0
$(mc)addi	r7,2
$(mc)stwsp	[r7],r0
$(n1)stlsp	[r7],r0

//	for (idx=0; idx<lfp_siz; idx++)
ldbiu	r6,0
ldbiu	r7,atn_idx
stwsp	[r7],r6
//	{
atn_for_idx1:
ldwi	r0,lfp_siz
ldw		r0,[r0]
cmp		r6,r0
bhs		atn_for_idx1_brk

// B=quo...rem=m/x
//		quo=int16 ((B[idx] + rem)/x);
//		rem=(B[idx] + rem - int32 (quo)*x)*base;
# r0:r1=B[idx] + rem
ldbiu	r7,atn_b
ldwsp	r7,[r7]
add		r7,r6
add		r7,r6
$(mc)ldbiu	r0,0
ldw		r1,[r7]
$(n1)exsgl	r1
ldbiu	r7,atn_rem
$(mc)ldwsp	r2,[r7]
$(mc)addi	r7,2
$(mc)ldwsp	r3,[r7]
$(n1)ldlsp	r3,[r7]
add		r1,r3
$(mc)adc		r0,r2
# r0:r1/x
ldbiu	r7,atn_x
ldwsp	r3,[r7]
$(mc)ldbiu	r2,0
$(mc)divlu
$(mc)divlq
$(n1)divu	r1,r3
# quo=r1
ldbiu	r7,atn_quo
stwsp	[r7],r1
# rem=r2:r3*base
$(mc)ldbiu	r0,0
ldwi	r1,base
$(mc)jalwr7	mulu32
$(n1)mulu	r1,r3
ldbiu	r7,atn_rem
$(mc)stwsp	[r7],r0
$(mc)addi	r7,2
$(mc)stwsp	[r7],r1
$(n1)stlsp	[r7],r1

//		B[idx]=quo;
ldbiu	r7,atn_quo
ldwsp	r1,[r7]
ldbiu	r7,atn_b
ldwsp	r7,[r7]
add		r7,r6
add		r7,r6
stw		[r7],r1
//		if (m<0)
//			A[idx] -= B[idx];
//		else
//			A[idx] += B[idx];
ldbiu	r7,atn_m
ldwsp	r2,[r7]
$(n1)exsgl	r2
ldbiu	r7,atn_a
ldwsp	r7,[r7]
add		r7,r6
add		r7,r6
ldw		r0,[r7]
mov		r2,r2
bmi		pcnt+4
add		r0,r1
bra		pcnt+2

sub		r0,r1
stw		[r7],r0

//	}
ldbiu	r7,atn_idx
ldwsp	r6,[r7]
addi	r6,1
stwsp	[r7],r6
bra		atn_for_idx1

atn_for_idx1_brk:

// k=1,2,...
//	xx=int32 (x)*x;
ldbiu	r7,atn_x
ldwsp	r1,[r7]
$(mc)ldbiu	r0,0
$(mc)mov		r2,r0
mov		r3,r1
$(mc)jalwr7	mulu32
$(n1)mulu	r1,r3
ldbiu	r7,atn_xx
$(mc)stwsp	[r7],r0
$(mc)addi	r7,2
$(mc)stwsp	[r7],r1
$(n1)stlsp	[r7],r1

//	allz=false;
ldbiu	r7,atn_allz
ldbiu	r0,false
stwsp	[r7],r0
//	for (idxk=1, sgnp=( m<0 ); !allz; idxk++, sgnp=!sgnp)
ldbiu	r7,atn_idxk
ldbiu	r0,1
stwsp	[r7],r0
ldbiu	r7,atn_m
ldwsp	r0,[r7]
$(n1)exsgl	r0
ldbiu	r1,false
cmpi	r0,0
bpl		pcnt+2
ldbiu	r1,true
ldbiu	r7,atn_sgnp
stwsp	[r7],r1
# kk=1
$(mc)ldbiu	r0,0
ldbiu	r1,1
ldbiu	r7,atn_kk
$(mc)stwsp	[r7],r0
$(mc)addi	r7,2
$(mc)stwsp	[r7],r1
$(n1)stlsp	[r7],r1
//	{
atn_for_idxk:
ldbiu	r7,atn_allz
ldwsp	r0,[r7]
cmpi	r0,0
bne		atn_for_idxk_brk

//		kk=2*idxk + 1;
ldbiu	r7,atn_kk
$(mc)ldwsp	r0,[r7]
$(mc)addi	r7,2
$(mc)ldwsp	r1,[r7]
$(n1)ldlsp	r1,[r7]
addi	r1,2
$(mc)stwsp	[r7],r1
$(mc)ldbiu	r1,0
$(mc)adc		r0,r1
$(mc)subi	r7,2
$(mc)stwsp	[r7],r0
$(n1)stlsp	[r7],r1

//		rem=0;
ldbiu	r0,0
ldbiu	r7,atn_rem
$(mc)stwsp	[r7],r0
$(mc)addi	r7,2
$(mc)stwsp	[r7],r0
$(n1)stlsp	[r7],r0
//		rem2=0;
ldbiu	r7,atn_rem2
$(mc)stwsp	[r7],r0
$(mc)addi	r7,2
$(mc)stwsp	[r7],r0
$(n1)stlsp	[r7],r0
//		allz=true;
ldbiu	r0,true
ldbiu	r7,atn_allz
stwsp	[r7],r0
//		for (idx=0; idx<lfp_siz; idx++)
ldbiu	r6,0
ldbiu	r7,atn_idx
stwsp	[r7],r6
//		{
atn_for_idx2:
ldwi	r1,lfp_siz
ldw		r1,[r1]
cmp		r6,r1
bhs		atn_for_idx2_brk

// B=quo...rem=B/sq(x)
//			quo=int16 ((B[idx] + rem)/xx);
//			rem=(B[idx] + rem - quo*xx)*base;
# r0:r1=B[idx] + rem
ldbiu	r7,atn_b
ldwsp	r7,[r7]
add		r7,r6
add		r7,r6
$(mc)ldbiu	r0,0
ldw		r1,[r7]
$(n1)exsgl	r1
ldbiu	r7,atn_rem
$(mc)ldwsp	r2,[r7]
$(mc)addi	r7,2
$(mc)ldwsp	r3,[r7]
$(n1)ldlsp	r3,[r7]
add		r1,r3
$(mc)adc		r0,r2
# r0:r1/xx
ldbiu	r7,atn_xx
$(mc)ldwsp	r2,[r7]
$(mc)addi	r7,2
$(mc)ldwsp	r3,[r7]
$(n1)ldlsp	r3,[r7]
$(mc)divlu
$(mc)divlq
$(n1)divu	r1,r3
# quo=r1
ldbiu	r7,atn_quo
stwsp	[r7],r1
# rem=r2:r3*base
$(mc)ldbiu	r0,0
ldwi	r1,base
$(mc)jalwr7	mulu32
$(n1)mulu	r1,r3
ldbiu	r7,atn_rem
$(mc)stwsp	[r7],r0
$(mc)addi	r7,2
$(mc)stwsp	[r7],r1
$(n1)stlsp	[r7],r1
//			B[idx]=quo;
ldbiu	r7,atn_quo
ldwsp	r1,[r7]
ldbiu	r7,atn_b
ldwsp	r7,[r7]
add		r7,r6
add		r7,r6
stw		[r7],r1

// quo2...rem2=B/(2*k + 1)
//			quo2=int16 ((B[idx] + rem2)/kk);
//			rem2=(B[idx] + rem2 - quo2*kk)*base;
# r0:r1=B[idx] + rem2
ldbiu	r7,atn_b
ldwsp	r7,[r7]
add		r7,r6
add		r7,r6
$(mc)ldbiu	r0,0
ldw		r1,[r7]
$(n1)exsgl	r1
ldbiu	r7,atn_rem2
$(mc)ldwsp	r2,[r7]
$(mc)addi	r7,2
$(mc)ldwsp	r3,[r7]
$(n1)ldlsp	r3,[r7]
add		r1,r3
$(mc)adc		r0,r2
# r0:r1/kk
ldbiu	r7,atn_kk
$(mc)ldwsp	r2,[r7]
$(mc)addi	r7,2
$(mc)ldwsp	r3,[r7]
$(n1)ldlsp	r3,[r7]
$(mc)divlu
$(mc)divlq
$(n1)divu	r1,r3
# quo2=r1
ldbiu	r7,atn_quo2
stwsp	[r7],r1
//			if (quo2)
//				allz=false;
ldbiu	r0,false
mov		r1,r1
beq		pcnt+4
ldbiu	r7,atn_allz
stwsp	[r7],r0
# rem2=r2:r3*base
ldwi	r1,base
$(mc)jalwr7	mulu32
$(n1)mulu	r1,r3
ldbiu	r7,atn_rem2
$(mc)stwsp	[r7],r0
$(mc)addi	r7,2
$(mc)stwsp	[r7],r1
$(n1)stlsp	[r7],r1

// A += ((-1)^k)*quo2; partial carry save addition
# r1=quo2, r0=A[idx]
ldbiu	r7,atn_quo2
ldwsp	r1,[r7]
ldbiu	r7,atn_a
ldwsp	r7,[r7]
add		r7,r6
add		r7,r6
ldw		r0,[r7]
$(n1)exsgl	r0
//			if (sgnp)
ldbiu	r7,atn_sgnp
ldwsp	r2,[r7]
ldbiu	r7,atn_a
ldwsp	r7,[r7]
add		r7,r6
add		r7,r6
mov		r2,r2
beq		atn_if_sgnp_else
//			{
//				A[idx] += quo2;
add		r0,r1
stw		[r7],r0

//				// partial carry propagation
//				if (A[idx]>=base)
ldwi	r1,base
cmp		r0,r1
blt		atn_if_A_ge_base_endif
//				{
//					A[idx] -= base;
sub		r0,r1
stw		[r7],r0
//					A[idx - 1]++;
subi	r7,2
ldw		r0,[r7]
addi	r0,1
stw		[r7],r0
//				}
atn_if_A_ge_base_endif:

//			}
bra		atn_if_sgnp_endif

//			else
//			{
align	4,0x0000						// @
atn_if_sgnp_else:
//				A[idx] -= quo2;
sub		r0,r1
stw		[r7],r0

// partial carry propagation
//				if (A[idx]<=-base)
$(mc)ldwi	r1,-base
$(n1)ldli	r1,-base
cmp		r1,r0
blt		atn_if_A_le_base_endif
//				{
//					A[idx] += base;
sub		r0,r1							// subtract because r1=-base
stw		[r7],r0
//					A[idx - 1]--;
subi	r7,2
ldw		r0,[r7]
$(n1)exsgl	r0
subi	r0,1
stw		[r7],r0
//				}
atn_if_A_le_base_endif:

//			}
atn_if_sgnp_endif:

//		}
ldbiu	r7,atn_idx
ldwsp	r6,[r7]
addi	r6,1
stwsp	[r7],r6
bra		atn_for_idx2

atn_for_idx2_brk:

//	}
ldbiu	r7,atn_idxk
ldwsp	r0,[r7]
addi	r0,1
stwsp	[r7],r0
ldbiu	r7,atn_sgnp
ldwsp	r0,[r7]
eori	r0,1
stwsp	[r7],r0
bra		atn_for_idxk

atn_for_idxk_brk:

# free auto variable area
movfc	r7,sp
addi	r7,atn_auto_va
movtc	sp,r7
$(n1)unlk
rtnw

// ================================
// carry propagation
// ================================
carry_prop:
# r2=base
ldwi	r2,base
//	for (idx=lfp_siz - 1; idx>0; idx--)
ldwi	r6,lfp_siz
ldw		r6,[r6]
subi	r6,1
ldwi	r7,cpi_a
add		r7,r6
add		r7,r6
ldwi	r5,cpi_a2
add		r5,r6
add		r5,r6
//	{
cry_for_idx:
cmpi	r6,0
beq		cry_for_idx_brk

# r0=A[idx] + A2[idx], r1=A[idx - 1]
ldw		r0,[r7]
$(n1)exsgl	r0
ldw		r3,[r5]
$(n1)exsgl	r3
add		r0,r3
subi	r7,2
ldw		r1,[r7]

//		while (A[idx]<0)
//		{
cry_while1:
cmpi	r0,0
bpl		cry_while_brk1

//			A[idx] += base;
add		r0,r2
//			A[idx - 1]--;
subi	r1,1
//		}
bra		cry_while1

cry_while_brk1:

//		while (A[idx]>=base)
//		{
cry_while2:
cmp		r0,r2
blt		cry_while_brk2

//			A[idx] -= base;
sub		r0,r2
//			A[idx - 1]++;
addi	r1,1
//		}
bra		cry_while2

cry_while_brk2:

# A[idx]=r0, A[idx - 1]=r1
stw		[r7],r1
addi	r7,2
stw		[r7],r0

//	}
subi	r6,1
subi	r7,2
subi	r5,2
bra		cry_for_idx

cry_for_idx_brk:
rtnw

// ================================
// calc_pi
//		parameter: r1=dgt
// ================================
calc_pi:
// parameter
//	dgt=(dgt<0)? 0: dgt;
mov		r1,r1
bpl		pcnt+2
ldbiu	r1,0
//	lfp_siz=(dgt + digt - 1)/digt + 1;
addi	r1,digt-1
ldwi	r2,digt
divu	r1,r2
addi	r1,1
//	lfp_siz=(lfp_siz<2)? 2: (lfp_siz>lfp_max)? lfp_max: lfp_siz;
cmpi	r1,2
bhs		pcnt+2
ldbiu	r1,2
ldwi	r2,lfp_max
cmp		r1,r2
blo		pcnt+2
mov		r1,r2
ldwi	r7,lfp_siz
stw		[r7],r1
# dgt
ldbiu	r0,digt
subi	r1,1
mulu	r0,r1
ldwi	r7,cpi_dgt
stw		[r7],r0

// clear A[]
//	for (idx=0; idx<lfp_siz; idx++)
//		A[idx]=A2[idx]=0;
ldwi	r7,cpi_a
ldwi	r6,cpi_a2
ldwi	r1,lfp_siz
ldw		r1,[r1]
lsli	r1,1
add		r1,r7
ldbiu	r0,0
cpi_clear_a:
stw		[r7],r0
addi	r7,2
stw		[r6],r0
addi	r6,2
cmp		r7,r1
blo		cpi_clear_a

// start calculation
//	usec_sta=micros ();
ldwi	r7,sytmctl
ldbiu	r0,sytm_rst
stw		[r7],r0

// branch CPU ID
movfc	r0,sr
lsri	r0,sreg_b_id0
andi	r0,sreg_id_3>>sreg_b_id0
beq		cpi_single_core

cpi_dual_core:
// interrupt request setting
ldwi	r7,intmsk
ldbiu	r0,~intc_icm1
stw		[r7],r0
// kick CPU2
ldwi	r7,intcpu
ldbiu	r0,intc_icf|intc_ice|intc_icrq
stw		[r7],r0

$(k)// Klingenstierna
$(k)//	m_atan_1_x (A, 4*(-1),239);
$(k)$(mc)ldwi	r0,4*(-1)
$(k)$(n1)ldli	r0,4*(-1)
$(k)ldbiu	r1,239
$(k)ldwi	r2,cpi_a
$(k)ldwi	r3,cpi_b
$(k)jalwr7	m_atan_1_x
$(k)
$(k)//	m_atan_1_x (A, 4*(-4),515);
$(k)$(mc)ldwi	r0,4*(-4)
$(k)$(n1)ldli	r0,4*(-4)
$(k)ldwi	r1,515
$(k)ldwi	r2,cpi_a
$(k)ldwi	r3,cpi_b
$(k)jalwr7	m_atan_1_x

$(m)// Machin
$(m)//	m_atan_1_x (A, 4*(-1),239);
$(m)$(mc)ldwi	r0,4*(-1)
$(m)$(n1)ldli	r0,4*(-1)
$(m)ldbiu	r1,239
$(m)ldwi	r2,cpi_a
$(m)ldwi	r3,cpi_b
$(m)jalwr7	m_atan_1_x

// wait ICR1
pause
// clear ICF1
ldwi	r7,intcpu
ldw		r0,[r7]
stw		[r7],r0

bra		cpi_common_proc

cpi_single_core:
$(k)// Klingenstierna
$(k)//	m_atan_1_x (A, 4*8,10);
$(k)ldbiu	r0,4*8
$(k)ldbiu	r1,10
$(k)ldwi	r2,cpi_a
$(k)ldwi	r3,cpi_b
$(k)jalwr7	m_atan_1_x
$(k)
$(k)//	m_atan_1_x (A, 4*(-1),239);
$(k)$(mc)ldwi	r0,4*(-1)
$(k)$(n1)ldli	r0,4*(-1)
$(k)ldbiu	r1,239
$(k)ldwi	r2,cpi_a
$(k)ldwi	r3,cpi_b
$(k)jalwr7	m_atan_1_x
$(k)
$(k)//	m_atan_1_x (A, 4*(-4),515);
$(k)$(mc)ldwi	r0,4*(-4)
$(k)$(n1)ldli	r0,4*(-4)
$(k)ldwi	r1,515
$(k)ldwi	r2,cpi_a
$(k)ldwi	r3,cpi_b
$(k)jalwr7	m_atan_1_x

$(m)// Machin
$(m)//	m_atan_1_x (A, 4*4,5);
$(m)ldbiu	r0,4*4
$(m)ldbiu	r1,5
$(m)ldwi	r2,cpi_a
$(m)ldwi	r3,cpi_b
$(m)jalwr7	m_atan_1_x
$(m)
$(m)//	m_atan_1_x (A, 4*(-1),239);
$(m)$(mc)ldwi	r0,4*(-1)
$(m)$(n1)ldli	r0,4*(-1)
$(m)ldbiu	r1,239
$(m)ldwi	r2,cpi_a
$(m)ldwi	r3,cpi_b
$(m)jalwr7	m_atan_1_x

cpi_common_proc:
//	carry_prop (A);
jalwr7	carry_prop

//	usec_end=micros ();
ldwi	r7,sytmmcrh
ldw		r0,[r7]
addi	r7,2
ldw		r1,[r7]
ldwi	r7,cpi_usec
stw		[r7],r0
addi	r7,2
stw		[r7],r1

// print result
$(s)// ********************************
$(s)ldwi	r7,princtl
$(s)ldbiu	r0,prin_nld
$(s)stw		[r7],r0
$(s)// ********************************
ldwi	r1,lab_str_msg_pi
jalwr7	print
#
ldwi	r7,cpi_a+2
ldwi	r6,lfp_siz
ldw		r6,[r6]
lsli	r6,1
subi	r6,2
add		r6,r7
cpi_print_loop:
ldw		r1,[r7]
pushwm	r6,r7
jalwr7	print_dec4
ldwi	r1,lab_str_crlf
jalwr7	print
popwm	r7,r6
addi	r7,2
cmp		r7,r6
blo		cpi_print_loop
#
ldwi	r1,lab_str_msg_digt
jalwr7	print
pushw	r1
ldwi	r7,cpi_dgt
ldw		r1,[r7]
jalwr7	print_dec4
popw	r1
jalwr7	print
pushw	r1
ldwi	r7,lfp_siz
ldw		r1,[r7]
jalwr7	print_dec4
popw	r1
jalwr7	print

rtnw


// ================================
// CPU2: wait ICR2, calculate m_atan_1_x(A2,16,5);
// ================================
cpu_id_2:
// set sp for CPU2
movfc	r7,sp
ldwi	r0,atn_auto_va*2				// atn_auto_va + X; X=atn_auto_va
sub		r7,r0
movtc	sp,r7

// interrupt request setting
ldwi	r7,intcpu
ldbiu	r0,intc_icf|intc_ice
stw		[r7],r0
ldwi	r7,intmsk
ldbiu	r0,~intc_icm2
stw		[r7],r0
nop

loop_cpu2:
// wait ICR2
pause
// clear ICF2
ldwi	r7,intcpu
ldw		r0,[r7]
stw		[r7],r0

$(k)// Klingenstierna
$(k)//	m_atan_1_x (A, 4*8,10);
$(k)ldbiu	r0,4*8
$(k)ldbiu	r1,10
$(k)ldwi	r2,cpi_a2
$(k)ldwi	r3,cpi_b2
$(k)jalwr7	m_atan_1_x

$(m)// Machin
$(m)//	m_atan_1_x (A2, 4*4,5);
$(m)ldbiu	r0,4*4
$(m)ldbiu	r1,5
$(m)ldwi	r2,cpi_a2
$(m)ldwi	r3,cpi_b2
$(m)jalwr7	m_atan_1_x

// tell CPU1 for calculation finished
ldwi	r7,intcpu
ldbiu	r0,intc_ice|intc_icrq
stw		[r7],r0
bra		loop_cpu2


// ================================
// get line from uart ; disturbed r0,r5~r7
// ================================
get_line:
ldwi	r7,uartrdat
ldwi	r6,uartctl
ldwi	r5,lbuf
gl_loop:
// polling until RAVL=1
ldw		r0,[r6]
andi	r0,uart_ravl
beq		gl_loop
// get byte
ldw		r0,[r7]
stb		[r5],r0
addi	r5,1
// LF code check
cmpi	r0,chr_lf
beq		gl_got
// size check
ldwi	r0,lbuf+max_lbuf-1
cmp		r5,r0
blo		gl_loop
// got line
gl_got:
// string terminate
ldbiu	r0,0
subi	r5,1
stb		[r5],r0
// check CR code
subi	r5,1
ldb		r0,[r5]
cmpi	r0,chr_cr
bne		gl_notcr
ldbiu	r0,0
stb		[r5],r0
gl_notcr:
rtnw

// ================================
// print r1 ; disturbed r0~r2,r6,r7
// ================================
print:
ldwi	r7,uarttdat
ldwi	r6,uartctl
pr_loop:
// get 1 byte
ldb		r2,[r1]
addi	r1,1
cmpi	r2,0
beq		pr_finish
$(s)// ********************************
$(s)ldwi	r7,princhr
$(s)stw		[r7],r2
$(s)bra		pr_loop
$(s)// ********************************
// polling uartctl.TFUL=0
pr_poll:
ldw		r0,[r6]
andi	r0,uart_tful
bne		pr_poll
// send data
stw		[r7],r2
bra		pr_loop
pr_finish:
rtnw

// ================================
// print_lst r1 ; disturbed r0~r2,r6,r7
// ================================
print_lst:
jalwr7	print
ldb		r0,[r1]
cmpi	r0,0
bne		print_lst
rtnw

// ================================
// print_hex r1 ; disturbed r0~r7
//		print r1 as 4 digits hexadecimal number
// ================================
print_hex:
ldbiu	r3,4
$(s)// ********************************
$(s)ldwi	r7,princtl
$(s)ldbiu	r0,prin_nld
$(s)stw		[r7],r0
$(s)ldwi	r7,prinhex
$(s)cmpi	r3,4
$(s)beq		ph_else
$(s)cendw	r1
$(s)stb		[r7],r1
$(s)bra		ph_endif
$(s)ph_else:
$(s)stw		[r7],r1
$(s)ph_endif:
$(s)rtnw
$(s)// ********************************
ldwi	r7,uarttdat
ldwi	r6,uartctl
ldwi	r5,lab_ph_hexdig
ph_loop:
// print nibble
mov		r0,r1
mov		r4,r5
lsri	r0,12
andi	r0,0x0f
add		r4,r0
ldb		r2,[r4]
// polling uartctl.TFUL=0
ph_poll:
ldw		r0,[r6]
andi	r0,uart_tful
bne		ph_poll
// send data
stw		[r7],r2
// loop
lsli	r1,4
subi	r3,1
bne		ph_loop
rtnw

ph_hexdig:
dats	"0123456789abcdef"

// ================================
// print_chr r1 ; disturbed r0,r7
// ================================
print_chr:
$(s)// ********************************
$(s)ldwi	r7,princhr
$(s)stw		[r7],r1
$(s)rtnw
$(s)// ********************************
// polling uartctl.TFUL=0
ldwi	r7,uartctl
pc_poll:
ldw		r0,[r7]
andi	r0,uart_tful
bne		pc_poll
// send data
ldwi	r7,uarttdat
stw		[r7],r1
rtnw

// ================================
// print decimal4 r1 ; disturbed r0~r4,r6,r7
// ================================
print_dec4:
$(s)// ********************************
$(s)ldwi	r7,prindec
$(s)stw		[r7],r1
$(s)rtnw
$(s)// ********************************
mov		r1,r1
bne		pdc_not_0
// 0000
ldbiu	r1,0a0
jalwr7	print_chr
jalwr7	print_chr
jalwr7	print_chr
jalwr7	print_chr
bra		pdc_fin

pdc_not_0:
// for each r0=1000, 100, 10, 1
ldwi	r6,lab_pdc_tab
ldbiu	r3,false						// omit leading zero flag
pdc_loop:
ldw		r0,[r6]
cmpi	r0,0
beq		pdc_fin
// r2=r1/r0
ldbiu	r2,0
pdc_div:
sub		r1,r0
blo		pcnt+4
addi	r2,1
bra		pdc_div

// omit leading zero
mov		r3,r3
beq		pcnt+4
mov		r2,r2
beq		pdc_omit
// print 1 digit
addi	r2,0a0
pushwm	r0,r1,r7
mov		r1,r2
jalwr7	print_chr
popwm	r7,r1,r0
$(n1)exsgl	r1
ldbiu	r3,false
pdc_omit:
// add back r1; loop
add		r1,r0
addi	r6,2
bra		pdc_loop

pdc_fin:
rtnw

pdc_tab:
datw	1000,100,10,1,0

// ================================
// string compare: r0=strcmp (r1,r2) ; disturbed r1~r4
// ================================
strcmp:
ldb		r3,[r1]
addi	r1,1
ldb		r4,[r2]
addi	r2,1
mov		r0,r3
sub		r0,r4
bne		sc_finish
// check nil code
cmpi	r3,0
beq		sc_finish
cmpi	r4,0
bne		strcmp
sc_finish:
rtnw

// ================================
// string n compare: r0=strncmp (r1,r2,r5) ; disturbed r1~r5
// ================================
strncmp:
ldb		r3,[r1]
addi	r1,1
ldb		r4,[r2]
addi	r2,1
mov		r0,r3
sub		r0,r4
bne		snc_finish
// check n character
subi	r5,1
beq		snc_finish
// check nil code
cmpi	r3,0
beq		snc_finish
cmpi	r4,0
bne		strncmp
snc_finish:
rtnw

// ================================
// ascii to integer: r0=atoi (r1)>=0 ; disturbed r1~r3
// ================================
atoi:
ldbiu	r0,0
// r2=[r1]-'0'
ldb		r2,[r1]
subi	r2,0a0
blo		a2i_finish
cmpi	r2,10
bhs		a2i_finish
// r0=10*r0+r2
mov		r3,r0
lsli	r3,3
add		r0,r0
add		r0,r3
add		r0,r2
// loop
addi	r1,1
bra		atoi+2
a2i_finish:
rtnw

// ================================
// unsigned 32*32=32 bits multiply routine:
//		r0:r1=r0:r1*r2:r3 ; disturbed r2,r3,sr[ML]
// ================================
#			r0:r1
#			r2:r3
#		*)-------
#			r3:r1	: 3) ML=1
#		 r3:r0		: 1) ML=0
#		 r1:r2		: 2) ML=0
#	  r0:r2			: not calculate
#	+)-----------
#			r0:r1	: 4)
# ================================
mulu32:
clsr	sreg_b_ml						// ML=0
mulu	r0,r3							// 1)
mulu	r2,r1							// 2)
sesr	sreg_b_ml						// ML=1
mulu	r1,r3							// 3)
add		r0,r2							// 4)
add		r0,r3
rtnw

// ================================
// c string data
// ================================
str_msg_who:
dats	"OK: $(prog_name) ver.$(prog_vers)\r\n\0"
str_msg_rdy:
dats	"RDY\r\n\0"
str_msg_help:
dats	"OK+ pi,<n_digit>\r\n"
dats	"OK+ who\r\n"
dats	"OK: help\r\n\0\0"
str_msg_start:
dats	"OK+ start calculation\r\n"
$(k)dats	"OK+ Klingenstierna's"
$(m)dats	"OK+ Machin's"
dats	" formula\r\n\0\0"
str_msg_time:
dats	"OK: pi time[us]=0x\0\r\n\0"
str_msg_digt:
dats	"OK+ digits=\0 (size=\0)\r\n\0"
str_msg_pi:
dats	"OK+ pi=3.\r\n\0"
str_msg_dual:
dats	"OK+ dual core cpu edition\r\n\0"
str_err_unspt:
dats	"ERR: unsupported command: \0\r\n\0"
str_err_syntx:
dats	"ERR: syntax\r\n\0"
str_err_no_mul:
dats	"ERR: no mulc16\r\n\0"
str_err_no_div:
dats	"ERR: no divc32\r\n\0"
str_cmd_pi:
dats	"pi,\0"
str_cmd_who:
dats	"who\0"
str_cmd_help:
dats	"help\0"
str_crlf:
dats	"\r\n\0"

