// File: /workspace/github_c_makefile_star2_O1/ZombiesYard[P]MPU6050-DMP-raspberry-pi/MotionSensor/inv_mpu_lib/inv_mpu_dmp_motion_driver.c
// Function: dmp_read_fifo(short*, short*, int*, short*, unsigned char*)
uint8_t dmp_read_fifo(int16_t *gyro, int16_t *accel, int32_t *quat, int16_t *sensors, uint8_t *more)
{
	uint8_t fifo_data[MAX_PACKET_LENGTH];
	uint8_t ii = 0;

	/* TODO: sensors[0] only changes when dmp_enable_feature is called. We can
	 * cache this value and save some cycles.
	 */
	sensors[0] = 0;

	/* Get a packet. */
	if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))
		return 1;

	/* Parse DMP packet. */
	if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
	{
#ifdef FIFO_CORRUPTION_CHECK
		int32_t quat_q14[4], quat_mag_sq;
#endif
		quat[0] = ((int32_t)fifo_data[0] << 24) | ((int32_t)fifo_data[1] << 16) |
							((int32_t)fifo_data[2] << 8) | fifo_data[3];
		quat[1] = ((int32_t)fifo_data[4] << 24) | ((int32_t)fifo_data[5] << 16) |
							((int32_t)fifo_data[6] << 8) | fifo_data[7];
		quat[2] = ((int32_t)fifo_data[8] << 24) | ((int32_t)fifo_data[9] << 16) |
							((int32_t)fifo_data[10] << 8) | fifo_data[11];
		quat[3] = ((int32_t)fifo_data[12] << 24) | ((int32_t)fifo_data[13] << 16) |
							((int32_t)fifo_data[14] << 8) | fifo_data[15];
		ii += 16;
#ifdef FIFO_CORRUPTION_CHECK
		/* We can detect a corrupted FIFO by monitoring the quaternion data and
		 * ensuring that the magnitude is always normalized to one. This
		 * shouldn't happen in normal operation, but if an I2C error occurs,
		 * the FIFO reads might become misaligned.
		 *
		 * Let's start by scaling down the quaternion data to avoid int64_t
		 * math.
		 */
		quat_q14[0] = quat[0] >> 16;
		quat_q14[1] = quat[1] >> 16;
		quat_q14[2] = quat[2] >> 16;
		quat_q14[3] = quat[3] >> 16;
		quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
									quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
		if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
				(quat_mag_sq > QUAT_MAG_SQ_MAX))
		{
			/* Quaternion is outside of the acceptable threshold. */
			mpu_reset_fifo();
			sensors[0] = 0;
			return 1;
		}
		sensors[0] |= INV_WXYZ_QUAT;
#endif
	}

	if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL)
	{
		accel[0] = ((int16_t)fifo_data[ii+0] << 8) | fifo_data[ii+1];
		accel[1] = ((int16_t)fifo_data[ii+2] << 8) | fifo_data[ii+3];
		accel[2] = ((int16_t)fifo_data[ii+4] << 8) | fifo_data[ii+5];
		ii += 6;
		sensors[0] |= INV_XYZ_ACCEL;
	}

	if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO)
	{
		gyro[0] = ((int16_t)fifo_data[ii+0] << 8) | fifo_data[ii+1];
		gyro[1] = ((int16_t)fifo_data[ii+2] << 8) | fifo_data[ii+3];
		gyro[2] = ((int16_t)fifo_data[ii+4] << 8) | fifo_data[ii+5];
		ii += 6;
		sensors[0] |= INV_XYZ_GYRO;
	}

	/* Gesture data is at the end of the DMP packet. Parse it and call
	 * the gesture callbacks (if registered).
	 */
	if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
		decode_gesture(fifo_data + ii);

	return 0;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r12
