// File: /workspace/github_c_makefile_star2_O2/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 %r15
pushq %r14
pushq %rbx
subq $0x28, %rsp
movl %edi, %ebx
leaq 0x4(%rsp), %r14
movl $0x9e3ca02, (%r14)      # imm = 0x9E3CA02
pushq $0x68
popq %rdi
pushq $0x4
popq %rsi
movq %r14, %rdx
callq 0x3596
movb $-0x5d, %al
movb %al, (%r14)
movd %ebx, %xmm0
pshufd $0x50, %xmm0, %xmm0     # xmm0 = xmm0[0,0,1,1]
pand 0x1315(%rip), %xmm0     # 0x5510
pxor %xmm1, %xmm1
movdqa %xmm0, 0x10(%rsp)
pcmpeqd %xmm1, %xmm0
pshufd $0x55, %xmm0, %xmm1     # xmm1 = xmm0[1,1,1,1]
movq %xmm1, %r15
pshuflw $0x80, %xmm0, %xmm0     # xmm0 = xmm0[0,0,0,2,4,5,6,7]
psrlw $0x8, %xmm0
packuswb %xmm0, %xmm0
psllw $0x7, %xmm0
pand 0x12f2(%rip), %xmm0     # 0x5520
pxor %xmm1, %xmm1
pcmpgtb %xmm0, %xmm1
movdqa %xmm1, %xmm0
pandn 0x12ee(%rip), %xmm0     # 0x5530
pand 0x12f6(%rip), %xmm1     # 0x5540
por %xmm0, %xmm1
testb $0x1, %r15b
movl $0xa3, %ecx
movl $0xcc, %edx
cmovnel %ecx, %edx
movl $0xc6, %esi
cmovnel %ecx, %esi
movd %xmm1, 0x1(%r14)
movb %dl, 0x5(%r14)
movb %sil, 0x6(%r14)
movw $0xa3a3, 0x7(%r14)      # imm = 0xA3A3
movb %al, 0x9(%r14)
pushq $0xa
popq %rsi
movl $0xaa7, %edi            # imm = 0xAA7
movq %r14, %rdx
callq 0x3596
testb $0x3, %bl
movl $0xd8, %eax
pushq $0x20
popq %rcx
cmovel %eax, %ecx
movb %cl, (%r14)
pushq $0x1
popq %rsi
movl $0xab6, %edi            # imm = 0xAB6
movq %r14, %rdx
callq 0x3596
movl %ebx, %eax
shrb $0x5, %al
andb $0x1, %al
movzbl %al, %edi
callq 0x4449
testb $0x1, %r15b
jne 0x431f
xorl %eax, %eax
btl $0x8, %ebx
setae %al
movd %eax, %xmm0
punpcklbw %xmm0, %xmm0    # xmm0 = xmm0[0,0,1,1,2,2,3,3,4,4,5,5,6,6,7,7]
pshuflw $0x0, %xmm0, %xmm0      # xmm0 = xmm0[0,0,0,0,4,5,6,7]
psllw $0x7, %xmm0
pand 0x1237(%rip), %xmm0     # 0x5520
pxor %xmm1, %xmm1
pcmpgtb %xmm0, %xmm1
movdqa %xmm1, %xmm0
pandn 0x1253(%rip), %xmm0     # 0x5550
pand 0x125b(%rip), %xmm1     # 0x5560
por %xmm0, %xmm1
leaq 0x4(%rsp), %rdx
movd %xmm1, (%rdx)
pushq $0x4
popq %rsi
movl $0xaa2, %edi            # imm = 0xAA2
callq 0x3596
leaq 0x4(%rsp), %rdx
testb $0x1, %bl
jne 0x433b
movb $-0x28, (%rdx)
pushq $0x1
popq %rsi
movl $0x8b0, %edi            # imm = 0x8B0
callq 0x3596
jmp 0x439e
movb $-0x8, (%rdx)
pushq $0x1
popq %rbp
movl $0x8b0, %edi            # imm = 0x8B0
movl %ebp, %esi
callq 0x3596
pushq $0x7
popq %r14
movl %r14d, %edi
movl $0xfa, %esi
callq 0x3e97
movl %r14d, %edi
callq 0x3fe1
movl %ebp, %edi
callq 0x401f
pushq $0x64
popq %rdi
callq 0x404d
movl $0x1f4, %edi            # imm = 0x1F4
callq 0x4074
movl $0x2cae309, %edi        # imm = 0x2CAE309
movl $0xc8, %esi
callq 0x409b
pushq $0x28
popq %rdi
callq 0x40c1
pushq $0xa
popq %rdi
callq 0x40e8
pxor %xmm0, %xmm0
movdqa 0x10(%rsp), %xmm1
pcmpeqd %xmm0, %xmm1
movdqa %xmm1, 0x10(%rsp)
testb $0x2, %bl
sete %al
xorb $-0x27, %al
leaq 0x4(%rsp), %rdx
movb %al, (%rdx)
pushq $0x1
popq %rsi
movl $0x73d, %edi            # imm = 0x73D
callq 0x3596
movl %ebx, %eax
shrb $0x2, %al
andb $0x1, %al
movzbl %al, %edi
callq 0x4491
movl %ebx, %eax
shrb $0x4, %al
andb $0x1, %al
movzbl %al, %edi
callq 0x44c4
movl %ebx, %eax
orl $0x8, %eax
movw %ax, 0x4e60(%rip)       # 0x9258
callq 0x2526
movdqa 0x10(%rsp), %xmm0
movd %xmm0, %eax
xorl %ecx, %ecx
testb $0x1, %al
pushq $0x6
popq %rax
cmovnel %ecx, %eax
leal 0x6(%rax), %ecx
movzbl %cl, %ecx
testb $0x1, %r15b
cmovnel %eax, %ecx
leal 0x10(%rcx), %eax
testb $0x14, %bl
movzbl %al, %eax
cmovel %ecx, %eax
leal 0x4(%rax), %ecx
testb $0x3, %bl
movzbl %cl, %ecx
cmovel %eax, %ecx
movb %cl, 0x4e22(%rip)       # 0x925e
xorl %eax, %eax
addq $0x28, %rsp
popq %rbx
popq %r14
popq %r15
popq %rbp
retq