// File: /workspace/github_c_makefile_star2_O0/ZombiesYard[P]MPU6050-DMP-raspberry-pi/MotionSensor/inv_mpu_lib/inv_mpu.c
// Function: mpu_lp_motion_interrupt(unsigned short, unsigned char, unsigned char)
uint8_t mpu_lp_motion_interrupt(uint16_t thresh, uint8_t time,
																uint8_t lpa_freq)
{
	uint8_t data[3];

	if (lpa_freq)
	{
		uint8_t thresh_hw;

#if defined MPU6050
		/* TODO: Make these const/#defines. */
		/* 1LSb = 32mg. */
		if (thresh > 8160)
			thresh_hw = 255;
		else if (thresh < 32)
			thresh_hw = 1;
		else
			thresh_hw = thresh >> 5;
#elif defined MPU6500
		/* 1LSb = 4mg. */
		if (thresh > 1020)
			thresh_hw = 255;
		else if (thresh < 4)
			thresh_hw = 1;
		else
			thresh_hw = thresh >> 2;
#endif

		if (!time)
			/* Minimum duration must be 1ms. */
			time = 1;

#if defined MPU6050
		if (lpa_freq > 40)
#elif defined MPU6500
		if (lpa_freq > 640)
#endif
			/* At this point, the chip has not been re-configured, so the
			 * function can safely exit.
			 */
			return 1;

		if (!st.chip_cfg.int_motion_only)
		{
			/* Store current settings for later. */
			if (st.chip_cfg.dmp_on)
			{
				mpu_set_dmp_state(0);
				st.chip_cfg.cache.dmp_on = 1;
			}
			else
				st.chip_cfg.cache.dmp_on = 0;
			mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);
			mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);
			mpu_get_lpf(&st.chip_cfg.cache.lpf);
			mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate);
			st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors;
			mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors);
		}

#ifdef MPU6050
		/* Disable hardware interrupts for now. */
		set_int_enable(0);

		/* Enter full-power accel-only mode. */
		mpu_lp_accel_mode(0);

		/* Override current LPF (and HPF) settings to obtain a valid accel
		 * reading.
		 */
		data[0] = INV_FILTER_256HZ_NOLPF2;
		if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
			return 1;

		/* NOTE: Digital high pass filter should be configured here. Since this
		 * driver doesn't modify those bits anywhere, they should already be
		 * cleared by default.
		 */

		/* Configure the device to send motion interrupts. */
		/* Enable motion interrupt. */
		data[0] = BIT_MOT_INT_EN;
		if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
			goto lp_int_restore;

		/* Set motion interrupt parameters. */
		data[0] = thresh_hw;
		data[1] = time;
		if (i2c_write(st.hw->addr, st.reg->motion_thr, 2, data))
			goto lp_int_restore;

		/* Force hardware to "lock" current accel sample. */
		delay_ms(5);
		data[0] = (st.chip_cfg.accel_fsr << 3) | BITS_HPF;
		if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
			goto lp_int_restore;

		/* Set up LP accel mode. */
		data[0] = BIT_LPA_CYCLE;
		if (lpa_freq == 1)
			data[1] = INV_LPA_1_25HZ;
		else if (lpa_freq <= 5)
			data[1] = INV_LPA_5HZ;
		else if (lpa_freq <= 20)
			data[1] = INV_LPA_20HZ;
		else
			data[1] = INV_LPA_40HZ;
		data[1] = (data[1] << 6) | BIT_STBY_XYZG;
		if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
			goto lp_int_restore;

		st.chip_cfg.int_motion_only = 1;
		return 0;
#elif defined MPU6500
		/* Disable hardware interrupts. */
		set_int_enable(0);

		/* Enter full-power accel-only mode, no FIFO/DMP. */
		data[0] = 0;
		data[1] = 0;
		data[2] = BIT_STBY_XYZG;
		if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data))
			goto lp_int_restore;

		/* Set motion threshold. */
		data[0] = thresh_hw;
		if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data))
			goto lp_int_restore;

		/* Set wake frequency. */
		if (lpa_freq == 1)
			data[0] = INV_LPA_1_25HZ;
		else if (lpa_freq == 2)
			data[0] = INV_LPA_2_5HZ;
		else if (lpa_freq <= 5)
			data[0] = INV_LPA_5HZ;
		else if (lpa_freq <= 10)
			data[0] = INV_LPA_10HZ;
		else if (lpa_freq <= 20)
			data[0] = INV_LPA_20HZ;
		else if (lpa_freq <= 40)
			data[0] = INV_LPA_40HZ;
		else if (lpa_freq <= 80)
			data[0] = INV_LPA_80HZ;
		else if (lpa_freq <= 160)
			data[0] = INV_LPA_160HZ;
		else if (lpa_freq <= 320)
			data[0] = INV_LPA_320HZ;
		else
			data[0] = INV_LPA_640HZ;
		if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data))
			goto lp_int_restore;

		/* Enable motion interrupt (MPU6500 version). */
		data[0] = BITS_WOM_EN;
		if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
			goto lp_int_restore;

		/* Enable cycle mode. */
		data[0] = BIT_LPA_CYCLE;
		if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
			goto lp_int_restore;

		/* Enable interrupt. */
		data[0] = BIT_MOT_INT_EN;
		if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
			goto lp_int_restore;

		st.chip_cfg.int_motion_only = 1;
		return 0;
