// File: /workspace/github_c_makefile_star2_O3/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 0x727b(%rip), %al       # 0xa24c
movb %al, 0xa(%rsp)
testb %al, %al
je 0x2fe0
xorl %edi, %edi
callq 0x3394
movl 0x724a(%rip), %eax      # 0xa230
movl %eax, %ecx
shll $0x4, %ecx
movabsq $0x7d003e801f400fa, %rdx # imm = 0x7D003E801F400FA
shrq %cl, %rdx
xorl %ecx, %ecx
cmpb $0x4, %al
cmovael %ecx, %edx
movq %rdx, 0x28(%rsp)
movzbl 0x7226(%rip), %eax      # 0xa231
movl %ecx, 0x18(%rsp)
cmpl $0x3, %eax
ja 0x303c
movl %eax, %ecx
shll $0x3, %ecx
movl $0x10080402, %edx       # imm = 0x10080402
shrl %cl, %edx
leal (,%rax,8), %ecx
movl $0x20100804, %eax       # imm = 0x20100804
shrl %cl, %eax
cmpb $0x0, 0x7206(%rip)      # 0xa23b
cmovel %edx, %eax
movl %eax, 0x18(%rsp)
movb 0x71f1(%rip), %al       # 0xa233
decb %al
cmpb $0x5, %al
ja 0x305c
movzbl %al, %eax
leaq 0x349e(%rip), %rcx      # 0x64f0
movzwl (%rcx,%rax,2), %eax
movl %eax, 0x14(%rsp)
jmp 0x3064
movl $0x0, 0x14(%rsp)
movzwl 0x71cb(%rip), %eax      # 0xa236
movw %ax, 0x12(%rsp)
movb 0x71bc(%rip), %al       # 0xa232
movb %al, 0x8(%rsp)
movb $0x1, %r15b
movb 0x71b5(%rip), %al       # 0xa238
movb %al, 0x9(%rsp)
xorl %ebp, %ebp
movq %rbx, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq 0x3443
testl %eax, %eax
je 0x30ab
testb $0x1, %r15b
movl $0x0, %r15d
jne 0x3089
jmp 0x3315
movb $0x1, %r13b
leaq 0x3c(%rsp), %r15
leaq 0x30(%rsp), %r12
xorl %ebp, %ebp
movq %r15, %rdi
movq %r12, %rsi
movl $0x1, %edx
callq 0x3443
testl %eax, %eax
je 0x30df
testb $0x1, %r13b
movl $0x0, %r13d
jne 0x30ba
jmp 0x3315
movq 0x713a(%rip), %rax      # 0xa220
movzbl (%rax), %edi
leaq 0xe(%rsp), %rcx
movl $0xd, %esi
movl $0x4, %edx
callq 0x4e41
cmpb $-0x1, %al
je 0x3197
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 0x33ad(%rip), %xmm0     # 0x6504
movss 0x33a9(%rip), %xmm1     # 0x6508
movb 0xb(%rsp,%rax), %cl
testb %cl, %cl
je 0x3186
movaps %xmm0, %xmm2
decb %cl
je 0x3179
movaps %xmm0, %xmm2
mulss %xmm1, %xmm2
decb %cl
jne 0x3171
movss %xmm2, 0x1c(%rsp,%rax,4)
movb $0x0, 0xb(%rsp,%rax)
jmp 0x318e
movl $0x0, 0x1c(%rsp,%rax,4)
incq %rax
cmpq $0x3, %rax
jne 0x315f
movss 0x7069(%rip), %xmm0     # 0xa208
movss 0x7065(%rip), %xmm1     # 0xa20c
xorl %eax, %eax
movss 0x705f(%rip), %xmm2     # 0xa210
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 0x2ebb(%rip), %xmm4     # 0x6090
movss 0x1c(%rsp,%rax,4), %xmm5
ucomiss %xmm3, %xmm5
jne 0x31e2
jnp 0x31fc
divss %xmm5, %xmm4
addss 0x2eb6(%rip), %xmm4     # 0x60a4
andps 0x32bb(%rip), %xmm4     # 0x64b0
ucomiss %xmm2, %xmm4
ja 0x3206
jmp 0x3209
ucomiss %xmm4, %xmm0
ja 0x3206
ucomiss %xmm1, %xmm4
jbe 0x3209
btsl %eax, %ebp
incq %rax
cmpq $0x3, %rax
jne 0x31b6
movq 0x7007(%rip), %rax      # 0xa220
movzbl (%rax), %edi
leaq 0x1c(%rsp), %rcx
movl $0xd, %esi
movl $0x3, %edx
callq 0x4e41
xorl %ecx, %ecx
cmpb $-0x1, %al
je 0x3306
andb $0x1f, 0x1c(%rsp)
andb $0x1f, 0x1d(%rsp)
andb $0x1f, 0x1e(%rsp)
movl 0x6f9d(%rip), %eax      # 0xa1ec
xorps %xmm1, %xmm1
cvtsi2ss %rax, %xmm1
movss 0x32ad(%rip), %xmm0     # 0x650c
divss %xmm1, %xmm0
movss 0x6f99(%rip), %xmm1     # 0xa204
movss 0x6f89(%rip), %xmm2     # 0xa1fc
movss 0x6f85(%rip), %xmm3     # 0xa200
movss 0x328d(%rip), %xmm4     # 0x6510
xorl %eax, %eax
movss 0x2e03(%rip), %xmm7     # 0x6090
movss 0x2e0e(%rip), %xmm8     # 0x60a4
movaps 0x3212(%rip), %xmm9     # 0x64b0
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 0x1c(%rsp,%rax), %dl
testb %dl, %dl
je 0x32eb
movaps %xmm0, %xmm6
decb %dl
je 0x32d2
movaps %xmm0, %xmm6
mulss %xmm4, %xmm6
decb %dl
jne 0x32ca
movb $0x0, 0x1c(%rsp,%rax)
divss %xmm6, %xmm5
addss %xmm8, %xmm5
andps %xmm9, %xmm5
ucomiss %xmm1, %xmm5
ja 0x32f5
jmp 0x32f8
ucomiss %xmm5, %xmm2
ja 0x32f5
ucomiss %xmm3, %xmm5
jbe 0x32f8
btsl %eax, %ecx
incq %rax
cmpq $0x3, %rax
jne 0x329e
testb %cl, %cl
sete %cl
leal 0x2(%rcx), %eax
testb %bpl, %bpl
movzbl %cl, %ecx
movzbl %al, %ebp
cmovnel %ecx, %ebp
movw $0xffff, 0x6f18(%rip)   # imm = 0xFFFF
movb $-0x1, 0x6f13(%rip)     # 0xa238
movl $0xffffffff, 0x6f01(%rip) # imm = 0xFFFFFFFF
movb $0x1, 0x6efe(%rip)      # 0xa234
movzwl 0x28(%rsp), %edi
callq 0x1ec8
movzbl 0x18(%rsp), %edi
callq 0x1f6b
movzwl 0x14(%rsp), %edi
callq 0x2002
movzwl 0x12(%rsp), %edi
callq 0x20b0
movzbl 0x8(%rsp), %edi
callq 0x233e
movzbl 0x9(%rsp), %edi
callq 0x2179
cmpb $0x0, 0xa(%rsp)
je 0x3383
movl $0x1, %edi
callq 0x3394
movl %ebp, %eax
addq $0x48, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq