// File: /workspace/github_c_makefile_star2_O2/ZombiesYard[P]MPU6050-DMP-raspberry-pi/MotionSensor/inv_mpu_lib/inv_mpu.c
// Function: mpu_run_self_test(int*, int*)
uint8_t mpu_run_self_test(int32_t *gyro, int32_t *accel)
{
#ifdef MPU6050
	const uint8_t tries = 2;
	int32_t gyro_st[3], accel_st[3];
	uint8_t accel_result, gyro_result;
#ifdef AK89xx_SECONDARY
	uint8_t compass_result;
#endif
	int ii;
#endif
	int result;
	uint8_t accel_fsr, fifo_sensors, sensors_on;
	uint16_t gyro_fsr, sample_rate, lpf;
	uint8_t dmp_was_on;

	if (st.chip_cfg.dmp_on)
	{
		mpu_set_dmp_state(0);
		dmp_was_on = 1;
	}
	else
		dmp_was_on = 0;

	/* Get initial settings. */
	mpu_get_gyro_fsr(&gyro_fsr);
	mpu_get_accel_fsr(&accel_fsr);
	mpu_get_lpf(&lpf);
	mpu_get_sample_rate(&sample_rate);
	sensors_on = st.chip_cfg.sensors;
	mpu_get_fifo_config(&fifo_sensors);

	/* For older chips, the self-test will be different. */
#if defined MPU6050
	for (ii = 0; ii < tries; ii++)
		if (!get_st_biases(gyro, accel, 0))
			break;
	if (ii == tries)
	{
		/* If we reach this point, we most likely encountered an I2C error.
		 * We'll just report an error for all three sensors.
		 */
		result = 0;
		goto restore;
	}
	for (ii = 0; ii < tries; ii++)
		if (!get_st_biases(gyro_st, accel_st, 1))
			break;
	if (ii == tries)
	{
		/* Again, probably an I2C error. */
		result = 0;
		goto restore;
	}
	accel_result = accel_self_test(accel, accel_st);
	gyro_result = gyro_self_test(gyro, gyro_st);

	result = 0;
	if (!gyro_result)
		result |= 0x01;
	if (!accel_result)
		result |= 0x02;

#ifdef AK89xx_SECONDARY
	compass_result = compass_self_test();
	if (!compass_result)
		result |= 0x04;
#endif
restore:
#elif defined MPU6500
	/* For now, this function will return a "pass" result for all three sensors
	 * for compatibility with current test applications.
	 */
	get_st_biases(gyro, accel, 0);
	result = 0x7;
#endif
	/* Set to invalid values to ensure no I2C writes are skipped. */
	st.chip_cfg.gyro_fsr = 0xFF;
	st.chip_cfg.accel_fsr = 0xFF;
	st.chip_cfg.lpf = 0xFF;
	st.chip_cfg.sample_rate = 0xFFFF;
	st.chip_cfg.sensors = 0xFF;
	st.chip_cfg.fifo_enable = 0xFF;
	st.chip_cfg.clk_src = INV_CLK_PLL;
	mpu_set_gyro_fsr(gyro_fsr);
	mpu_set_accel_fsr(accel_fsr);
	mpu_set_lpf(lpf);
	mpu_set_sample_rate(sample_rate);
	mpu_set_sensors(sensors_on);
	mpu_configure_fifo(fifo_sensors);

	if (dmp_was_on)
		mpu_set_dmp_state(1);

	return result;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movq %rsi, %r14
movq %rdi, %rbx
movb 0x65ac(%rip), %r15b     # 0x923c
testb %r15b, %r15b
je 0x2c9c
xorl %edi, %edi
callq 0x2fff
leaq 0x12(%rsp), %rdi
callq 0x2767
leaq 0x8(%rsp), %rdi
callq 0x278c
leaq 0x10(%rsp), %rdi
callq 0x27c4
movzwl 0x6565(%rip), %eax      # 0x9226
movw %ax, 0x6(%rsp)
movb 0x6555(%rip), %r13b     # 0x9222
pushq $0x3
popq %rbp
movb 0x6551(%rip), %r12b     # 0x9228
decl %ebp
je 0x2dea
movq %rbx, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq 0x30ad
testl %eax, %eax
jne 0x2cd7
movb %r13b, 0x3(%rsp)
movb %r12b, 0x4(%rsp)
movb %r15b, 0x5(%rsp)
pushq $0x3
popq %r13
leaq 0x2c(%rsp), %r15
leaq 0x20(%rsp), %r12
pushq $0x1
popq %rbp
decl %r13d
je 0x2dee
movq %r15, %rdi
movq %r12, %rsi
movl %ebp, %edx
callq 0x30ad
testl %eax, %eax
jne 0x2d10
movq 0x64df(%rip), %rax      # 0x9210
movzbl (%rax), %edi
pushq $0xd
popq %rsi
pushq $0x4
popq %rdx
leaq 0xc(%rsp), %rcx
callq 0x4847
cmpb $-0x1, %al
movb 0x5(%rsp), %r15b
movb 0x4(%rsp), %r12b
movb 0x3(%rsp), %r13b
je 0x2e0a
movb 0xc(%rsp), %al
movb 0xd(%rsp), %cl
shrb $0x3, %al
andb $0x1c, %al
movb 0xf(%rsp), %dl
movl %edx, %esi
shrb $0x4, %sil
andb $0x3, %sil
orb %al, %sil
movb %sil, 0x9(%rsp)
shrb $0x3, %cl
andb $0x1c, %cl
movl %edx, %eax
shrb $0x2, %al
andb $0x3, %al
orb %cl, %al
movb %al, 0xa(%rsp)
movb 0xe(%rsp), %al
shrb $0x3, %al
andb $0x1c, %al
andb $0x3, %dl
orb %al, %dl
movb %dl, 0xb(%rsp)
xorl %eax, %eax
movss 0x2747(%rip), %xmm0     # 0x54f4
movss 0x2743(%rip), %xmm1     # 0x54f8
cmpq $0x3, %rax
je 0x2e0a
movb 0x9(%rsp,%rax), %cl
testb %cl, %cl
je 0x2de0
decb %cl
movaps %xmm0, %xmm2
subb $0x1, %cl
jb 0x2dd3
mulss %xmm1, %xmm2
jmp 0x2dc8
movss %xmm2, 0x14(%rsp,%rax,4)
movb $0x0, 0x9(%rsp,%rax)
jmp 0x2de5
andl $0x0, 0x14(%rsp,%rax,4)
incq %rax
jmp 0x2db5
xorl %ebx, %ebx
jmp 0x2dff
xorl %ebx, %ebx
movb 0x5(%rsp), %r15b
movb 0x4(%rsp), %r12b
movb 0x3(%rsp), %r13b
movzwl 0x6(%rsp), %r14d
jmp 0x2f8b
movss 0x63e6(%rip), %xmm0     # 0x91f8
movss 0x63e2(%rip), %xmm1     # 0x91fc
xorl %eax, %eax
movss 0x63dc(%rip), %xmm2     # 0x9200
xorps %xmm3, %xmm3
xorl %ebp, %ebp
cmpq $0x3, %rax
je 0x2e84
movl (%r14,%rax,4), %ecx
subl 0x20(%rsp,%rax,4), %ecx
movl %ecx, %edx
negl %edx
cmovsl %ecx, %edx
cvtsi2ss %rdx, %xmm4
mulss 0x2215(%rip), %xmm4     # 0x5060
movss 0x14(%rsp,%rax,4), %xmm5
ucomiss %xmm3, %xmm5
jne 0x2e58
jnp 0x2e72
divss %xmm5, %xmm4
addss 0x2210(%rip), %xmm4     # 0x5074
andps 0x2635(%rip), %xmm4     # 0x54a0
ucomiss %xmm2, %xmm4
ja 0x2e7c
jmp 0x2e7f
ucomiss %xmm4, %xmm0
ja 0x2e7c
ucomiss %xmm1, %xmm4
jbe 0x2e7f
btsl %eax, %ebp
incq %rax
jmp 0x2e29
movq 0x6385(%rip), %rax      # 0x9210
movzbl (%rax), %edi
pushq $0xd
popq %rsi
pushq $0x3
popq %rdx
leaq 0x14(%rsp), %rcx
callq 0x4847
xorl %ecx, %ecx
cmpb $-0x1, %al
je 0x2f76
andb $0x1f, 0x14(%rsp)
andb $0x1f, 0x15(%rsp)
andb $0x1f, 0x16(%rsp)
movl 0x631f(%rip), %eax      # 0x91dc
cvtsi2ss %rax, %xmm1
movss 0x2632(%rip), %xmm0     # 0x54fc
divss %xmm1, %xmm0
movss 0x631e(%rip), %xmm1     # 0x91f4
movss 0x630e(%rip), %xmm2     # 0x91ec
movss 0x630a(%rip), %xmm3     # 0x91f0
movss 0x2612(%rip), %xmm4     # 0x5500
xorl %eax, %eax
movzwl 0x6(%rsp), %r14d
movss 0x2162(%rip), %xmm7     # 0x5060
movss 0x216d(%rip), %xmm8     # 0x5074
movaps 0x2591(%rip), %xmm9     # 0x54a0
cmpq $0x3, %rax
je 0x2f6f
movl (%rbx,%rax,4), %edx
subl 0x2c(%rsp,%rax,4), %edx
movl %edx, %esi
negl %esi
cmovsl %edx, %esi
cvtsi2ss %rsi, %xmm5
mulss %xmm7, %xmm5
movb 0x14(%rsp,%rax), %dl
testb %dl, %dl
je 0x2f5d
decb %dl
movaps %xmm0, %xmm6
subb $0x1, %dl
jb 0x2f44
mulss %xmm4, %xmm6
jmp 0x2f39
movb $0x0, 0x14(%rsp,%rax)
divss %xmm6, %xmm5
addss %xmm8, %xmm5
andps %xmm9, %xmm5
ucomiss %xmm1, %xmm5
ja 0x2f67
jmp 0x2f6a
ucomiss %xmm5, %xmm2
ja 0x2f67
ucomiss %xmm3, %xmm5
jbe 0x2f6a
btsl %eax, %ecx
incq %rax
jmp 0x2f0f
testb %cl, %cl
sete %cl
jmp 0x2f7c
movzwl 0x6(%rsp), %r14d
leal 0x2(%rcx), %eax
testb %bpl, %bpl
movzbl %cl, %ecx
movzbl %al, %ebx
cmovnel %ecx, %ebx
orw $-0x1, 0x6293(%rip)     # 0x9226
movb $-0x1, 0x628e(%rip)     # 0x9228
orl $-0x1, 0x627f(%rip)     # 0x9220
movb $0x1, 0x627c(%rip)      # 0x9224
movzwl 0x12(%rsp), %edi
callq 0x1bf9
movzbl 0x8(%rsp), %edi
callq 0x1c8f
movzwl 0x10(%rsp), %edi
callq 0x1d24
movzwl %r14w, %edi
callq 0x1dcd
movzbl %r13b, %edi
callq 0x2049
movzbl %r12b, %edi
callq 0x1e90
testb %r15b, %r15b
je 0x2fee
pushq $0x1
popq %rdi
callq 0x2fff
movl %ebx, %eax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq