// 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_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 %r15
pushq %r14
pushq %rbx
subq $0x30, %rsp
movl %edi, %ebx
leaq 0x14(%rsp), %r14
movl $0x9e3ca02, (%r14)      # imm = 0x9E3CA02
movl $0x68, %edi
movl $0x4, %esi
movq %r14, %rdx
callq 0x3995
movb $-0x5d, %al
movd %ebx, %xmm0
pshufd $0x50, %xmm0, %xmm1     # xmm1 = xmm0[0,0,1,1]
pand 0x1e49(%rip), %xmm1     # 0x6520
movb %al, (%r14)
pxor %xmm0, %xmm0
movdqa %xmm1, 0x20(%rsp)
pcmpeqd %xmm0, %xmm1
pshufd $0x55, %xmm1, %xmm2     # xmm2 = xmm1[1,1,1,1]
movq %xmm2, %r15
pshuflw $0x80, %xmm1, %xmm1     # xmm1 = xmm1[0,0,0,2,4,5,6,7]
psrlw $0x8, %xmm1
packuswb %xmm1, %xmm1
psllw $0x7, %xmm1
pand 0x1e23(%rip), %xmm1     # 0x6530
pcmpgtb %xmm1, %xmm0
movdqa %xmm0, %xmm1
pandn 0x1e23(%rip), %xmm1     # 0x6540
pand 0x1e2b(%rip), %xmm0     # 0x6550
por %xmm1, %xmm0
testb $0x1, %r15b
movl $0xa3, %ecx
movl $0xcc, %edx
cmovnel %ecx, %edx
movl $0xc6, %esi
cmovnel %ecx, %esi
movd %xmm0, 0x1(%r14)
movb %dl, 0x5(%r14)
movb %sil, 0x6(%r14)
movw $0xa3a3, 0x7(%r14)      # imm = 0xA3A3
movb %al, 0x9(%r14)
movl $0xaa7, %edi            # imm = 0xAA7
movl $0xa, %esi
movq %r14, %rdx
callq 0x3995
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 0x3995
testb $0x20, %bl
jne 0x47af
movabsq $-0x3a3c774f55555548, %rax # imm = 0xC5C388B0AAAAAAB8
leaq 0x8(%rsp), %rdx
movq %rax, (%rdx)
movb $-0x39, 0x8(%rdx)
jmp 0x47c5
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 0x3995
testb $0x1, %r15b
jne 0x4835
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 0x1d33(%rip), %xmm0     # 0x6530
pxor %xmm1, %xmm1
pcmpgtb %xmm0, %xmm1
movdqa %xmm1, %xmm0
pandn 0x1d4f(%rip), %xmm0     # 0x6560
pand 0x1d57(%rip), %xmm1     # 0x6570
por %xmm0, %xmm1
leaq 0x14(%rsp), %rdx
movd %xmm1, (%rdx)
movl $0xaa2, %edi            # imm = 0xAA2
movl $0x4, %esi
callq 0x3995
leaq 0x14(%rsp), %rdx
testb $0x1, %bl
jne 0x4851
movb $-0x28, (%rdx)
movl $0x8b0, %edi            # imm = 0x8B0
movl $0x1, %esi
jmp 0x4932
movb $-0x8, (%rdx)
movl $0x8b0, %edi            # imm = 0x8B0
movl $0x1, %esi
callq 0x3995
movl $0x7, %edi
movl $0xfa, %esi
callq 0x4341
leaq 0x8(%rsp), %r14
movb $0x3f, (%r14)
movl $0x148, %edi            # imm = 0x148
movl $0x1, %esi
movq %r14, %rdx
callq 0x3995
movb $0x0, (%r14)
leaq 0x8(%rsp), %r14
movl $0x14f, %edi            # imm = 0x14F
movl $0x1, %esi
movq %r14, %rdx
callq 0x3995
movw $0x1400, (%r14)         # imm = 0x1400
leaq 0x8(%rsp), %r14
movl $0x1de, %edi            # imm = 0x1DE
movl $0x2, %esi
movq %r14, %rdx
callq 0x3995
movw $0x6400, (%r14)         # imm = 0x6400
leaq 0x8(%rsp), %r14
movl $0x1da, %edi            # imm = 0x1DA
movl $0x2, %esi
movq %r14, %rdx
callq 0x3995
movl $0x90f98e00, (%r14)     # imm = 0x90F98E00
leaq 0x8(%rsp), %r14
movl $0x15c, %edi            # imm = 0x15C
movl $0x4, %esi
movq %r14, %rdx
callq 0x3995
movw $0x800, (%r14)          # imm = 0x800
leaq 0x8(%rsp), %r14
movl $0x15a, %edi            # imm = 0x15A
movl $0x2, %esi
movq %r14, %rdx
callq 0x3995
movw $0x200, (%r14)          # imm = 0x200
leaq 0x8(%rsp), %rdx
movl $0x158, %edi            # imm = 0x158
movl $0x2, %esi
callq 0x3995
testb $0x2, %bl
sete %al
xorb $-0x27, %al
leaq 0x14(%rsp), %rdx
movb %al, (%rdx)
movl $0x73d, %edi            # imm = 0x73D
movl $0x1, %esi
callq 0x3995
leaq 0x8(%rsp), %rdx
testb $0x4, %bl
jne 0x4967
movl $0x8b8b8b8b, (%rdx)     # imm = 0x8B8B8B8B
jmp 0x496d
movl $0xc6c4c2c0, (%rdx)     # imm = 0xC6C4C2C0
movl $0xa98, %edi            # imm = 0xA98
movl $0x4, %esi
callq 0x3995
callq 0x2845
leaq 0x8(%rsp), %rdx
testb $0x10, %bl
jne 0x4993
movl $0xa3a3a3a3, (%rdx)     # imm = 0xA3A3A3A3
jmp 0x4999
movl $0x38302820, (%rdx)     # imm = 0x38302820
movl $0xa9e, %edi            # imm = 0xA9E
movl $0x4, %esi
callq 0x3995
callq 0x2845
pxor %xmm0, %xmm0
movdqa 0x20(%rsp), %xmm1
pcmpeqd %xmm0, %xmm1
movdqa %xmm1, 0x20(%rsp)
movl %ebx, %eax
orl $0x8, %eax
movw %ax, 0x589b(%rip)       # 0xa268
callq 0x2845
movdqa 0x20(%rsp), %xmm0
movd %xmm0, %eax
xorl %ecx, %ecx
testb $0x1, %al
movl $0x6, %eax
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, 0x585b(%rip)       # 0xa26e
xorl %eax, %eax
addq $0x30, %rsp
popq %rbx
popq %r14
popq %r15
retq