// File: /workspace/github_c_makefile_star2_O0/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
movq %rsp, %rbp
subq $0x40, %rsp
movb %dl, %al
movq %rdi, -0x10(%rbp)
movq %rsi, -0x18(%rbp)
movb %al, -0x19(%rbp)
movb $0x1, -0x25(%rbp)
movb $0x0, -0x24(%rbp)
movq 0x7f64(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7f52(%rip), %rax      # 0xc210
movb 0x15(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x2, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x42e7
movl $0x1, -0x4(%rbp)
jmp 0x4987
movl $0x30d40, %edi          # imm = 0x30D40
callq 0x10f0
movb $0x0, -0x25(%rbp)
movq 0x7f1c(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7f0a(%rip), %rax      # 0xc210
movb 0x11(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x432f
movl $0x1, -0x4(%rbp)
jmp 0x4987
movq 0x7ee2(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7ed0(%rip), %rax      # 0xc210
movb 0x5(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x4369
movl $0x1, -0x4(%rbp)
jmp 0x4987
movq 0x7ea8(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7e96(%rip), %rax      # 0xc210
movb 0x15(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x43a3
movl $0x1, -0x4(%rbp)
jmp 0x4987
movq 0x7e6e(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7e5c(%rip), %rax      # 0xc210
movb 0x1a(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x43dd
movl $0x1, -0x4(%rbp)
jmp 0x4987
movq 0x7e34(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7e22(%rip), %rax      # 0xc210
movb 0x4(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x4417
movl $0x1, -0x4(%rbp)
jmp 0x4987
movb $0xc, -0x25(%rbp)
movq 0x7df6(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7de4(%rip), %rax      # 0xc210
movb 0x4(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x4455
movl $0x1, -0x4(%rbp)
jmp 0x4987
movl $0x3a98, %edi           # imm = 0x3A98
callq 0x10f0
movq 0x7dba(%rip), %rax      # 0xc220
movb 0x9(%rax), %al
movb %al, -0x25(%rbp)
movq 0x7da5(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7d93(%rip), %rax      # 0xc210
movb 0x2(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x44a6
movl $0x1, -0x4(%rbp)
jmp 0x4987
movq 0x7d73(%rip), %rax      # 0xc220
movb 0x8(%rax), %al
movb %al, -0x25(%rbp)
movq 0x7d5e(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7d4c(%rip), %rax      # 0xc210
movb 0x1(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x44ed
movl $0x1, -0x4(%rbp)
jmp 0x4987
cmpb $0x0, -0x19(%rbp)
je 0x4508
movq 0x7d26(%rip), %rax      # 0xc220
movzbl 0xa(%rax), %eax
orl $0xe0, %eax
movb %al, -0x25(%rbp)
jmp 0x4515
movq 0x7d11(%rip), %rax      # 0xc220
movb 0xa(%rax), %al
movb %al, -0x25(%rbp)
movq 0x7cfc(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7cea(%rip), %rax      # 0xc210
movb 0x6(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x454f
movl $0x1, -0x4(%rbp)
jmp 0x4987
cmpb $0x0, -0x19(%rbp)
je 0x456a
movq 0x7cc4(%rip), %rax      # 0xc220
movzbl 0xb(%rax), %eax
orl $0xe0, %eax
movb %al, -0x25(%rbp)
jmp 0x4573
movb 0x7c7f(%rip), %al       # 0xc1ef
movb %al, -0x25(%rbp)
movq 0x7c9e(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7c8c(%rip), %rax      # 0xc210
movb 0x7(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x45ad
movl $0x1, -0x4(%rbp)
jmp 0x4987
cmpb $0x0, -0x19(%rbp)
je 0x45bd
movl $0x30d40, %edi          # imm = 0x30D40
callq 0x10f0
movb $0x40, -0x25(%rbp)
movq 0x7c50(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7c3e(%rip), %rax      # 0xc210
movb 0x4(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x45fb
movl $0x1, -0x4(%rbp)
jmp 0x4987
movb $0x78, -0x25(%rbp)
movq 0x7c12(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7c00(%rip), %rax      # 0xc210
movb 0x5(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x4639
movl $0x1, -0x4(%rbp)
jmp 0x4987
movzwl 0x7bb0(%rip), %eax      # 0xc1f0
imull $0x3e8, %eax, %edi      # imm = 0x3E8
callq 0x10f0
movb $0x0, -0x25(%rbp)
movq 0x7bc2(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7bb0(%rip), %rax      # 0xc210
movb 0x5(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x4689
movl $0x1, -0x4(%rbp)
jmp 0x4987
movq 0x7b88(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7b76(%rip), %rax      # 0xc210
movb 0xc(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0x2, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x6df0
movsbl %al, %edx
movl $0x1, %eax
xorl %ecx, %ecx
cmpl $-0x1, %edx
cmovnel %ecx, %eax
cmpl $0x0, %eax
je 0x46d3
movl $0x1, -0x4(%rbp)
jmp 0x4987
movzbl -0x25(%rbp), %eax
shll $0x8, %eax
movzbl -0x24(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x2a(%rbp)
movzwl -0x2a(%rbp), %eax
movl $0xc, %ecx
cltd
idivl %ecx
movb %al, -0x26(%rbp)
movq -0x10(%rbp), %rax
movl $0x0, 0x8(%rax)
movq -0x10(%rbp), %rax
movl $0x0, 0x4(%rax)
movq -0x10(%rbp), %rax
movl $0x0, (%rax)
movq -0x18(%rbp), %rax
movl $0x0, 0x8(%rax)
movq -0x18(%rbp), %rax
movl $0x0, 0x4(%rax)
movq -0x18(%rbp), %rax
movl $0x0, (%rax)
movb $0x0, -0x27(%rbp)
movzbl -0x27(%rbp), %eax
movzbl -0x26(%rbp), %ecx
cmpl %ecx, %eax
jge 0x485a
movq 0x7aca(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7ab8(%rip), %rax      # 0xc210
movb 0xd(%rax), %al
leaq -0x25(%rbp), %rcx
movl $0xc, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x6df0
movsbl %al, %edx
movl $0x1, %eax
xorl %ecx, %ecx
cmpl $-0x1, %edx
cmovnel %ecx, %eax
cmpl $0x0, %eax
je 0x4791
movl $0x1, -0x4(%rbp)
jmp 0x4987
movzbl -0x25(%rbp), %eax
cwtl
shll $0x8, %eax
movzbl -0x24(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x30(%rbp)
movzbl -0x23(%rbp), %eax
cwtl
shll $0x8, %eax
movzbl -0x22(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x2e(%rbp)
movzbl -0x21(%rbp), %eax
cwtl
shll $0x8, %eax
movzbl -0x20(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x2c(%rbp)
movswl -0x30(%rbp), %ecx
movq -0x18(%rbp), %rax
addl (%rax), %ecx
movl %ecx, (%rax)
movswl -0x2e(%rbp), %ecx
movq -0x18(%rbp), %rax
addl 0x4(%rax), %ecx
movl %ecx, 0x4(%rax)
movswl -0x2c(%rbp), %ecx
movq -0x18(%rbp), %rax
addl 0x8(%rax), %ecx
movl %ecx, 0x8(%rax)
movzbl -0x1f(%rbp), %eax
cwtl
shll $0x8, %eax
movzbl -0x1e(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x36(%rbp)
movzbl -0x1d(%rbp), %eax
cwtl
shll $0x8, %eax
movzbl -0x1c(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x34(%rbp)
movzbl -0x1b(%rbp), %eax
cwtl
shll $0x8, %eax
movzbl -0x1a(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x32(%rbp)
movswl -0x36(%rbp), %ecx
movq -0x10(%rbp), %rax
addl (%rax), %ecx
movl %ecx, (%rax)
movswl -0x34(%rbp), %ecx
movq -0x10(%rbp), %rax
addl 0x4(%rax), %ecx
movl %ecx, 0x4(%rax)
movswl -0x32(%rbp), %ecx
movq -0x10(%rbp), %rax
addl 0x8(%rax), %ecx
movl %ecx, 0x8(%rax)
movb -0x27(%rbp), %al
addb $0x1, %al
movb %al, -0x27(%rbp)
jmp 0x4737
movq -0x10(%rbp), %rax
movslq (%rax), %rax
shlq $0x10, %rax
movl 0x7979(%rip), %ecx      # 0xc1e4
cqto
idivq %rcx
movzbl -0x26(%rbp), %ecx
cqto
idivq %rcx
movl %eax, %ecx
movq -0x10(%rbp), %rax
movl %ecx, (%rax)
movq -0x10(%rbp), %rax
movslq 0x4(%rax), %rax
shlq $0x10, %rax
movl 0x7951(%rip), %ecx      # 0xc1e4
cqto
idivq %rcx
movzbl -0x26(%rbp), %ecx
cqto
idivq %rcx
movl %eax, %ecx
movq -0x10(%rbp), %rax
movl %ecx, 0x4(%rax)
movq -0x10(%rbp), %rax
movslq 0x8(%rax), %rax
shlq $0x10, %rax
movl 0x7928(%rip), %ecx      # 0xc1e4
cqto
idivq %rcx
movzbl -0x26(%rbp), %ecx
cqto
idivq %rcx
movl %eax, %ecx
movq -0x10(%rbp), %rax
movl %ecx, 0x8(%rax)
movq -0x18(%rbp), %rax
movslq (%rax), %rax
shlq $0x10, %rax
movl 0x7904(%rip), %ecx      # 0xc1e8
cqto
idivq %rcx
movzbl -0x26(%rbp), %ecx
cqto
idivq %rcx
movl %eax, %ecx
movq -0x18(%rbp), %rax
movl %ecx, (%rax)
movq -0x18(%rbp), %rax
movslq 0x4(%rax), %rax
shlq $0x10, %rax
movl 0x78dc(%rip), %ecx      # 0xc1e8
cqto
idivq %rcx
movzbl -0x26(%rbp), %ecx
cqto
idivq %rcx
movl %eax, %ecx
movq -0x18(%rbp), %rax
movl %ecx, 0x4(%rax)
movq -0x18(%rbp), %rax
movslq 0x8(%rax), %rax
shlq $0x10, %rax
movl 0x78b3(%rip), %ecx      # 0xc1e8
cqto
idivq %rcx
movzbl -0x26(%rbp), %ecx
cqto
idivq %rcx
movl %eax, %ecx
movq -0x18(%rbp), %rax
movl %ecx, 0x8(%rax)
movq -0x18(%rbp), %rax
movslq 0x8(%rax), %rax
cmpq $0x0, %rax
jle 0x496e
movq -0x18(%rbp), %rax
movslq 0x8(%rax), %rcx
subq $0x10000, %rcx          # imm = 0x10000
movl %ecx, 0x8(%rax)
jmp 0x4980
movq -0x18(%rbp), %rax
movslq 0x8(%rax), %rcx
addq $0x10000, %rcx          # imm = 0x10000
movl %ecx, 0x8(%rax)
movl $0x0, -0x4(%rbp)
movl -0x4(%rbp), %eax
addq $0x40, %rsp
popq %rbp
retq