// File: /workspace/github_c_makefile_star2_O1/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, %ebp
movq %rsi, %rbx
movq %rdi, %r14
leaq 0xa(%rsp), %rcx
movw $0x1, (%rcx)
movq 0x6d90(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6d7e(%rip), %rax      # 0xa210
movzbl 0x15(%rax), %esi
movl $0x2, %edx
callq 0x5044
movl $0x1, %r15d
testl %eax, %eax
jne 0x37be
movl $0x30d40, %edi          # imm = 0x30D40
callq 0x10e0
leaq 0xa(%rsp), %rcx
movb $0x0, (%rcx)
movq 0x6d51(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6d3f(%rip), %rax      # 0xa210
movzbl 0x11(%rax), %esi
movl $0x1, %r15d
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
movq 0x6d24(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6d12(%rip), %rax      # 0xa210
movzbl 0x5(%rax), %esi
movl $0x1, %r15d
leaq 0xa(%rsp), %rcx
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
movq 0x6cf2(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6ce0(%rip), %rax      # 0xa210
movzbl 0x15(%rax), %esi
movl $0x1, %r15d
leaq 0xa(%rsp), %rcx
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
movq 0x6cc0(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6cae(%rip), %rax      # 0xa210
movzbl 0x1a(%rax), %esi
movl $0x1, %r15d
leaq 0xa(%rsp), %rcx
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
movq 0x6c8e(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6c7c(%rip), %rax      # 0xa210
movzbl 0x4(%rax), %esi
movl $0x1, %r15d
leaq 0xa(%rsp), %rcx
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
leaq 0xa(%rsp), %rcx
movb $0xc, (%rcx)
movq 0x6c54(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6c42(%rip), %rax      # 0xa210
movzbl 0x4(%rax), %esi
movl $0x1, %r15d
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
movl $0x3a98, %edi           # imm = 0x3A98
callq 0x10e0
movq 0x6c25(%rip), %rax      # 0xa220
movb 0x9(%rax), %al
leaq 0xa(%rsp), %rcx
movb %al, (%rcx)
movq 0x6c0c(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6bfa(%rip), %rax      # 0xa210
movzbl 0x2(%rax), %esi
movl $0x1, %r15d
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
movq 0x6be7(%rip), %rax      # 0xa220
movb 0x8(%rax), %al
leaq 0xa(%rsp), %rcx
movb %al, (%rcx)
movq 0x6bce(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6bbc(%rip), %rax      # 0xa210
movzbl 0x1(%rax), %esi
movl $0x1, %r15d
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
movq 0x6ba9(%rip), %rax      # 0xa220
movzbl 0xa(%rax), %eax
movl %eax, %ecx
orb $-0x20, %cl
testb %bpl, %bpl
movzbl %cl, %edx
cmovel %eax, %edx
leaq 0xa(%rsp), %rcx
movb %dl, (%rcx)
movq 0x6b81(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6b6f(%rip), %rax      # 0xa210
movzbl 0x6(%rax), %esi
movl $0x1, %r15d
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
testb %bpl, %bpl
je 0x36d0
movq 0x6b57(%rip), %rax      # 0xa220
movb 0xb(%rax), %al
orb $-0x20, %al
jmp 0x36d6
movb 0x6b19(%rip), %al       # 0xa1ef
leaq 0xa(%rsp), %rcx
movb %al, (%rcx)
movq 0x6b34(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6b22(%rip), %rax      # 0xa210
movzbl 0x7(%rax), %esi
movl $0x1, %r15d
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
testb %bpl, %bpl
je 0x3719
movl $0x30d40, %edi          # imm = 0x30D40
callq 0x10e0
leaq 0xa(%rsp), %rcx
movb $0x40, (%rcx)
movq 0x6af0(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6ade(%rip), %rax      # 0xa210
movzbl 0x4(%rax), %esi
movl $0x1, %r15d
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
leaq 0xa(%rsp), %rcx
movb $0x78, (%rcx)
movq 0x6abf(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6aad(%rip), %rax      # 0xa210
movzbl 0x5(%rax), %esi
movl $0x1, %r15d
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
jne 0x37be
movzwl 0x6a6e(%rip), %eax      # 0xa1f0
imull $0x3e8, %eax, %edi      # imm = 0x3E8
callq 0x10e0
leaq 0xa(%rsp), %rcx
movb $0x0, (%rcx)
movq 0x6a7c(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6a6a(%rip), %rax      # 0xa210
movzbl 0x5(%rax), %esi
movl $0x1, %edx
callq 0x5044
testl %eax, %eax
je 0x37d0
movl $0x1, %r15d
movl %r15d, %eax
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x6a41(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x6a2f(%rip), %rax      # 0xa210
movzbl 0xc(%rax), %esi
leaq 0xa(%rsp), %rcx
movl $0x2, %edx
callq 0x4dda
cmpb $-0x1, %al
je 0x37b8
movzwl 0xa(%rsp), %eax
rolw $0x8, %ax
movzwl %ax, %eax
imull $0xaaab, %eax, %r13d    # imm = 0xAAAB
shrl $0x13, %r13d
xorl %r15d, %r15d
movl %r15d, 0x8(%r14)
movq %r15, (%r14)
movl %r15d, 0x8(%rbx)
movq %r15, (%rbx)
movl %r13d, %eax
andw $0xff, %ax
movw %ax, 0x16(%rsp)
je 0x38ea
xorl %ebp, %ebp
leaq 0xa(%rsp), %r12
movq 0x69d8(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x69c6(%rip), %rax      # 0xa210
movzbl 0xd(%rax), %esi
movl $0xc, %edx
movq %r12, %rcx
callq 0x4dda
cmpb $-0x1, %al
je 0x37b8
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, %ecx
orl %edx, %ecx
movzbl 0x14(%rsp), %edx
shll $0x8, %edx
movzbl 0x15(%rsp), %esi
addl %eax, (%r14)
movswl %dx, %eax
addl %ecx, 0x4(%r14)
orl %esi, %eax
addl %eax, 0x8(%r14)
incb %bpl
cmpb %r13b, %bpl
jb 0x3839
movslq (%r14), %rax
shlq $0x10, %rax
movl 0x68ed(%rip), %ecx      # 0xa1e4
cqto
idivq %rcx
movzwl 0x16(%rsp), %ecx
cqto
idivq %rcx
movl %eax, (%r14)
movslq 0x4(%r14), %rax
shlq $0x10, %rax
movl 0x68cd(%rip), %esi      # 0xa1e4
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, 0x4(%r14)
movslq 0x8(%r14), %rax
shlq $0x10, %rax
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, 0x8(%r14)
movslq (%rbx), %rax
shlq $0x10, %rax
movl 0x68a0(%rip), %esi      # 0xa1e8
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, (%rbx)
movslq 0x4(%rbx), %rax
shlq $0x10, %rax
movl 0x6886(%rip), %esi      # 0xa1e8
cqto
idivq %rsi
cqto
idivq %rcx
movl %eax, 0x4(%rbx)
movslq 0x8(%rbx), %rax
shlq $0x10, %rax
movl 0x686b(%rip), %esi      # 0xa1e8
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)
jmp 0x37be