// ================================
// MPU9250(AK8963) i2c communication test
//		(c) 2021	1YEN Toru
//
//
//		2023/10/28	ver.1.02
//			brushed up
//
//		2021/08/28	ver.1.00
//
// ================================
asm		"mcoc_irom.mem"
#asm		"mcoc_irom.dat2",bit16,list
#asm		"mcoc_irom.mif"
incl	"mcoc115.incl"
incl	"mcvm_lib_wire_h.incl"
# ================================
# string macros
def		prog_name,"test_mpu9250"
def		prog_vers,"1.02"
def		s,"#"							// simulation="" / real fpga="#"
# constants
equ		max_lbuf,64						// size of lbuf
equ		baud,9600						// baud rate
equ		uart_baud,fcpu/baud-1			// uartbaud setting
equ		stws_baud,fcpu/400000-1			// i2c: clock speed
$(s)// ********************************
$(s)equ		stws_baud,fcpu/400000/3-1	// i2c: clock speed for sim
$(s)// ********************************
equ		chr_lf,0x0a						// lf code
equ		chr_cr,0x0d						// cr code
equ		chr_cma,0x2c					// comma ',' code
equ		sin_msec,500					// length of sine wave [ms]
equ		i2c_mpu9250,0x68				// i2c: MPU9250 slave address
equ		mpu_reg_whoami,0x75				// mpu: reg: WHO_AM_I
equ		mpu_reg_int_pin,0x37			// mpu: reg: INT_PIN_CFG
equ		mpu_reg_acl_out,0x3b			// mpu: reg: ACCEL_*OUT_*
equ		mpu_reg_gyr_out,0x43			// mpu: reg: GYRO_*OUT_*
equ		mpu_mod_byps,0x02				// mpu: mode auxiliary i2c bypass
equ		mpu_idnum,0x71					// mpu: id of mpu9250
equ		i2c_ak8963,0x0c					// i2c: AK8963 slave address
equ		mag_reg_wia,0x00				// mag: reg: WIA
equ		mag_reg_st1,0x02				// mag: reg: ST1
equ		mag_reg_hxl,0x03				// mag: reg: HXL
equ		mag_reg_cntl1,0x0a				// mag: reg: CNTL1
equ		mag_reg_asax,0x10				// mag: reg: ASAX
equ		mag_mod_pdown,0x00				// mag: mode power down
equ		mag_mod_singl,0x01				// mag: mode single measurement
equ		mag_mod_fuse,0x0f				// mag: mode fuse ROM read
equ		mag_mod_16bit,0x10				// mag: 16 bit data mode
equ		mag_twat,100					// mag: Twat mode change interval [us]
equ		mag_idnum,0x48					// mag: id of ak8963
# variables
equ		va,ramtop
equ		reg_addr,va						// register address
equ		va,va+2
equ		reg_data,va						// register data
equ		va,va+2
equ		static_va,va
# array variables
equ		lbuf,va							// receive data buffer
equ		va,va+max_lbuf
equ		array_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

// 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

// 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

// stwser setting
ldwi	r0,stws_baud
ldwi	r7,stwmbaud
stw		[r7],r0