#endif
	}
	else
	{
		/* Don't "restore" the previous state if no state has been saved. */
		int ii;
		char *cache_ptr = (char*)&st.chip_cfg.cache;
		for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++)
		{
			if (cache_ptr[ii] != 0)
				goto lp_int_restore;
		}
		/* If we reach this point, motion interrupt mode hasn't been used yet. */
		return 1;
	}
lp_int_restore:
	/* Set to invalid values to ensure no I2C writes are skipped. */
	st.chip_cfg.gyro_fsr = 0xFF;
	st.chip_cfg.accel_fsr = 0xFF;
	st.chip_cfg.lpf = 0xFF;
	st.chip_cfg.sample_rate = 0xFFFF;
	st.chip_cfg.sensors = 0xFF;
	st.chip_cfg.fifo_enable = 0xFF;
	st.chip_cfg.clk_src = INV_CLK_PLL;
	mpu_set_sensors(st.chip_cfg.cache.sensors_on);
	mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);
	mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);
	mpu_set_lpf(st.chip_cfg.cache.lpf);
	mpu_set_sample_rate(st.chip_cfg.cache.sample_rate);
	mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors);

	if (st.chip_cfg.cache.dmp_on)
		mpu_set_dmp_state(1);

#ifdef MPU6500
	/* Disable motion interrupt (MPU6500 version). */
	data[0] = 0;
	if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
		goto lp_int_restore;
#endif

	st.chip_cfg.int_motion_only = 0;
	return 0;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x20, %rsp
