// File: /workspace/github_c_makefile_star2_O3/ZombiesYard[P]MPU6050-DMP-raspberry-pi/MotionSensor/inv_mpu_lib/inv_mpu.c
// Function: get_st_biases(int*, int*, unsigned char)
static int get_st_biases(int32_t *gyro, int32_t *accel, uint8_t hw_test)
{
	uint8_t data[MAX_PACKET_LENGTH];
	uint8_t packet_count, ii;
	uint16_t fifo_count;

	data[0] = 0x01;
	data[1] = 0;
	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
		return 1;
	delay_ms(200);
	data[0] = 0;
	if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
		return 1;
	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
		return 1;
	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
		return 1;
	if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
		return 1;
	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
		return 1;
	data[0] = BIT_FIFO_RST | BIT_DMP_RST;
	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
		return 1;
	delay_ms(15);
	data[0] = st.test->reg_lpf;
	if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
		return 1;
	data[0] = st.test->reg_rate_div;
	if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
		return 1;
	if (hw_test)
		data[0] = st.test->reg_gyro_fsr | 0xE0;
	else
		data[0] = st.test->reg_gyro_fsr;
	if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
		return 1;

	if (hw_test)
		data[0] = st.test->reg_accel_fsr | 0xE0;
	else
		data[0] = test.reg_accel_fsr;
	if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
		return 1;
	if (hw_test)
		delay_ms(200);

	/* Fill FIFO for test.wait_ms milliseconds. */
	data[0] = BIT_FIFO_EN;
	if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
		return 1;

	data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
		return 1;
	delay_ms(test.wait_ms);
	data[0] = 0;
	if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
		return 1;

	if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
		return 1;

	fifo_count = (data[0] << 8) | data[1];
	packet_count = fifo_count / MAX_PACKET_LENGTH;
	gyro[0] = gyro[1] = gyro[2] = 0;
	accel[0] = accel[1] = accel[2] = 0;

	for (ii = 0; ii < packet_count; ii++)
	{
		int16_t accel_cur[3], gyro_cur[3];
		if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
			return 1;
		accel_cur[0] = ((int16_t)data[0] << 8) | data[1];
		accel_cur[1] = ((int16_t)data[2] << 8) | data[3];
		accel_cur[2] = ((int16_t)data[4] << 8) | data[5];
		accel[0] += (int32_t)accel_cur[0];
		accel[1] += (int32_t)accel_cur[1];
		accel[2] += (int32_t)accel_cur[2];
		gyro_cur[0] = (((int16_t)data[6] << 8) | data[7]);
		gyro_cur[1] = (((int16_t)data[8] << 8) | data[9]);
		gyro_cur[2] = (((int16_t)data[10] << 8) | data[11]);
		gyro[0] += (int32_t)gyro_cur[0];
		gyro[1] += (int32_t)gyro_cur[1];
		gyro[2] += (int32_t)gyro_cur[2];
	}
#ifdef EMPL_NO_64BIT
	gyro[0] = (int32_t)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
	gyro[1] = (int32_t)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
	gyro[2] = (int32_t)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
	if (has_accel)
	{
		accel[0] = (int32_t)(((float)accel[0]*65536.f) / test.accel_sens /
												 packet_count);
		accel[1] = (int32_t)(((float)accel[1]*65536.f) / test.accel_sens /
												 packet_count);
		accel[2] = (int32_t)(((float)accel[2]*65536.f) / test.accel_sens /
												 packet_count);
		/* Don't remove gravity! */
		accel[2] -= 65536L;
	}
#else
	gyro[0] = (int32_t)(((int64_t)gyro[0]<<16) / test.gyro_sens / packet_count);
	gyro[1] = (int32_t)(((int64_t)gyro[1]<<16) / test.gyro_sens / packet_count);
	gyro[2] = (int32_t)(((int64_t)gyro[2]<<16) / test.gyro_sens / packet_count);
	accel[0] = (int32_t)(((int64_t)accel[0]<<16) / test.accel_sens /
											 packet_count);
	accel[1] = (int32_t)(((int64_t)accel[1]<<16) / test.accel_sens /
											 packet_count);
	accel[2] = (int32_t)(((int64_t)accel[2]<<16) / test.accel_sens /
											 packet_count);
	/* Don't remove gravity! */
	if (accel[2] > 0L)
		accel[2] -= 65536L;
	else
		accel[2] += 65536L;
#endif

	return 0;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x18, %rsp
movl %edx, %r15d
movq %rsi, %rbx
movq %rdi, %r14
leaq 0xa(%rsp), %rcx
movw $0x1, (%rcx)
movq 0x6db5(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6da3(%rip), %rax      # 0xa218
movzbl 0x15(%rax), %esi
movl $0x2, %edx
callq 0x50a2
movl $0x1, %ebp
testl %eax, %eax
jne 0x3984
movl $0x30d40, %edi          # imm = 0x30D40
callq 0x10e0
leaq 0xa(%rsp), %rcx
movb $0x0, (%rcx)
movq 0x6d77(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6d65(%rip), %rax      # 0xa218
movzbl 0x11(%rax), %esi
movl $0x1, %ebp
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
movq 0x6d4b(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6d39(%rip), %rax      # 0xa218
movzbl 0x5(%rax), %esi
movl $0x1, %ebp
leaq 0xa(%rsp), %rcx
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
movq 0x6d1a(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6d08(%rip), %rax      # 0xa218
movzbl 0x15(%rax), %esi
movl $0x1, %ebp
leaq 0xa(%rsp), %rcx
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
movq 0x6ce9(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6cd7(%rip), %rax      # 0xa218
movzbl 0x1a(%rax), %esi
movl $0x1, %ebp
leaq 0xa(%rsp), %rcx
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
movq 0x6cb8(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6ca6(%rip), %rax      # 0xa218
movzbl 0x4(%rax), %esi
movl $0x1, %ebp
leaq 0xa(%rsp), %rcx
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
leaq 0xa(%rsp), %rcx
movb $0xc, (%rcx)
movq 0x6c7f(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6c6d(%rip), %rax      # 0xa218
movzbl 0x4(%rax), %esi
movl $0x1, %ebp
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
movl $0x3a98, %edi           # imm = 0x3A98
callq 0x10e0
movq 0x6c51(%rip), %rax      # 0xa228
movb 0x9(%rax), %al
leaq 0xa(%rsp), %rcx
movb %al, (%rcx)
movq 0x6c38(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6c26(%rip), %rax      # 0xa218
movzbl 0x2(%rax), %esi
movl $0x1, %ebp
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
movq 0x6c14(%rip), %rax      # 0xa228
movb 0x8(%rax), %al
leaq 0xa(%rsp), %rcx
movb %al, (%rcx)
movq 0x6bfb(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6be9(%rip), %rax      # 0xa218
movzbl 0x1(%rax), %esi
movl $0x1, %ebp
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
movq 0x6bd7(%rip), %rax      # 0xa228
movzbl 0xa(%rax), %eax
movl %eax, %ecx
orb $-0x20, %cl
testb %r15b, %r15b
movzbl %cl, %edx
cmovel %eax, %edx
leaq 0xa(%rsp), %rcx
movb %dl, (%rcx)
movq 0x6baf(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6b9d(%rip), %rax      # 0xa218
movzbl 0x6(%rax), %esi
movl $0x1, %ebp
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
testb %r15b, %r15b
je 0x36a9
movq 0x6b86(%rip), %rax      # 0xa228
movb 0xb(%rax), %al
orb $-0x20, %al
jmp 0x36af
movb 0x6b48(%rip), %al       # 0xa1f7
leaq 0xa(%rsp), %rcx
movb %al, (%rcx)
movq 0x6b63(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6b51(%rip), %rax      # 0xa218
movzbl 0x7(%rax), %esi
movl $0x1, %ebp
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
testb %r15b, %r15b
je 0x36f1
movl $0x30d40, %edi          # imm = 0x30D40
callq 0x10e0
leaq 0xa(%rsp), %rcx
movb $0x40, (%rcx)
movq 0x6b20(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6b0e(%rip), %rax      # 0xa218
movzbl 0x4(%rax), %esi
movl $0x1, %ebp
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
leaq 0xa(%rsp), %rcx
movb $0x78, (%rcx)
movq 0x6aec(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6ada(%rip), %rax      # 0xa218
movzbl 0x5(%rax), %esi
movl $0x1, %ebp
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
movzwl 0x6a98(%rip), %eax      # 0xa1f8
imull $0x3e8, %eax, %edi      # imm = 0x3E8
callq 0x10e0
leaq 0xa(%rsp), %rcx
movb $0x0, (%rcx)
movq 0x6aa6(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6a94(%rip), %rax      # 0xa218
movzbl 0x5(%rax), %esi
movl $0x1, %ebp
movl $0x1, %edx
callq 0x50a2
testl %eax, %eax
jne 0x3984
movq 0x6a7a(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x6a68(%rip), %rax      # 0xa218
movzbl 0xc(%rax), %esi
leaq 0xa(%rsp), %rcx
movl $0x2, %edx
callq 0x4e41
cmpb $-0x1, %al
je 0x3984
movzwl 0xa(%rsp), %eax
rolw $0x8, %ax
movzwl %ax, %eax
imull $0xaaab, %eax, %r13d    # imm = 0xAAAB
shrl $0x13, %r13d
xorl %eax, %eax
movl %eax, 0x8(%r14)
movq %rax, (%r14)
movl %eax, 0x8(%rbx)
movq %rax, (%rbx)
movl %r13d, %eax
andw $0xff, %ax
movw %ax, 0x16(%rsp)
je 0x38c9
xorl %r12d, %r12d
leaq 0xa(%rsp), %r15
movq 0x6a0e(%rip), %rax      # 0xa220
movzbl (%rax), %edi
movq 0x69fc(%rip), %rax      # 0xa218
movzbl 0xd(%rax), %esi
movl $0xc, %edx
movq %r15, %rcx
callq 0x4e41
cmpb $-0x1, %al
je 0x3984
movzbl 0xa(%rsp), %eax
shll $0x8, %eax
movzbl 0xb(%rsp), %ecx
cwtl
orl %ecx, %eax
movzbl 0xc(%rsp), %ecx
shll $0x8, %ecx
movzbl 0xd(%rsp), %edx
movswl %cx, %ecx
orl %edx, %ecx
movzbl 0xe(%rsp), %edx
shll $0x8, %edx
movzbl 0xf(%rsp), %esi
addl %eax, (%rbx)
movswl %dx, %eax
addl %ecx, 0x4(%rbx)
orl %esi, %eax
addl %eax, 0x8(%rbx)
movzbl 0x10(%rsp), %eax
shll $0x8, %eax
movzbl 0x11(%rsp), %ecx
cwtl
orl %ecx, %eax
movzbl 0x12(%rsp), %ecx
shll $0x8, %ecx
movzbl 0x13(%rsp), %edx
movswl %cx, %edi
orl %edx, %edi
movzbl 0x14(%rsp), %ecx
shll $0x8, %ecx
movzbl 0x15(%rsp), %edx
addl (%r14), %eax
movswl %cx, %esi
movl %eax, (%r14)
addl 0x4(%r14), %edi
orl %edx, %esi
movl %edi, 0x4(%r14)
addl 0x8(%r14), %esi
movl %esi, 0x8(%r14)
incb %r12b
cmpb %r13b, %r12b
jb 0x380b
jmp 0x38d4
movl (%r14), %eax
movl 0x4(%r14), %edi
movl 0x8(%r14), %esi
cltq
shlq $0x10, %rax
movl 0x690c(%rip), %ecx      # 0xa1ec
cqto
idivq %rcx
movzwl 0x16(%rsp), %ecx
cqto
idivq %rcx
movl %eax, (%r14)
movslq %edi, %rax
shlq $0x10, %rax
movl 0x68ed(%rip), %edi      # 0xa1ec
cqto
idivq %rdi
cqto
idivq %rcx
movl %eax, 0x4(%r14)
movslq %esi, %rax
shlq $0x10, %rax
cqto
idivq %rdi
cqto
idivq %rcx
movl %eax, 0x8(%r14)
movslq (%rbx), %rax
shlq $0x10, %rax
movl 0x68c1(%rip), %esi      # 0xa1f0
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, (%rbx)
movslq 0x4(%rbx), %rax
shlq $0x10, %rax
movl 0x68a7(%rip), %esi      # 0xa1f0
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, 0x4(%rbx)
movslq 0x8(%rbx), %rax
shlq $0x10, %rax
movl 0x688c(%rip), %esi      # 0xa1f0
cqto
idivq %rsi
cqto
idivq %rcx
xorl %ecx, %ecx
testl %eax, %eax
setle %cl
shll $0x11, %ecx
addl %ecx, %eax
addl $0xffff0000, %eax       # imm = 0xFFFF0000
movl %eax, 0x8(%rbx)
xorl %ebp, %ebp
movl %ebp, %eax
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq