// File: /workspace/github_c_makefile_star2_O2/ZombiesYard[P]MPU6050-DMP-raspberry-pi/MotionSensor/inv_mpu_lib/inv_mpu.c
// Function: mpu_read_fifo(short*, short*, unsigned char*, unsigned char*)
uint8_t mpu_read_fifo(int16_t *gyro, int16_t *accel,
											uint8_t *sensors, uint8_t *more)
{
	/* Assumes maximum packet size is gyro (6) + accel (6). */
	uint8_t data[MAX_PACKET_LENGTH];
	uint8_t packet_size = 0;
	uint16_t fifo_count, index = 0;

	if (st.chip_cfg.dmp_on)
		return 1;

	sensors[0] = 0;
	if (!st.chip_cfg.sensors)
		return 1;
	if (!st.chip_cfg.fifo_enable)
		return 1;

	if (st.chip_cfg.fifo_enable & INV_X_GYRO)
		packet_size += 2;
	if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
		packet_size += 2;
	if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
		packet_size += 2;
	if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
		packet_size += 6;

	if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
		return 1;
	fifo_count = (data[0] << 8) | data[1];
	if (fifo_count < packet_size)
		return 0;
#if defined MPU_DEBUG
    printf_P("FIFO count: %hd\r\n", fifo_count);
#endif
	if (fifo_count > (st.hw->max_fifo >> 1))
	{
		/* FIFO is 50% full, better check overflow bit. */
		if (i2c_read(st.hw->addr, st.reg->int_status, 1, data))
			return 1;
		if (data[0] & BIT_FIFO_OVERFLOW)
		{
			mpu_reset_fifo();
			return 2;
		}
	}

	if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data))
		return 1;
	more[0] = fifo_count / packet_size - 1;
	sensors[0] = 0;

	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
	{
		accel[0] = (data[index+0] << 8) | data[index+1];
		accel[1] = (data[index+2] << 8) | data[index+3];
		accel[2] = (data[index+4] << 8) | data[index+5];
		sensors[0] |= INV_XYZ_ACCEL;
		index += 6;
	}
	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO)
	{
		gyro[0] = (data[index+0] << 8) | data[index+1];
		sensors[0] |= INV_X_GYRO;
		index += 2;
	}
	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO)
	{
		gyro[1] = (data[index+0] << 8) | data[index+1];
		sensors[0] |= INV_Y_GYRO;
		index += 2;
	}
	if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO)
	{
		gyro[2] = (data[index+0] << 8) | data[index+1];
		sensors[0] |= INV_Z_GYRO;
		index += 2;
	}

	return 0;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x18, %rsp
movb $0x1, %r15b
cmpb $0x0, 0x68dd(%rip)      # 0x923c
je 0x2973
movl %r15d, %eax
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rdx, %rbx
movb $0x0, (%rdx)
cmpb $0x0, 0x68a2(%rip)      # 0x9222
je 0x2961
movb 0x68a0(%rip), %al       # 0x9228
testb %al, %al
je 0x2961
movq %rcx, %r13
movq %rsi, %r12
movq %rdi, %rbp
movl %eax, %ecx
shrb $0x5, %cl
andb $0x2, %cl
movl %eax, %edx
shrb $0x4, %dl
andb $0x2, %dl
addb %cl, %dl
movl %eax, %ecx
shrb $0x3, %cl
andb $0x2, %cl
addb %dl, %cl
leal 0x6(%rcx), %edx
testb $0x8, %al
movzbl %cl, %eax
movzbl %dl, %r14d
cmovel %eax, %r14d
movq 0x6848(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x6836(%rip), %rax      # 0x9208
movzbl 0xc(%rax), %esi
pushq $0x2
popq %rdx
leaq 0x4(%rsp), %rcx
callq 0x4847
cmpb $-0x1, %al
je 0x2961
movq %rbp, 0x10(%rsp)
movzwl 0x4(%rsp), %ebp
rolw $0x8, %bp
movzwl %bp, %ecx
movzbl %r14b, %r14d
cmpl %r14d, %ecx
jae 0x2a0d
xorl %r15d, %r15d
jmp 0x2961
movq 0x67fc(%rip), %rax      # 0x9210
movzwl 0x2(%rax), %edx
shrl %edx
cmpl %edx, %ecx
jbe 0x2a53
movzbl (%rax), %edi
movq 0x67e0(%rip), %rax      # 0x9208
movzbl 0x13(%rax), %esi
pushq $0x1
popq %rdx
leaq 0x4(%rsp), %rcx
callq 0x4847
cmpb $-0x1, %al
je 0x2961
testb $0x10, 0x4(%rsp)
jne 0x2b65
movq 0x67bd(%rip), %rax      # 0x9210
movzbl (%rax), %edi
movq 0x67ab(%rip), %rax      # 0x9208
movzbl 0xd(%rax), %esi
leaq 0x4(%rsp), %rcx
movl %r14d, %edx
callq 0x4847
cmpb $-0x1, %al
je 0x2961
xorl %ecx, %ecx
movl %ebp, %eax
xorl %edx, %edx
divw %r14w
decb %al
movb %al, (%r13)
movb $0x0, (%rbx)
testb $0x8, 0x6798(%rip)      # 0x9228
je 0x2ac5
movzwl 0x4(%rsp), %eax
rolw $0x8, %ax
movw %ax, (%r12)
movzwl 0x6(%rsp), %eax
rolw $0x8, %ax
movw %ax, 0x2(%r12)
movzwl 0x8(%rsp), %eax
rolw $0x8, %ax
movw %ax, 0x4(%r12)
orb $0x8, (%rbx)
movw $0x6, %cx
movzwl %cx, %eax
cmpl %r14d, %eax
movq 0x10(%rsp), %rsi
je 0x2af7
testb $0x40, 0x674f(%rip)     # 0x9228
je 0x2af7
movzbl 0x4(%rsp,%rax), %ecx
shll $0x8, %ecx
movl %eax, %edx
movzbl 0x5(%rsp,%rdx), %edx
orl %ecx, %edx
movw %dx, (%rsi)
orb $0x40, (%rbx)
addl $0x2, %eax
movl %eax, %ecx
cmpl %r14d, %eax
je 0x2b2a
testb $0x20, 0x6725(%rip)     # 0x9228
je 0x2b2a
movzwl %cx, %ecx
movzbl 0x4(%rsp,%rcx), %edx
shll $0x8, %edx
movl %eax, %eax
leaq 0x4(%rsp), %rdi
movzbl 0x1(%rax,%rdi), %eax
orl %edx, %eax
movw %ax, 0x2(%rsi)
orb $0x20, (%rbx)
addl $0x2, %ecx
movl %ecx, %eax
cmpl %r14d, %eax
je 0x2a05
testb $0x10, 0x66ee(%rip)     # 0x9228
je 0x2a05
movzwl %cx, %ecx
movzbl 0x4(%rsp,%rcx), %ecx
shll $0x8, %ecx
movl %eax, %eax
leaq 0x4(%rsp), %rdx
movzbl 0x1(%rax,%rdx), %eax
orl %ecx, %eax
movw %ax, 0x4(%rsi)
orb $0x10, (%rbx)
jmp 0x2a05
callq 0x2526
movb $0x2, %r15b
jmp 0x2961