// 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)// setup dummy slave
$(s)ldbiu	r0,i2c_mpu9250
$(s)ldwi	r7,stwsadr
$(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	"mwr1,12,aa\0"				// mpu9250
$(s)dats	"awr1,34,55\0"				// ak8963 (caused error)
$(s)dats	"who\0"
$(s)dats	"help\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 "awr1," / "mwr1," command
ldwi	r1,lbuf
ldwi	r2,lab_str_cmd_awr1
ldbiu	r5,5
jalwr7	strncmp
cmpi	r0,0
beq		awr1_mwr1
ldwi	r1,lbuf
ldwi	r2,lab_str_cmd_mwr1
ldbiu	r5,5
jalwr7	strncmp
mov		r0,r0
bne		awr1_notid
// ================================
// awr1,<reg_addr_hex>,<data_hex>
// mwr1,<reg_addr_hex>,<data_hex>
awr1_mwr1:
ldwi	r1,lbuf+5
// <reg_addr_hex>
jalwr7	xtoi
andi	r0,0xff
ldwi	r7,reg_addr
stw		[r7],r0
// check ','
ldb		r0,[r1]
cmpi	r0,chr_cma
bne		err_syntx
addi	r1,1
// <data_hex>
jalwr7	xtoi
andi	r0,0xff
ldwi	r7,reg_data
stw		[r7],r0

// access to mpu9250(ak8963)
// make i2c frame: S [ADR,W] [reg_addr] [reg_data] P
movfc	r7,sp
subi	r7,2*4
movtc	sp,r7
// S [ADR,W]
ldwi	r0,wire_send|wire_star|wire_adrw
ldwi	r1,lbuf
ldb		r1,[r1]
cmpi	r1,0aa
beq		pcnt+4
addi	r0,i2c_mpu9250<<1
bra		pcnt+2
addi	r0,i2c_ak8963<<1
stw		[r7],r0
// [reg_addr]
addi	r7,2
ldwi	r0,wire_send
ldwi	r1,reg_addr
ldw		r1,[r1]
add		r0,r1
stw		[r7],r0
// [reg_data]
addi	r7,2
ldwi	r0,wire_send
ldwi	r1,reg_data
ldw		r1,[r1]
add		r0,r1
stw		[r7],r0
// P
addi	r7,2
ldbiu	r0,wire_stop
stw		[r7],r0
// call mcvm_wire
movfc	r1,sp
jalwr7	mcvm_wire
// destroy i2c frame
movfc	r7,sp
addi	r7,2*4
movtc	sp,r7

// response
cmpi	r0,0
beq		awr1_ok
// ERROR:
pushw	r0
ldwi	r1,lab_str_err_mpuak
jalwr7	print
popw	r0
pushw	r1
mov		r1,r0
jalwr7	print_hex
popw	r1
jalwr7	print
bra		no_data

awr1_ok:
ldwi	r1,lab_str_msg_awr1
jalwr7	print
pushw	r1
ldwi	r7,reg_addr
ldw		r1,[r7]
#lsli	r1,8
cendw	r1
ldbiu	r3,2
jalwr7	print_hex+2
popw	r1
jalwr7	print
pushw	r1
ldwi	r7,reg_data
ldw		r1,[r7]
#lsli	r1,8
cendw	r1
ldbiu	r3,2
jalwr7	print_hex+2
popw	r1
jalwr7	print
bra		no_data
awr1_notid:

// check "ard1," / "mrd1," command
ldwi	r1,lbuf
ldwi	r2,lab_str_cmd_ard1
ldbiu	r5,5
jalwr7	strncmp
mov		r0,r0
beq		ard1_mrd1
ldwi	r1,lbuf
ldwi	r2,lab_str_cmd_mrd1
ldbiu	r5,5
jalwr7	strncmp
mov		r0,r0
bne		ard1_notid
// ================================
// ard1,<reg_addr_hex>
// mrd1,<reg_addr_hex>
ard1_mrd1:
ldwi	r1,lbuf+5
// <reg_addr_hex>
jalwr7	xtoi
andi	r0,0xff
ldwi	r7,reg_addr
stw		[r7],r0

// access from mpu9250(ak8963)
// make i2c frame: S [ADR,W] [reg_addr] R [ADR,R] [[RCV]](N) P
movfc	r7,sp
subi	r7,2*5
movtc	sp,r7
// S [ADR,W]
ldwi	r0,wire_send|wire_star|wire_adrw
ldwi	r1,lbuf
ldb		r1,[r1]
cmpi	r1,0aa
beq		pcnt+4
addi	r0,i2c_mpu9250<<1
bra		pcnt+2
addi	r0,i2c_ak8963<<1
stw		[r7],r0
pushw	r0
// [reg_addr]
addi	r7,2
ldwi	r0,wire_send
ldwi	r1,reg_addr
ldw		r1,[r1]
add		r0,r1
stw		[r7],r0
// R [ADR,R]
addi	r7,2
popw	r0
ori		r0,wire_adrr
stw		[r7],r0
// [[RCV]](N)
addi	r7,2
ldwi	r0,wire_recv|wire_nack
stw		[r7],r0
// P
addi	r7,2
ldbiu	r0,wire_stop
stw		[r7],r0
// call mcvm_wire
movfc	r1,sp
jalwr7	mcvm_wire
// data received
pushw	r0
movfc	r7,sp
addi	r7,2*3+1+2							// +2=push r0
ldb		r0,[r7]
ldwi	r7,reg_data
stw		[r7],r0
popw	r0
// destroy i2c frame
movfc	r7,sp
addi	r7,2*5
movtc	sp,r7

// response
cmpi	r0,0
beq		ard1_ok
// ERROR:
pushw	r0
ldwi	r1,lab_str_err_mpuak
jalwr7	print
popw	r0
pushw	r1
mov		r1,r0
jalwr7	print_hex
popw	r1
jalwr7	print
bra		no_data

ard1_ok:
ldwi	r1,lab_str_msg_ard1
jalwr7	print
pushw	r1
ldwi	r7,reg_addr
ldw		r1,[r7]
lsli	r1,8
ldbiu	r3,2
jalwr7	print_hex+2
popw	r1
jalwr7	print
pushw	r1
ldwi	r7,reg_data
ldw		r1,[r7]
lsli	r1,8
ldbiu	r3,2
jalwr7	print_hex+2
popw	r1
jalwr7	print
bra		no_data
ard1_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


// ================================
// 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"

// ================================
// 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

// ================================
// hexadecimal to integer: r0=xtoi (r1) ; disturbed r1,r2
// ================================
xtoi:
ldbiu	r0,0
// r2=[r1]-'0'
ldb		r2,[r1]
subi	r2,0a0
blo		x2i_finish						// [r1]<'0'
cmpi	r2,10
blo		x2i_next						// [r1]<='9'
subi	r2,(0aA-0x20)-0a0-10
blo		x2i_finish						// [r1]<'A'
cmpi	r2,16
blo		x2i_next						// [r1]<='F'
subi	r2,0aa-(0aA-0x20)
blo		x2i_finish						// [r1]<'a'
cmpi	r2,16
bhs		x2i_finish						// [r1]>'f'
x2i_next:
// r0=16*r0+r2
lsli	r0,4
add		r0,r2
// end of loop
addi	r1,1
bra		xtoi+2
x2i_finish:
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+ awr1,<reg_addr_hex>,<data_hex>\r\n"
dats	"OK+ ard1,<reg_addr_hex>\r\n"
dats	"OK+ mwr1,<reg_addr_hex>,<data_hex>\r\n"
dats	"OK+ mrd1,<reg_addr_hex>\r\n"
dats	"OK+ who\r\n"
dats	"OK: help\r\n\0\0"
str_msg_awr1:
dats	"OK: wr1[0x\0]=0x\0\r\n\0"
str_msg_ard1:
dats	"OK: rd1[0x\0]=0x\0\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_mpuak:
dats	"ERR: could not communicate mpu9250(ak8963): \0\r\n\0"
str_cmd_awr1:
dats	"awr1,\0"
str_cmd_mwr1:
dats	"mwr1,\0"
str_cmd_ard1:
dats	"ard1,\0"
str_cmd_mrd1:
dats	"mrd1,\0"
str_cmd_who:
dats	"who\0"
str_cmd_help:
dats	"help\0"

# ================================
incl	"mcvm_lib_wire_m.incl"

