// File: /workspace/github_c_makefile_star2_O1/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, %bpl
cmpb $0x0, 0x7575(%rip)      # 0xa244
je 0x2ce2
movl %ebp, %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, 0x753b(%rip)      # 0xa22a
je 0x2cd1
movb 0x7539(%rip), %al       # 0xa230
testb %al, %al
je 0x2cd1
movq %rcx, %r13
movq %rsi, %r12
movq %rdi, %r14
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, %r15d
cmovel %eax, %r15d
movq 0x74e1(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x74cf(%rip), %rax      # 0xa210
movzbl 0xc(%rax), %esi
leaq 0x8(%rsp), %rcx
movl $0x2, %edx
callq 0x4dda
cmpb $-0x1, %al
je 0x2cd1
movzwl 0x8(%rsp), %eax
rolw $0x8, %ax
movzwl %ax, %edx
movzbl %r15b, %r15d
cmpl %r15d, %edx
jae 0x2d78
xorl %ebp, %ebp
jmp 0x2cd1
movq 0x7499(%rip), %rax      # 0xa218
movzwl 0x2(%rax), %ecx
shrl %ecx
movl %edx, 0x14(%rsp)
cmpl %ecx, %edx
jbe 0x2dbd
movzbl (%rax), %edi
movq 0x7479(%rip), %rax      # 0xa210
movzbl 0x13(%rax), %esi
leaq 0x8(%rsp), %rcx
movl $0x1, %edx
callq 0x4dda
cmpb $-0x1, %al
je 0x2cd1
testb $0x10, 0x8(%rsp)
jne 0x2ebd
movq 0x7454(%rip), %rax      # 0xa218
movzbl (%rax), %edi
movq 0x7442(%rip), %rax      # 0xa210
movzbl 0xd(%rax), %esi
leaq 0x8(%rsp), %rcx
movl %r15d, %edx
callq 0x4dda
cmpb $-0x1, %al
je 0x2cd1
xorl %ecx, %ecx
movl 0x14(%rsp), %eax
xorl %edx, %edx
divl %r15d
decb %al
movb %al, (%r13)
movb $0x0, (%rbx)
testb $0x8, 0x742e(%rip)      # 0xa230
je 0x2e37
movzwl 0x8(%rsp), %eax
rolw $0x8, %ax
movw %ax, (%r12)
movzwl 0xa(%rsp), %eax
rolw $0x8, %ax
movw %ax, 0x2(%r12)
movzwl 0xc(%rsp), %eax
rolw $0x8, %ax
movw %ax, 0x4(%r12)
orb $0x8, (%rbx)
movw $0x6, %cx
movzwl %cx, %eax
cmpl %r15d, %eax
je 0x2e65
testb $0x40, 0x73ea(%rip)     # 0xa230
je 0x2e65
movzbl 0x8(%rsp,%rax), %ecx
shll $0x8, %ecx
movl %eax, %edx
movzbl 0x9(%rsp,%rdx), %edx
orl %ecx, %edx
movw %dx, (%r14)
orb $0x40, (%rbx)
addl $0x2, %eax
movl %eax, %ecx
movzwl %cx, %eax
cmpl %r15d, %eax
je 0x2e8c
testb $0x20, 0x73bc(%rip)     # 0xa230
je 0x2e8c
movzwl 0x8(%rsp,%rax), %ecx
rolw $0x8, %cx
movw %cx, 0x2(%r14)
orb $0x20, (%rbx)
addl $0x2, %eax
movl %eax, %ecx
movzwl %cx, %eax
cmpl %r15d, %eax
je 0x2d71
testb $0x10, 0x7391(%rip)     # 0xa230
je 0x2d71
movl %eax, %eax
movzwl 0x8(%rsp,%rax), %eax
rolw $0x8, %ax
movw %ax, 0x4(%r14)
orb $0x10, (%rbx)
jmp 0x2d71
callq 0x289f
movb $0x2, %bpl
jmp 0x2cd1