// File: /workspace/github_c_makefile_star2_O3/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 0x570c(%rip), %edi      # 0xa26e
movq %rsp, %rsi
movq %r8, %rdx
callq 0x2eb0
movb $0x1, %bpl
testb %al, %al
jne 0x4d3e
movzwl 0x56e9(%rip), %eax      # 0xa268
testb $0x14, %al
je 0x4c55
movl (%rsp), %edx
shll $0x18, %edx
movzbl 0x1(%rsp), %ecx
shll $0x10, %ecx
orl %edx, %ecx
movzwl 0x2(%rsp), %edx
rolw $0x8, %dx
movzwl %dx, %edx
orl %ecx, %edx
movl %edx, (%r12)
movl 0x4(%rsp), %esi
shll $0x18, %esi
movzbl 0x5(%rsp), %edx
shll $0x10, %edx
orl %esi, %edx
movzwl 0x6(%rsp), %esi
rolw $0x8, %si
movzwl %si, %esi
orl %edx, %esi
movl %esi, 0x4(%r12)
movl 0x8(%rsp), %edi
shll $0x18, %edi
movzbl 0x9(%rsp), %esi
shll $0x10, %esi
orl %edi, %esi
movzwl 0xa(%rsp), %edi
rolw $0x8, %di
movzwl %di, %edi
orl %esi, %edi
movl %edi, 0x8(%r12)
movl 0xc(%rsp), %r8d
shll $0x18, %r8d
movzbl 0xd(%rsp), %edi
shll $0x10, %edi
movzwl 0xe(%rsp), %r9d
rolw $0x8, %r9w
orl %r8d, %edi
movzwl %r9w, %r8d
orl %edi, %r8d
movl %r8d, 0xc(%r12)
sarl $0x10, %ecx
sarl $0x10, %edx
sarl $0x10, %esi
sarl $0x10, %edi
imull %ecx, %ecx
imull %edx, %edx
addl %ecx, %edx
imull %esi, %esi
addl %edx, %esi
imull %edi, %edi
leal (%rdi,%rsi), %ecx
addl $0xf1000000, %ecx       # imm = 0xF1000000
cmpl $0x2000000, %ecx        # imm = 0x2000000
ja 0x4d34
orb $0x1, 0x1(%rbx)
movb $0x10, %cl
jmp 0x4c57
xorl %ecx, %ecx
testb $0x40, %al
je 0x4c9c
movzbl %cl, %ecx
movl (%rsp,%rcx), %edx
shll $0x8, %edx
movzbl 0x1(%rsp,%rcx), %esi
orl %edx, %esi
movw %si, (%r15)
movzbl 0x2(%rsp,%rcx), %edx
shll $0x8, %edx
movzbl 0x3(%rsp,%rcx), %esi
orl %edx, %esi
movw %si, 0x2(%r15)
movl 0x4(%rsp,%rcx), %edx
shll $0x8, %edx
movzbl 0x5(%rsp,%rcx), %esi
orl %edx, %esi
movw %si, 0x4(%r15)
orb $0x6, %cl
orb $0x8, (%rbx)
testl $0x180, %eax            # imm = 0x180
je 0x4cd4
movzbl %cl, %ecx
movzwl (%rsp,%rcx), %edx
rolw $0x8, %dx
movw %dx, (%r14)
movzwl 0x2(%rsp,%rcx), %edx
rolw $0x8, %dx
movw %dx, 0x2(%r14)
movzwl 0x4(%rsp,%rcx), %edx
rolw $0x8, %dx
movw %dx, 0x4(%r14)
addb $0x6, %cl
orb $0x70, (%rbx)
testb $0x3, %al
je 0x4d30
movzbl %cl, %r14d
movb 0x1(%rsp,%r14), %al
movb 0x3(%rsp,%r14), %bl
testb $0x1, %al
je 0x4d11
movq 0x555f(%rip), %rcx      # 0xa250
testq %rcx, %rcx
je 0x4d11
movl %ebx, %eax
andb $0x7, %al
incb %al
movl %ebx, %edx
shrb $0x3, %dl
andb $0x7, %dl
movzbl %dl, %edi
movzbl %al, %esi
callq *%rcx
movb 0x1(%rsp,%r14), %al
shrb $0x3, %al
movq 0x553d(%rip), %rcx      # 0xa258
testq %rcx, %rcx
setne %dl
andb %al, %dl
cmpb $0x1, %dl
jne 0x4d30
shrb $0x6, %bl
movzbl %bl, %edi
callq *%rcx
xorl %ebp, %ebp
jmp 0x4d3e
callq 0x2845
movw $0x0, (%rbx)
movl %ebp, %eax
addq $0x20, %rsp
popq %rbx
popq %r12
popq %r14
popq %r15
popq %rbp
retq