pushq %rbx
subq $0x20, %rsp
movq %rcx, %rbx
movq %rdx, %r12
movq %rsi, %r15
movq %rdi, %r14
movw $0x0, (%rcx)
movzbl 0x5780(%rip), %edi      # 0xa266
movq %rsp, %rsi
movq %r8, %rdx
callq 0x2eca
movb $0x1, %bpl
testb %al, %al
jne 0x4cd4
testb $0x14, 0x575d(%rip)     # 0xa260
je 0x4bd4
movl (%rsp), %ecx
shll $0x18, %ecx
movzbl 0x1(%rsp), %eax
shll $0x10, %eax
orl %ecx, %eax
movzwl 0x2(%rsp), %ecx
rolw $0x8, %cx
movzwl %cx, %ecx
orl %eax, %ecx
movl %ecx, (%r12)
movl 0x4(%rsp), %edx
shll $0x18, %edx
movzbl 0x5(%rsp), %ecx
shll $0x10, %ecx
orl %edx, %ecx
movzwl 0x6(%rsp), %edx
rolw $0x8, %dx
movzwl %dx, %edx
orl %ecx, %edx
movl %edx, 0x4(%r12)
movl 0x8(%rsp), %esi
shll $0x18, %esi
movzbl 0x9(%rsp), %edx
shll $0x10, %edx
orl %esi, %edx
movzwl 0xa(%rsp), %esi
rolw $0x8, %si
movzwl %si, %esi
orl %edx, %esi
movl %esi, 0x8(%r12)
movl 0xc(%rsp), %edi
shll $0x18, %edi
movzbl 0xd(%rsp), %esi
shll $0x10, %esi
movzwl 0xe(%rsp), %r8d
rolw $0x8, %r8w
orl %edi, %esi
movzwl %r8w, %edi
orl %esi, %edi
movl %edi, 0xc(%r12)
sarl $0x10, %eax
sarl $0x10, %ecx
sarl $0x10, %edx
sarl $0x10, %esi
imull %eax, %eax
imull %ecx, %ecx
addl %eax, %ecx
imull %edx, %edx
addl %ecx, %edx
imull %esi, %esi
leal (%rsi,%rdx), %r12d
addl $0xf1000000, %r12d      # imm = 0xF1000000
cmpl $0x2000001, %r12d       # imm = 0x2000001
jae 0x4bd8
movzwl (%rbx), %eax
orl $0x100, %eax            # imm = 0x100
jmp 0x4bdf
xorl %eax, %eax
jmp 0x4bf1
callq 0x289f
xorl %eax, %eax
movw %ax, (%rbx)
movb $0x10, %al
cmpl $0x2000000, %r12d       # imm = 0x2000000
ja 0x4cd4
movzwl 0x5668(%rip), %ecx      # 0xa260
testb $0x40, %cl
je 0x4c3d
movzbl %al, %eax
movl (%rsp,%rax), %edx
shll $0x8, %edx
movzbl 0x1(%rsp,%rax), %esi
orl %edx, %esi
movw %si, (%r15)
movzbl 0x2(%rsp,%rax), %edx
shll $0x8, %edx
movzbl 0x3(%rsp,%rax), %esi
orl %edx, %esi
movw %si, 0x2(%r15)
movl 0x4(%rsp,%rax), %edx
shll $0x8, %edx
movzbl 0x5(%rsp,%rax), %esi
orl %edx, %esi
movw %si, 0x4(%r15)
orb $0x6, %al
orb $0x8, (%rbx)
testl $0x180, %ecx            # imm = 0x180
je 0x4c75
movzbl %al, %eax
movzwl (%rsp,%rax), %edx
rolw $0x8, %dx
movw %dx, (%r14)
movzwl 0x2(%rsp,%rax), %edx
rolw $0x8, %dx
movw %dx, 0x2(%r14)
movzwl 0x4(%rsp,%rax), %edx
rolw $0x8, %dx
movw %dx, 0x4(%r14)
addb $0x6, %al
orb $0x70, (%rbx)
testb $0x3, %cl
je 0x4cd2
movzbl %al, %r14d
movb 0x3(%rsp,%r14), %bl
testb $0x1, 0x1(%rsp,%r14)
je 0x4cae
movq 0x55b6(%rip), %rax      # 0xa248
testq %rax, %rax
je 0x4cae
movl %ebx, %ecx
andb $0x7, %cl
incb %cl
movl %ebx, %edx
shrb $0x3, %dl
andb $0x7, %dl
movzbl %dl, %edi
movzbl %cl, %esi
callq *%rax
movb 0x1(%rsp,%r14), %cl
shrb $0x3, %cl
movq 0x5593(%rip), %rax      # 0xa250
testq %rax, %rax
setne %dl
andb %cl, %dl
cmpb $0x1, %dl
jne 0x4cd2
shrb $0x6, %bl
movzbl %bl, %edi
callq *%rax
xorl %ebp, %ebp
movl %ebp, %eax
addq $0x20, %rsp
popq %rbx
popq %r12
popq %r14
popq %r15
popq %rbp
retq