// File: /workspace/github_c_makefile_star2_O0/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
movq %rsp, %rbp
subq $0x80, %rsp
movq %rdi, -0x10(%rbp)
movq %rsi, -0x18(%rbp)
movq %rdx, -0x20(%rbp)
movq %rcx, -0x28(%rbp)
movq %r8, -0x30(%rbp)
movb $0x0, -0x51(%rbp)
movq -0x28(%rbp), %rax
movw $0x0, (%rax)
movzbl 0x5aeb(%rip), %eax      # 0xc25e
leaq -0x50(%rbp), %rsi
movq -0x30(%rbp), %rdx
movzwl %ax, %edi
callq 0x3e40
cmpb $0x0, %al
je 0x6790
movb $0x1, -0x1(%rbp)
jmp 0x6a68
movzwl 0x5ac3(%rip), %eax      # 0xc25a
andl $0x14, %eax
cmpl $0x0, %eax
je 0x68db
movzbl -0x50(%rbp), %ecx
shll $0x18, %ecx
movzbl -0x4f(%rbp), %eax
shll $0x10, %eax
orl %eax, %ecx
movzbl -0x4e(%rbp), %eax
shll $0x8, %eax
orl %eax, %ecx
movzbl -0x4d(%rbp), %eax
orl %eax, %ecx
movq -0x20(%rbp), %rax
movl %ecx, (%rax)
movzbl -0x4c(%rbp), %ecx
shll $0x18, %ecx
movzbl -0x4b(%rbp), %eax
shll $0x10, %eax
orl %eax, %ecx
movzbl -0x4a(%rbp), %eax
shll $0x8, %eax
orl %eax, %ecx
movzbl -0x49(%rbp), %eax
orl %eax, %ecx
movq -0x20(%rbp), %rax
movl %ecx, 0x4(%rax)
movzbl -0x48(%rbp), %ecx
shll $0x18, %ecx
movzbl -0x47(%rbp), %eax
shll $0x10, %eax
orl %eax, %ecx
movzbl -0x46(%rbp), %eax
shll $0x8, %eax
orl %eax, %ecx
movzbl -0x45(%rbp), %eax
orl %eax, %ecx
movq -0x20(%rbp), %rax
movl %ecx, 0x8(%rax)
movzbl -0x44(%rbp), %ecx
shll $0x18, %ecx
movzbl -0x43(%rbp), %eax
shll $0x10, %eax
orl %eax, %ecx
movzbl -0x42(%rbp), %eax
shll $0x8, %eax
orl %eax, %ecx
movzbl -0x41(%rbp), %eax
orl %eax, %ecx
movq -0x20(%rbp), %rax
movl %ecx, 0xc(%rax)
movzbl -0x51(%rbp), %eax
addl $0x10, %eax
movb %al, -0x51(%rbp)
movq -0x20(%rbp), %rax
movl (%rax), %eax
sarl $0x10, %eax
movl %eax, -0x70(%rbp)
movq -0x20(%rbp), %rax
movl 0x4(%rax), %eax
sarl $0x10, %eax
movl %eax, -0x6c(%rbp)
movq -0x20(%rbp), %rax
movl 0x8(%rax), %eax
sarl $0x10, %eax
movl %eax, -0x68(%rbp)
movq -0x20(%rbp), %rax
movl 0xc(%rax), %eax
sarl $0x10, %eax
movl %eax, -0x64(%rbp)
movl -0x70(%rbp), %eax
imull -0x70(%rbp), %eax
movl -0x6c(%rbp), %ecx
imull -0x6c(%rbp), %ecx
addl %ecx, %eax
movl -0x68(%rbp), %ecx
imull -0x68(%rbp), %ecx
addl %ecx, %eax
movl -0x64(%rbp), %ecx
imull -0x64(%rbp), %ecx
addl %ecx, %eax
movl %eax, -0x74(%rbp)
movslq -0x74(%rbp), %rax
cmpq $0xf000000, %rax        # imm = 0xF000000
jl 0x68b4
movslq -0x74(%rbp), %rax
cmpq $0x11000000, %rax       # imm = 0x11000000
jle 0x68cb
callq 0x3270
movq -0x28(%rbp), %rax
movw $0x0, (%rax)
movb $0x1, -0x1(%rbp)
jmp 0x6a68
movq -0x28(%rbp), %rax
movswl (%rax), %ecx
orl $0x100, %ecx            # imm = 0x100
movw %cx, (%rax)
movzwl 0x5978(%rip), %eax      # 0xc25a
andl $0x40, %eax
cmpl $0x0, %eax
je 0x698e
movzbl -0x51(%rbp), %eax
addl $0x0, %eax
cltq
movzbl -0x50(%rbp,%rax), %eax
cwtl
shll $0x8, %eax
movzbl -0x51(%rbp), %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
movzbl -0x50(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x18(%rbp), %rax
movw %cx, (%rax)
movzbl -0x51(%rbp), %eax
addl $0x2, %eax
cltq
movzbl -0x50(%rbp,%rax), %eax
cwtl
shll $0x8, %eax
movzbl -0x51(%rbp), %ecx
addl $0x3, %ecx
movslq %ecx, %rcx
movzbl -0x50(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x18(%rbp), %rax
movw %cx, 0x2(%rax)
movzbl -0x51(%rbp), %eax
addl $0x4, %eax
cltq
movzbl -0x50(%rbp,%rax), %eax
cwtl
shll $0x8, %eax
movzbl -0x51(%rbp), %ecx
addl $0x5, %ecx
movslq %ecx, %rcx
movzbl -0x50(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x18(%rbp), %rax
movw %cx, 0x4(%rax)
movzbl -0x51(%rbp), %eax
addl $0x6, %eax
movb %al, -0x51(%rbp)
movq -0x28(%rbp), %rax
movswl (%rax), %ecx
orl $0x8, %ecx
movw %cx, (%rax)
movzwl 0x58c5(%rip), %eax      # 0xc25a
andl $0x180, %eax            # imm = 0x180
cmpl $0x0, %eax
je 0x6a43
movzbl -0x51(%rbp), %eax
addl $0x0, %eax
cltq
movzbl -0x50(%rbp,%rax), %eax
cwtl
shll $0x8, %eax
movzbl -0x51(%rbp), %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
movzbl -0x50(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x10(%rbp), %rax
movw %cx, (%rax)
movzbl -0x51(%rbp), %eax
addl $0x2, %eax
cltq
movzbl -0x50(%rbp,%rax), %eax
cwtl
shll $0x8, %eax
movzbl -0x51(%rbp), %ecx
addl $0x3, %ecx
movslq %ecx, %rcx
movzbl -0x50(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x10(%rbp), %rax
movw %cx, 0x2(%rax)
movzbl -0x51(%rbp), %eax
addl $0x4, %eax
cltq
movzbl -0x50(%rbp,%rax), %eax
cwtl
shll $0x8, %eax
movzbl -0x51(%rbp), %ecx
addl $0x5, %ecx
movslq %ecx, %rcx
movzbl -0x50(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x10(%rbp), %rax
movw %cx, 0x4(%rax)
movzbl -0x51(%rbp), %eax
addl $0x6, %eax
movb %al, -0x51(%rbp)
movq -0x28(%rbp), %rax
movswl (%rax), %ecx
orl $0x70, %ecx
movw %cx, (%rax)
movzwl 0x5810(%rip), %eax      # 0xc25a
andl $0x3, %eax
cmpl $0x0, %eax
je 0x6a64
leaq -0x50(%rbp), %rdi
movzbl -0x51(%rbp), %eax
cltq
addq %rax, %rdi
callq 0x6a80
movb $0x0, -0x1(%rbp)
movb -0x1(%rbp), %al
addq $0x80, %rsp
popq %rbp
retq
nopw %cs:(%rax,%rax)