// 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_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
movq %rsp, %rbp
subq $0x10, %rsp
movw %di, %ax
movw %ax, -0x2(%rbp)
movb $0x2, -0xc(%rbp)
movb $-0x36, -0xb(%rbp)
movb $-0x1d, -0xa(%rbp)
movb $0x9, -0x9(%rbp)
leaq -0xc(%rbp), %rdx
movl $0x68, %edi
movl $0x4, %esi
callq 0x4c40
movb $-0x5d, -0xc(%rbp)
movzwl -0x2(%rbp), %eax
andl $0x40, %eax
cmpl $0x0, %eax
je 0x62d0
movb $-0x40, -0xb(%rbp)
movb $-0x38, -0xa(%rbp)
movb $-0x3e, -0x9(%rbp)
jmp 0x62dc
movb $-0x5d, -0xb(%rbp)
movb $-0x5d, -0xa(%rbp)
movb $-0x5d, -0x9(%rbp)
movzwl -0x2(%rbp), %eax
andl $0x180, %eax            # imm = 0x180
cmpl $0x0, %eax
je 0x62f8
movb $-0x3c, -0x8(%rbp)
movb $-0x34, -0x7(%rbp)
movb $-0x3a, -0x6(%rbp)
jmp 0x6304
movb $-0x5d, -0x8(%rbp)
movb $-0x5d, -0x7(%rbp)
movb $-0x5d, -0x6(%rbp)
movb $-0x5d, -0x5(%rbp)
movb $-0x5d, -0x4(%rbp)
movb $-0x5d, -0x3(%rbp)
leaq -0xc(%rbp), %rdx
movl $0xaa7, %edi            # imm = 0xAA7
movl $0xa, %esi
callq 0x4c40
movzwl -0x2(%rbp), %eax
andl $0x3, %eax
cmpl $0x0, %eax
je 0x6335
movb $0x20, -0xc(%rbp)
jmp 0x6339
movb $-0x28, -0xc(%rbp)
leaq -0xc(%rbp), %rdx
movl $0xab6, %edi            # imm = 0xAB6
movl $0x1, %esi
callq 0x4c40
movzwl -0x2(%rbp), %eax
andl $0x20, %eax
cmpl $0x0, %eax
je 0x6364
movl $0x1, %edi
callq 0x6550
jmp 0x636b
xorl %edi, %edi
callq 0x6550
movzwl -0x2(%rbp), %eax
andl $0x180, %eax            # imm = 0x180
cmpl $0x0, %eax
je 0x63bc
movzwl -0x2(%rbp), %eax
andl $0x100, %eax            # imm = 0x100
cmpl $0x0, %eax
je 0x6399
movb $-0x4e, -0xc(%rbp)
movb $-0x75, -0xb(%rbp)
movb $-0x4a, -0xa(%rbp)
movb $-0x65, -0x9(%rbp)
jmp 0x63a9
movb $-0x50, -0xc(%rbp)
movb $-0x80, -0xb(%rbp)
movb $-0x4c, -0xa(%rbp)
movb $-0x70, -0x9(%rbp)
leaq -0xc(%rbp), %rdx
movl $0xaa2, %edi            # imm = 0xAA2
movl $0x4, %esi
callq 0x4c40
movzwl -0x2(%rbp), %eax
andl $0x1, %eax
cmpl $0x0, %eax
je 0x643b
movb $-0x8, -0xc(%rbp)
leaq -0xc(%rbp), %rdx
movl $0x8b0, %edi            # imm = 0x8B0
movl $0x1, %esi
callq 0x4c40
movl $0x7, %edi
movl $0xfa, %esi
callq 0x5bd0
movl $0x7, %edi
callq 0x5e50
movl $0x1, %edi
callq 0x5ec0
movl $0x64, %edi
callq 0x5f10
movl $0x1f4, %edi            # imm = 0x1F4
callq 0x5f60
movl $0x2cae309, %edi        # imm = 0x2CAE309
movl $0xc8, %esi
callq 0x5fb0
movl $0x28, %edi
callq 0x6030
movl $0xa, %edi
callq 0x6080
jmp 0x6452
movb $-0x28, -0xc(%rbp)
leaq -0xc(%rbp), %rdx
movl $0x8b0, %edi            # imm = 0x8B0
movl $0x1, %esi
callq 0x4c40
movzwl -0x2(%rbp), %eax
andl $0x2, %eax
cmpl $0x0, %eax
je 0x6464
movb $-0x27, -0xc(%rbp)
jmp 0x6468
movb $-0x28, -0xc(%rbp)
leaq -0xc(%rbp), %rdx
movl $0x73d, %edi            # imm = 0x73D
movl $0x1, %esi
callq 0x4c40
movzwl -0x2(%rbp), %eax
andl $0x4, %eax
cmpl $0x0, %eax
je 0x6493
movl $0x1, %edi
callq 0x65d0
jmp 0x649a
xorl %edi, %edi
callq 0x65d0
movzwl -0x2(%rbp), %eax
andl $0x10, %eax
cmpl $0x0, %eax
je 0x64b2
movl $0x1, %edi
callq 0x6630
jmp 0x64b9
xorl %edi, %edi
callq 0x6630
movzwl -0x2(%rbp), %eax
orl $0x8, %eax
movw %ax, 0x5d93(%rip)       # 0xc25a
callq 0x3270
movb $0x0, 0x5d8b(%rip)      # 0xc25e
movzwl -0x2(%rbp), %eax
andl $0x40, %eax
cmpl $0x0, %eax
je 0x64ef
movzbl 0x5d78(%rip), %eax      # 0xc25e
addl $0x6, %eax
movb %al, 0x5d6f(%rip)       # 0xc25e
movzwl -0x2(%rbp), %eax
andl $0x180, %eax            # imm = 0x180
cmpl $0x0, %eax
je 0x650d
movzbl 0x5d5a(%rip), %eax      # 0xc25e
addl $0x6, %eax
movb %al, 0x5d51(%rip)       # 0xc25e
movzwl -0x2(%rbp), %eax
andl $0x14, %eax
cmpl $0x0, %eax
je 0x6529
movzbl 0x5d3e(%rip), %eax      # 0xc25e
addl $0x10, %eax
movb %al, 0x5d35(%rip)       # 0xc25e
movzwl -0x2(%rbp), %eax
andl $0x3, %eax
cmpl $0x0, %eax
je 0x6545
movzbl 0x5d22(%rip), %eax      # 0xc25e
addl $0x4, %eax
movb %al, 0x5d19(%rip)       # 0xc25e
xorl %eax, %eax
addq $0x10, %rsp
popq %rbp
retq
nopl (%rax)