// File: /workspace/github_c_makefile_star2_O1/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 $0x48, %rsp
movq %rsi, %r14
movq %rdi, %rbx
movb 0x7256(%rip), %r12b     # 0xa244
testb %r12b, %r12b
je 0x2ffa
xorl %edi, %edi
callq 0x33b2
movl 0x7228(%rip), %eax      # 0xa228
movl %eax, %ecx
shll $0x4, %ecx
movabsq $0x7d003e801f400fa, %r13 # imm = 0x7D003E801F400FA
shrq %cl, %r13
xorl %ecx, %ecx
cmpb $0x4, %al
cmovael %ecx, %r13d
movzbl 0x7208(%rip), %ecx      # 0xa229
movl %eax, 0x14(%rsp)
cmpl $0x3, %ecx
ja 0x3044
shll $0x3, %ecx
movl $0x10080402, %eax       # imm = 0x10080402
shrl %cl, %eax
cmpb $0x0, 0x71f8(%rip)      # 0xa233
setne %cl
shlb %cl, %al
movl %eax, 0x14(%rsp)
movb 0x71e1(%rip), %al       # 0xa22b
decb %al
xorl %r15d, %r15d
movl $0x0, %ecx
cmpb $0x5, %al
ja 0x3066
movzbl %al, %eax
leaq 0x346e(%rip), %rcx      # 0x64d0
movzwl (%rcx,%rax,2), %ecx
movl %ecx, 0x24(%rsp)
movzwl 0x71bd(%rip), %eax      # 0xa22e
movw %ax, 0x12(%rsp)
movb 0x71ae(%rip), %al       # 0xa22a
movb %al, 0x9(%rsp)
movb 0x71aa(%rip), %al       # 0xa230
movb %al, 0xa(%rsp)
movq %rbx, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq 0x3461
testl %eax, %eax
je 0x30aa
incl %r15d
cmpl $0x1, %r15d
je 0x308a
movl $0x2, %r15d
xorl %ebp, %ebp
cmpl $0x2, %r15d
je 0x3336
movq %r13, 0x28(%rsp)
movl %r12d, %r13d
leaq 0x3c(%rsp), %r15
leaq 0x30(%rsp), %r12
movq %r15, %rdi
movq %r12, %rsi
movl $0x1, %edx
callq 0x3461
testl %eax, %eax
je 0x30e8
incl %ebp
cmpl $0x1, %ebp
je 0x30c8
movl $0x2, %ebp
cmpl $0x2, %ebp
movl %r13d, %r12d
jne 0x30fc
xorl %ebp, %ebp
movq 0x28(%rsp), %r13
jmp 0x3336
movq 0x7115(%rip), %rax      # 0xa218
movzbl (%rax), %edi
leaq 0xe(%rsp), %rcx
movl $0xd, %esi
movl $0x4, %edx
callq 0x4dda
cmpb $-0x1, %al
movq 0x28(%rsp), %r13
je 0x31b7
movb 0xe(%rsp), %al
movb 0xf(%rsp), %cl
shrb $0x3, %al
andb $0x1c, %al
movb 0x11(%rsp), %dl
movl %edx, %esi
shrb $0x4, %sil
andb $0x3, %sil
orb %al, %sil
movb %sil, 0xb(%rsp)
shrb $0x3, %cl
andb $0x1c, %cl
movl %edx, %eax
shrb $0x2, %al
andb $0x3, %al
orb %cl, %al
movb %al, 0xc(%rsp)
movb 0x10(%rsp), %al
shrb $0x3, %al
andb $0x1c, %al
andb $0x3, %dl
orb %al, %dl
movb %dl, 0xd(%rsp)
xorl %eax, %eax
movss 0x336b(%rip), %xmm0     # 0x64e4
movss 0x3367(%rip), %xmm1     # 0x64e8
movb 0xb(%rsp,%rax), %cl
testb %cl, %cl
je 0x31a6
movaps %xmm0, %xmm2
decb %cl
je 0x319a
mulss %xmm1, %xmm2
decb %cl
jne 0x3190
xorl %ecx, %ecx
movss %xmm2, 0x18(%rsp,%rax,4)
movb %cl, 0xb(%rsp,%rax)
jmp 0x31ae
movl $0x0, 0x18(%rsp,%rax,4)
incq %rax
cmpq $0x3, %rax
jne 0x3181
movss 0x7041(%rip), %xmm0     # 0xa200
movss 0x703d(%rip), %xmm1     # 0xa204
xorl %eax, %eax
movss 0x7037(%rip), %xmm2     # 0xa208
xorps %xmm3, %xmm3
xorl %ebp, %ebp
movl (%r14,%rax,4), %ecx
subl 0x30(%rsp,%rax,4), %ecx
movl %ecx, %edx
negl %edx
cmovsl %ecx, %edx
xorps %xmm4, %xmm4
cvtsi2ss %rdx, %xmm4
mulss 0x2e6f(%rip), %xmm4     # 0x6064
movss 0x18(%rsp,%rax,4), %xmm5
ucomiss %xmm3, %xmm5
jne 0x3202
jnp 0x321c
divss %xmm5, %xmm4
addss 0x2e6a(%rip), %xmm4     # 0x6078
andps 0x327b(%rip), %xmm4     # 0x6490
ucomiss %xmm2, %xmm4
ja 0x3226
jmp 0x3229
ucomiss %xmm4, %xmm0
ja 0x3226
ucomiss %xmm1, %xmm4
jbe 0x3229
btsl %eax, %ebp
incq %rax
cmpq $0x3, %rax
jne 0x31d6
movq 0x6fdf(%rip), %rax      # 0xa218
movzbl (%rax), %edi
leaq 0x18(%rsp), %rcx
movl $0xd, %esi
movl $0x3, %edx
callq 0x4dda
cmpb $-0x1, %al
je 0x3328
andb $0x1f, 0x18(%rsp)
andb $0x1f, 0x19(%rsp)
andb $0x1f, 0x1a(%rsp)
movl 0x6f77(%rip), %eax      # 0xa1e4
xorps %xmm1, %xmm1
cvtsi2ss %rax, %xmm1
movss 0x326f(%rip), %xmm0     # 0x64ec
divss %xmm1, %xmm0
movss 0x6f73(%rip), %xmm1     # 0xa1fc
movss 0x6f63(%rip), %xmm2     # 0xa1f4
xorl %eax, %eax
movss 0x6f5d(%rip), %xmm3     # 0xa1f8
movss 0x324d(%rip), %xmm4     # 0x64f0
xorl %ecx, %ecx
movss 0x2db7(%rip), %xmm7     # 0x6064
movss 0x2dc2(%rip), %xmm8     # 0x6078
movaps 0x31d2(%rip), %xmm9     # 0x6490
movl (%rbx,%rax,4), %edx
subl 0x3c(%rsp,%rax,4), %edx
movl %edx, %esi
negl %esi
cmovsl %edx, %esi
xorps %xmm5, %xmm5
cvtsi2ss %rsi, %xmm5
mulss %xmm7, %xmm5
movb 0x18(%rsp,%rax), %dl
testb %dl, %dl
je 0x3309
movaps %xmm0, %xmm6
decb %dl
je 0x32f1
mulss %xmm4, %xmm6
decb %dl
jne 0x32e7
xorl %edx, %edx
movb %dl, 0x18(%rsp,%rax)
divss %xmm6, %xmm5
addss %xmm8, %xmm5
andps %xmm9, %xmm5
ucomiss %xmm1, %xmm5
ja 0x3313
jmp 0x3316
ucomiss %xmm5, %xmm2
ja 0x3313
ucomiss %xmm3, %xmm5
jbe 0x3316
btsl %eax, %ecx
incq %rax
cmpq $0x3, %rax
jne 0x32be
xorl %eax, %eax
testb %cl, %cl
sete %al
jmp 0x332a
xorl %eax, %eax
leal 0x2(%rax), %ecx
testb %bpl, %bpl
movzbl %cl, %ebp
cmovnel %eax, %ebp
movw $0xffff, 0x6eef(%rip)   # imm = 0xFFFF
movb $-0x1, 0x6eea(%rip)     # 0xa230
movl $0xffffffff, 0x6ed8(%rip) # imm = 0xFFFFFFFF
movb $0x1, 0x6ed5(%rip)      # 0xa22c
movzwl %r13w, %edi
callq 0x1f36
movzbl 0x14(%rsp), %edi
callq 0x1fd8
movzwl 0x24(%rsp), %edi
callq 0x206e
movzwl 0x12(%rsp), %edi
callq 0x2114
movzbl 0x9(%rsp), %edi
callq 0x2398
movzbl 0xa(%rsp), %edi
callq 0x21dd
testb %r12b, %r12b
je 0x33a1
movl $0x1, %edi
callq 0x33b2
movl %ebp, %eax
addq $0x48, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq