// 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_enable_feature(unsigned short)
uint8_t dmp_enable_feature(uint16_t mask)
{
	uint8_t tmp[10];

	/* TODO: All of these settings can probably be integrated into the default
	 * DMP image.
	 */
	/* Set integration scale factor. */
	tmp[0] = (uint8_t)((GYRO_SF >> 24) & 0xFF);
	tmp[1] = (uint8_t)((GYRO_SF >> 16) & 0xFF);
	tmp[2] = (uint8_t)((GYRO_SF >> 8) & 0xFF);
	tmp[3] = (uint8_t)(GYRO_SF & 0xFF);
	mpu_write_mem(D_0_104, 4, tmp);

	/* Send sensor data to the FIFO. */
	tmp[0] = 0xA3;
	if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
	{
		tmp[1] = 0xC0;
		tmp[2] = 0xC8;
		tmp[3] = 0xC2;
	}
	else
	{
		tmp[1] = 0xA3;
		tmp[2] = 0xA3;
		tmp[3] = 0xA3;
	}
	if (mask & DMP_FEATURE_SEND_ANY_GYRO)
	{
		tmp[4] = 0xC4;
		tmp[5] = 0xCC;
		tmp[6] = 0xC6;
	}
	else
	{
		tmp[4] = 0xA3;
		tmp[5] = 0xA3;
		tmp[6] = 0xA3;
	}
	tmp[7] = 0xA3;
	tmp[8] = 0xA3;
	tmp[9] = 0xA3;
	mpu_write_mem(CFG_15,10,tmp);

	/* Send gesture data to the FIFO. */
	if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
		tmp[0] = DINA20;
	else
		tmp[0] = 0xD8;
	mpu_write_mem(CFG_27,1,tmp);

	if (mask & DMP_FEATURE_GYRO_CAL)
		dmp_enable_gyro_cal(1);
	else
		dmp_enable_gyro_cal(0);

	if (mask & DMP_FEATURE_SEND_ANY_GYRO)
	{
		if (mask & DMP_FEATURE_SEND_CAL_GYRO)
		{
			tmp[0] = 0xB2;
			tmp[1] = 0x8B;
			tmp[2] = 0xB6;
			tmp[3] = 0x9B;
		}
		else
		{
			tmp[0] = DINAC0;
			tmp[1] = DINA80;
			tmp[2] = DINAC2;
			tmp[3] = DINA90;
		}
		mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp);
	}

	if (mask & DMP_FEATURE_TAP)
	{
		/* Enable tap. */
		tmp[0] = 0xF8;
		mpu_write_mem(CFG_20, 1, tmp);
		dmp_set_tap_thresh(TAP_XYZ, 250);
		dmp_set_tap_axes(TAP_XYZ);
		dmp_set_tap_count(1);
		dmp_set_tap_time(100);
		dmp_set_tap_time_multi(500);

		dmp_set_shake_reject_thresh(GYRO_SF, 200);
		dmp_set_shake_reject_time(40);
		dmp_set_shake_reject_timeout(10);
	}
	else
	{
		tmp[0] = 0xD8;
		mpu_write_mem(CFG_20, 1, tmp);
	}

	if (mask & DMP_FEATURE_ANDROID_ORIENT)
	{
		tmp[0] = 0xD9;
	}
	else
		tmp[0] = 0xD8;
	mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);

	if (mask & DMP_FEATURE_LP_QUAT)
		dmp_enable_lp_quat(1);
	else
		dmp_enable_lp_quat(0);

	if (mask & DMP_FEATURE_6X_LP_QUAT)
		dmp_enable_6x_lp_quat(1);
	else
		dmp_enable_6x_lp_quat(0);

	/* Pedometer is always enabled. */
	dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER;
	mpu_reset_fifo();

	dmp.packet_length = 0;
	if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
		dmp.packet_length += 6;
	if (mask & DMP_FEATURE_SEND_ANY_GYRO)
		dmp.packet_length += 6;
	if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
		dmp.packet_length += 16;
	if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
		dmp.packet_length += 4;

	return 0;
}

// Assembly:
pushq %rbp
pushq %r14
pushq %rbx
subq $0x20, %rsp
movl %edi, %ebx
leaq 0x16(%rsp), %r14
movl $0x9e3ca02, (%r14)      # imm = 0x9E3CA02
movl $0x68, %edi
movl $0x4, %esi
movq %r14, %rdx
callq 0x39a0
movb $-0x5d, %al
movb %al, (%r14)
testb $0x40, %bl
movl $0xa3, %ecx
movl $0xc0, %edx
cmovel %ecx, %edx
movl $0xc8, %esi
cmovel %ecx, %esi
movl $0xc2, %edi
cmovel %ecx, %edi
movb %dl, 0x1(%r14)
movb %sil, 0x2(%r14)
testl $0x180, %ebx            # imm = 0x180
movl $0xc4, %edx
cmovel %ecx, %edx
movb %dil, 0x3(%r14)
movl $0xcc, %esi
cmovel %ecx, %esi
movl $0xc6, %edi
cmovel %ecx, %edi
movb %dl, 0x4(%r14)
movb %sil, 0x5(%r14)
movb %dil, 0x6(%r14)
movw $0xa3a3, 0x7(%r14)      # imm = 0xA3A3
movb %al, 0x9(%r14)
movl $0xaa7, %edi            # imm = 0xAA7
movl $0xa, %esi
movq %r14, %rdx
callq 0x39a0
testb $0x3, %bl
movl $0xd8, %eax
movl $0x20, %ecx
cmovel %eax, %ecx
movb %cl, (%r14)
movl $0xab6, %edi            # imm = 0xAB6
movl $0x1, %esi
movq %r14, %rdx
callq 0x39a0
testb $0x20, %bl
jne 0x4758
movabsq $-0x3a3c774f55555548, %rax # imm = 0xC5C388B0AAAAAAB8
leaq 0x8(%rsp), %rdx
movq %rax, (%rdx)
movb $-0x39, 0x8(%rdx)
jmp 0x476e
movabsq $0x350d98b48db3aab8, %rax # imm = 0x350D98B48DB3AAB8
leaq 0x8(%rsp), %rdx
movq %rax, (%rdx)
movb $0x5d, 0x8(%rdx)
movl $0x4b8, %edi            # imm = 0x4B8
movl $0x9, %esi
callq 0x39a0
testl $0x180, %ebx            # imm = 0x180
je 0x47d9
movl %ebx, %eax
andl $0x100, %eax            # imm = 0x100
movl %eax, %ecx
shrl $0x8, %ecx
addb %cl, %cl
leal -0x50(%rcx), %esi
orb $-0x4c, %cl
testl %eax, %eax
movl $0x80, %eax
movl $0x8b, %edi
cmovel %eax, %edi
movl $0x90, %eax
movl $0x9b, %r8d
cmovel %eax, %r8d
leaq 0x16(%rsp), %rdx
movb %sil, (%rdx)
movb %dil, 0x1(%rdx)
movb %cl, 0x2(%rdx)
movb %r8b, 0x3(%rdx)
movl $0xaa2, %edi            # imm = 0xAA2
movl $0x4, %esi
callq 0x39a0
leaq 0x16(%rsp), %rdx
testb $0x1, %bl
jne 0x47f5
movb $-0x28, (%rdx)
movl $0x8b0, %edi            # imm = 0x8B0
movl $0x1, %esi
jmp 0x48d6
movb $-0x8, (%rdx)
movl $0x8b0, %edi            # imm = 0x8B0
movl $0x1, %esi
callq 0x39a0
movl $0x7, %edi
movl $0xfa, %esi
callq 0x431e
leaq 0x8(%rsp), %r14
movb $0x3f, (%r14)
movl $0x148, %edi            # imm = 0x148
movl $0x1, %esi
movq %r14, %rdx
callq 0x39a0
movb $0x0, (%r14)
leaq 0x8(%rsp), %r14
movl $0x14f, %edi            # imm = 0x14F
movl $0x1, %esi
movq %r14, %rdx
callq 0x39a0
movw $0x1400, (%r14)         # imm = 0x1400
leaq 0x8(%rsp), %r14
movl $0x1de, %edi            # imm = 0x1DE
movl $0x2, %esi
movq %r14, %rdx
callq 0x39a0
movw $0x6400, (%r14)         # imm = 0x6400
leaq 0x8(%rsp), %r14
movl $0x1da, %edi            # imm = 0x1DA
movl $0x2, %esi
movq %r14, %rdx
callq 0x39a0
movl $0x90f98e00, (%r14)     # imm = 0x90F98E00
leaq 0x8(%rsp), %r14
movl $0x15c, %edi            # imm = 0x15C
movl $0x4, %esi
movq %r14, %rdx
callq 0x39a0
movw $0x800, (%r14)          # imm = 0x800
leaq 0x8(%rsp), %r14
movl $0x15a, %edi            # imm = 0x15A
movl $0x2, %esi
movq %r14, %rdx
callq 0x39a0
movw $0x200, (%r14)          # imm = 0x200
leaq 0x8(%rsp), %rdx
movl $0x158, %edi            # imm = 0x158
movl $0x2, %esi
callq 0x39a0
testb $0x2, %bl
sete %al
xorb $-0x27, %al
leaq 0x16(%rsp), %rdx
movb %al, (%rdx)
movl $0x73d, %edi            # imm = 0x73D
movl $0x1, %esi
callq 0x39a0
leaq 0x8(%rsp), %rdx
testb $0x4, %bl
jne 0x490b
movl $0x8b8b8b8b, (%rdx)     # imm = 0x8B8B8B8B
jmp 0x4911
movl $0xc6c4c2c0, (%rdx)     # imm = 0xC6C4C2C0
movl $0xa98, %edi            # imm = 0xA98
movl $0x4, %esi
callq 0x39a0
callq 0x289f
leaq 0x8(%rsp), %rdx
testb $0x10, %bl
jne 0x4937
movl $0xa3a3a3a3, (%rdx)     # imm = 0xA3A3A3A3
jmp 0x493d
movl $0x38302820, (%rdx)     # imm = 0x38302820
movl $0xa9e, %edi            # imm = 0xA9E
movl $0x4, %esi
callq 0x39a0
callq 0x289f
movl %ebx, %ebp
andl $0x40, %ebp
movl %ebx, %eax
orl $0x8, %eax
movw %ax, 0x58fe(%rip)       # 0xa260
callq 0x289f
testl %ebp, %ebp
movl $0x6, %eax
cmovel %ebp, %eax
leal 0x6(%rax), %ecx
testl $0x180, %ebx            # imm = 0x180
movzbl %cl, %ecx
cmovel %eax, %ecx
movb %cl, 0x58e0(%rip)       # 0xa266
testb $0x14, %bl
je 0x4992
addb $0x10, 0x58d4(%rip)     # 0xa266
testb $0x3, %bl
je 0x499e
addb $0x4, 0x58c8(%rip)      # 0xa266
xorl %eax, %eax
addq $0x20, %rsp
popq %rbx
popq %r14
popq %rbp
retq