movb %dl, %al
movb %sil, %cl
movw %di, %dx
movw %dx, -0x4(%rbp)
movb %cl, -0x5(%rbp)
movb %al, -0x6(%rbp)
cmpb $0x0, -0x6(%rbp)
je 0x52db
movzwl -0x4(%rbp), %eax
cmpl $0x1fe0, %eax           # imm = 0x1FE0
jle 0x5065
movb $-0x1, -0xa(%rbp)
jmp 0x5080
movzwl -0x4(%rbp), %eax
cmpl $0x20, %eax
jge 0x5074
movb $0x1, -0xa(%rbp)
jmp 0x507e
movzwl -0x4(%rbp), %eax
sarl $0x5, %eax
movb %al, -0xa(%rbp)
jmp 0x5080
cmpb $0x0, -0x5(%rbp)
jne 0x508a
movb $0x1, -0x5(%rbp)
movzbl -0x6(%rbp), %eax
cmpl $0x28, %eax
jle 0x509c
movb $0x1, -0x1(%rbp)
jmp 0x53c1
cmpb $0x0, 0x7192(%rip)      # 0xc235
jne 0x5149
cmpb $0x0, 0x7194(%rip)      # 0xc244
je 0x50c2
xorl %edi, %edi
callq 0x41b0
movb $0x1, 0x7180(%rip)      # 0xc240
jmp 0x50c9
movb $0x0, 0x7177(%rip)      # 0xc240
leaq 0x7140(%rip), %rdi      # 0xc210
addq $0x18, %rdi
addq $0xe, %rdi
callq 0x3580
leaq 0x712c(%rip), %rdi      # 0xc210
addq $0x18, %rdi
addq $0xe, %rdi
addq $0x2, %rdi
callq 0x35f0
leaq 0x7114(%rip), %rdi      # 0xc210
addq $0x18, %rdi
addq $0xe, %rdi
addq $0x4, %rdi
callq 0x3670
leaq 0x70fc(%rip), %rdi      # 0xc210
addq $0x18, %rdi
addq $0xe, %rdi
addq $0x6, %rdi
callq 0x36f0
movb 0x70ff(%rip), %al       # 0xc22a
movb %al, 0x710d(%rip)       # 0xc23e
leaq 0x70d8(%rip), %rdi      # 0xc210
addq $0x18, %rdi
addq $0xe, %rdi
addq $0x9, %rdi
callq 0x3860
xorl %edi, %edi
callq 0x3880
xorl %edi, %edi
callq 0x2bf0
movb $0x0, -0x9(%rbp)
movq 0x70b6(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x70a4(%rip), %rax      # 0xc210
movb 0x2(%rax), %al
leaq -0x9(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x5192
movb $0x1, -0x1(%rbp)
jmp 0x53c1
movb $0x40, -0x9(%rbp)
movq 0x707b(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x7069(%rip), %rax      # 0xc210
movb 0x11(%rax), %al
leaq -0x9(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x51c9
jmp 0x5328
movb -0xa(%rbp), %al
movb %al, -0x9(%rbp)
movb -0x5(%rbp), %al
movb %al, -0x8(%rbp)
movq 0x703c(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x702a(%rip), %rax      # 0xc210
movb 0xa(%rax), %al
leaq -0x9(%rbp), %rcx
movl $0x2, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x5208
jmp 0x5328
movl $0x1388, %edi           # imm = 0x1388
callq 0x10f0
movzbl 0x7010(%rip), %eax      # 0xc229
shll $0x3, %eax
orl $0x7, %eax
movb %al, -0x9(%rbp)
movq 0x6fef(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x6fdd(%rip), %rax      # 0xc210
movb 0x7(%rax), %al
leaq -0x9(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x5255
jmp 0x5328
movb $0x20, -0x9(%rbp)
movzbl -0x6(%rbp), %eax
cmpl $0x1, %eax
jne 0x5268
movb $0x0, -0x8(%rbp)
jmp 0x528e
movzbl -0x6(%rbp), %eax
cmpl $0x5, %eax
jg 0x5277
movb $0x1, -0x8(%rbp)
jmp 0x528c
movzbl -0x6(%rbp), %eax
cmpl $0x14, %eax
jg 0x5286
movb $0x2, -0x8(%rbp)
jmp 0x528a
movb $0x3, -0x8(%rbp)
jmp 0x528c
jmp 0x528e
movzbl -0x8(%rbp), %eax
shll $0x6, %eax
orl $0x7, %eax
movb %al, -0x8(%rbp)
movq 0x6f76(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0x6f64(%rip), %rax      # 0xc210
movb 0x15(%rax), %al
leaq -0x9(%rbp), %rcx
movl $0x2, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x52cb
jmp 0x5328
movb $0x1, 0x6f63(%rip)      # 0xc235
movb $0x0, -0x1(%rbp)
jmp 0x53c1
leaq 0x6f2e(%rip), %rax      # 0xc210
addq $0x18, %rax
addq $0xe, %rax
movq %rax, -0x18(%rbp)
movl $0x0, -0x10(%rbp)
movslq -0x10(%rbp), %rax
cmpq $0xc, %rax
jae 0x531f
movq -0x18(%rbp), %rax
movslq -0x10(%rbp), %rcx
movsbl (%rax,%rcx), %eax
cmpl $0x0, %eax
je 0x5312
jmp 0x5328
jmp 0x5314
movl -0x10(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x10(%rbp)
jmp 0x52f5
movb $0x1, -0x1(%rbp)
jmp 0x53c1
movb $-0x1, 0x6ef9(%rip)     # 0xc228
movb $-0x1, 0x6ef3(%rip)     # 0xc229
movb $-0x1, 0x6eee(%rip)     # 0xc22b
movw $0xffff, 0x6ee8(%rip)   # imm = 0xFFFF
movb $-0x1, 0x6edd(%rip)     # 0xc22a
movb $-0x1, 0x6edc(%rip)     # 0xc230
movb $0x1, 0x6ed1(%rip)      # 0xc22c
movzbl 0x6edc(%rip), %edi      # 0xc23e
callq 0x2aa0
movzwl 0x6ec8(%rip), %edi      # 0xc236
callq 0x23e0
movzbl 0x6ebe(%rip), %edi      # 0xc238
callq 0x24c0
movzwl 0x6eb4(%rip), %edi      # 0xc23a
callq 0x2590
movzwl 0x6eaa(%rip), %edi      # 0xc23c
callq 0x2670
movzbl 0x6ea1(%rip), %edi      # 0xc23f
callq 0x2780
cmpb $0x0, 0x6e96(%rip)      # 0xc240
je 0x53b6
movl $0x1, %edi
callq 0x41b0
movb $0x0, 0x6e78(%rip)      # 0xc235
movb $0x0, -0x1(%rbp)
movb -0x1(%rbp), %al
addq $0x20, %rsp
popq %rbp
retq
nopw (%rax,%rax)