// File: /workspace/github_c_makefile_star2_O0/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
movq %rsp, %rbp
subq $0x40, %rsp
movq %rdi, -0x10(%rbp)
movq %rsi, -0x18(%rbp)
movq %rdx, -0x20(%rbp)
movq %rcx, -0x28(%rbp)
movb $0x0, -0x35(%rbp)
movw $0x0, -0x3a(%rbp)
cmpb $0x0, 0x87db(%rip)      # 0xc244
je 0x3a74
movb $0x1, -0x1(%rbp)
jmp 0x3e2d
movq -0x20(%rbp), %rax
movb $0x0, (%rax)
cmpb $0x0, 0x87a8(%rip)      # 0xc22a
jne 0x3a8d
movb $0x1, -0x1(%rbp)
jmp 0x3e2d
cmpb $0x0, 0x879c(%rip)      # 0xc230
jne 0x3a9f
movb $0x1, -0x1(%rbp)
jmp 0x3e2d
movzbl 0x878a(%rip), %eax      # 0xc230
andl $0x40, %eax
cmpl $0x0, %eax
je 0x3ab8
movzbl -0x35(%rbp), %eax
addl $0x2, %eax
movb %al, -0x35(%rbp)
movzbl 0x8771(%rip), %eax      # 0xc230
andl $0x20, %eax
cmpl $0x0, %eax
je 0x3ad1
movzbl -0x35(%rbp), %eax
addl $0x2, %eax
movb %al, -0x35(%rbp)
movzbl 0x8758(%rip), %eax      # 0xc230
andl $0x10, %eax
cmpl $0x0, %eax
je 0x3aea
movzbl -0x35(%rbp), %eax
addl $0x2, %eax
movb %al, -0x35(%rbp)
movzbl 0x873f(%rip), %eax      # 0xc230
andl $0x8, %eax
cmpl $0x0, %eax
je 0x3b03
movzbl -0x35(%rbp), %eax
addl $0x6, %eax
movb %al, -0x35(%rbp)
movq 0x870e(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x86fc(%rip), %rax      # 0xc210
movb 0xc(%rax), %al
leaq -0x34(%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 0x3b4a
movb $0x1, -0x1(%rbp)
jmp 0x3e2d
movzbl -0x34(%rbp), %eax
shll $0x8, %eax
movzbl -0x33(%rbp), %ecx
orl %ecx, %eax
movw %ax, -0x38(%rbp)
movzwl -0x38(%rbp), %eax
movzbl -0x35(%rbp), %ecx
cmpl %ecx, %eax
jge 0x3b70
movb $0x0, -0x1(%rbp)
jmp 0x3e2d
movzwl -0x38(%rbp), %eax
movq 0x869d(%rip), %rcx      # 0xc218
movzwl 0x2(%rcx), %ecx
sarl %ecx
cmpl %ecx, %eax
jle 0x3be8
movq 0x868c(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x867a(%rip), %rax      # 0xc210
movb 0x13(%rax), %al
leaq -0x34(%rbp), %rcx
movl $0x1, %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 0x3bcc
movb $0x1, -0x1(%rbp)
jmp 0x3e2d
movzbl -0x34(%rbp), %eax
andl $0x10, %eax
cmpl $0x0, %eax
je 0x3be6
callq 0x3270
movb $0x2, -0x1(%rbp)
jmp 0x3e2d
jmp 0x3be8
movq 0x8629(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x8617(%rip), %rax      # 0xc210
movb 0xd(%rax), %dl
movb -0x35(%rbp), %al
leaq -0x34(%rbp), %rcx
movzbl %sil, %edi
movzbl %dl, %esi
movzbl %al, %edx
callq 0x6df0
movsbl %al, %edx
movl $0x1, %eax
xorl %ecx, %ecx
cmpl $-0x1, %edx
cmovnel %ecx, %eax
cmpl $0x0, %eax
je 0x3c30
movb $0x1, -0x1(%rbp)
jmp 0x3e2d
movzwl -0x38(%rbp), %eax
movzbl -0x35(%rbp), %ecx
cltd
idivl %ecx
subl $0x1, %eax
movb %al, %cl
movq -0x28(%rbp), %rax
movb %cl, (%rax)
movq -0x20(%rbp), %rax
movb $0x0, (%rax)
movzwl -0x3a(%rbp), %eax
movzbl -0x35(%rbp), %ecx
cmpl %ecx, %eax
je 0x3d0d
movzbl 0x85cc(%rip), %eax      # 0xc230
andl $0x8, %eax
cmpl $0x0, %eax
je 0x3d0d
movzwl -0x3a(%rbp), %eax
addl $0x0, %eax
cltq
movzbl -0x34(%rbp,%rax), %eax
shll $0x8, %eax
movzwl -0x3a(%rbp), %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
movzbl -0x34(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x18(%rbp), %rax
movw %cx, (%rax)
movzwl -0x3a(%rbp), %eax
addl $0x2, %eax
cltq
movzbl -0x34(%rbp,%rax), %eax
shll $0x8, %eax
movzwl -0x3a(%rbp), %ecx
addl $0x3, %ecx
movslq %ecx, %rcx
movzbl -0x34(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x18(%rbp), %rax
movw %cx, 0x2(%rax)
movzwl -0x3a(%rbp), %eax
addl $0x4, %eax
cltq
movzbl -0x34(%rbp,%rax), %eax
shll $0x8, %eax
movzwl -0x3a(%rbp), %ecx
addl $0x5, %ecx
movslq %ecx, %rcx
movzbl -0x34(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x18(%rbp), %rax
movw %cx, 0x4(%rax)
movq -0x20(%rbp), %rax
movzbl (%rax), %ecx
orl $0x8, %ecx
movb %cl, (%rax)
movzwl -0x3a(%rbp), %eax
addl $0x6, %eax
movw %ax, -0x3a(%rbp)
movzwl -0x3a(%rbp), %eax
movzbl -0x35(%rbp), %ecx
cmpl %ecx, %eax
je 0x3d6b
movzbl 0x8510(%rip), %eax      # 0xc230
andl $0x40, %eax
cmpl $0x0, %eax
je 0x3d6b
movzwl -0x3a(%rbp), %eax
addl $0x0, %eax
cltq
movzbl -0x34(%rbp,%rax), %eax
shll $0x8, %eax
movzwl -0x3a(%rbp), %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
movzbl -0x34(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x10(%rbp), %rax
movw %cx, (%rax)
movq -0x20(%rbp), %rax
movzbl (%rax), %ecx
orl $0x40, %ecx
movb %cl, (%rax)
movzwl -0x3a(%rbp), %eax
addl $0x2, %eax
movw %ax, -0x3a(%rbp)
movzwl -0x3a(%rbp), %eax
movzbl -0x35(%rbp), %ecx
cmpl %ecx, %eax
je 0x3dca
movzbl 0x84b2(%rip), %eax      # 0xc230
andl $0x20, %eax
cmpl $0x0, %eax
je 0x3dca
movzwl -0x3a(%rbp), %eax
addl $0x0, %eax
cltq
movzbl -0x34(%rbp,%rax), %eax
shll $0x8, %eax
movzwl -0x3a(%rbp), %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
movzbl -0x34(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x10(%rbp), %rax
movw %cx, 0x2(%rax)
movq -0x20(%rbp), %rax
movzbl (%rax), %ecx
orl $0x20, %ecx
movb %cl, (%rax)
movzwl -0x3a(%rbp), %eax
addl $0x2, %eax
movw %ax, -0x3a(%rbp)
movzwl -0x3a(%rbp), %eax
movzbl -0x35(%rbp), %ecx
cmpl %ecx, %eax
je 0x3e29
movzbl 0x8453(%rip), %eax      # 0xc230
andl $0x10, %eax
cmpl $0x0, %eax
je 0x3e29
movzwl -0x3a(%rbp), %eax
addl $0x0, %eax
cltq
movzbl -0x34(%rbp,%rax), %eax
shll $0x8, %eax
movzwl -0x3a(%rbp), %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
movzbl -0x34(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x10(%rbp), %rax
movw %cx, 0x4(%rax)
movq -0x20(%rbp), %rax
movzbl (%rax), %ecx
orl $0x10, %ecx
movb %cl, (%rax)
movzwl -0x3a(%rbp), %eax
addl $0x2, %eax
movw %ax, -0x3a(%rbp)
movb $0x0, -0x1(%rbp)
movb -0x1(%rbp), %al
addq $0x40, %rsp
popq %rbp
retq
nopw %cs:(%rax,%rax)