// File: /workspace/github_c_makefile_star2_O0/ZombiesYard[P]MPU6050-DMP-raspberry-pi/MotionSensor/inv_mpu_lib/inv_mpu.c
// Function: mpu_init(int_param_s*)
uint8_t mpu_init(struct int_param_s *int_param)
{
	uint8_t data[6], rev;

    /* Init all structs*/
    init_structs();
    
	/* Reset device. */
	data[0] = BIT_RESET;
	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
		return 1;
	delay_ms(100);

	/* Wake up chip. */
	data[0] = 0x00;
	if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
		return 1;

#if defined MPU6050
	/* Check product revision. */
	if (i2c_read(st.hw->addr, st.reg->accel_offs, 6, data))
		return 1;
	rev = ((data[5] & 0x01) << 2) | ((data[3] & 0x01) << 1) |
				(data[1] & 0x01);
#if defined MPU_DEBUG
	printf_P("Software product rev. %d.\r\n", rev);
#endif
	if (rev)
	{
		/* Congrats, these parts are better. */
		if (rev == 1)
			st.chip_cfg.accel_half = 1;
		else if (rev == 2)
			st.chip_cfg.accel_half = 0;
		else
		{
#if defined MPU_DEBUG
			printf_P("Unsupported software product rev. %d.\r\n", rev);
#endif
			return 1;
		}
	}
	else
	{
		if (i2c_read(st.hw->addr, st.reg->prod_id, 1, data))
			return 1;
		rev = data[0] & 0x0F;
		if (!rev)
		{
#if defined MPU_DEBUG
			printf_P("Product ID read as 0 indicates device is either "
						"incompatible or an MPU3050.\r\n");
#endif
			return 1;
		}
		else if (rev == 4)
		{
#if defined MPU_DEBUG
			printf_P("Half sensitivity part found.\r\n");
#endif
			st.chip_cfg.accel_half = 1;
		}
		else
			st.chip_cfg.accel_half = 0;
	}
#elif defined MPU6500
#define MPU6500_MEM_REV_ADDR    (0x17)
	if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev))
		return 1;
	if (rev == 0x1)
		st.chip_cfg.accel_half = 0;
	else
	{
#if defined MPU_DEBUG
		printf_P("Unsupported software product rev. %d.\r\n", rev);
#endif
		return 1;
	}

	/* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
	 * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
	 */
	data[0] = BIT_FIFO_SIZE_1024 | 0x8;
	if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data))
		return 1;
#endif

	/* Set to invalid values to ensure no I2C writes are skipped. */
	st.chip_cfg.sensors = 0xFF;
	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.fifo_enable = 0xFF;
	st.chip_cfg.bypass_mode = 0xFF;
#ifdef AK89xx_SECONDARY
	st.chip_cfg.compass_sample_rate = 0xFFFF;
#endif
	/* mpu_set_sensors always preserves this setting. */
	st.chip_cfg.clk_src = INV_CLK_PLL;
	/* Handled in next call to mpu_set_bypass. */
	st.chip_cfg.active_low_int = 1;
	st.chip_cfg.latched_int = 0;
	st.chip_cfg.int_motion_only = 0;
	st.chip_cfg.lp_accel_mode = 0;
	memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
	st.chip_cfg.dmp_on = 0;
	st.chip_cfg.dmp_loaded = 0;
	st.chip_cfg.dmp_sample_rate = 0;

	if (mpu_set_gyro_fsr(2000))
		return 1;
	if (mpu_set_accel_fsr(2))
		return 1;
	if (mpu_set_lpf(42))
		return 1;
	if (mpu_set_sample_rate(50))
		return 1;
	if (mpu_configure_fifo(0))
		return 1;

	/* if (int_param)
	    reg_int_cb(int_param); */

#ifdef AK89xx_SECONDARY
	setup_compass();
	if (mpu_set_compass_sample_rate(10))
		return 1;
#else
	/* Already disabled by setup_compass. */
	if (mpu_set_bypass(0))
		return 1;
#endif

	mpu_set_sensors(0);
#if defined MPU_DEBUG
	printf_P("Initializing is done...\r\n");
#endif
	return 0;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x20, %rsp
movq %rdi, -0x10(%rbp)
callq 0x2200
movb $-0x80, -0x16(%rbp)
movq 0xa2bc(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0xa2aa(%rip), %rax      # 0xc210
movb 0x15(%rax), %al
leaq -0x16(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x1f8c
movb $0x1, -0x1(%rbp)
jmp 0x21f2
movl $0x186a0, %edi          # imm = 0x186A0
callq 0x10f0
movb $0x0, -0x16(%rbp)
movq 0xa277(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0xa265(%rip), %rax      # 0xc210
movb 0x15(%rax), %al
leaq -0x16(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x7360
cmpl $0x0, %eax
je 0x1fd1
movb $0x1, -0x1(%rbp)
jmp 0x21f2
movq 0xa240(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0xa22e(%rip), %rax      # 0xc210
movb 0x19(%rax), %al
leaq -0x16(%rbp), %rcx
movl $0x6, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x6df0
movsbl %al, %edx
movl $0x1, %eax
xorl %ecx, %ecx
cmpl $-0x1, %edx
cmovnel %ecx, %eax
cmpl $0x0, %eax
je 0x2018
movb $0x1, -0x1(%rbp)
jmp 0x21f2
movzbl -0x11(%rbp), %eax
andl $0x1, %eax
shll $0x2, %eax
movzbl -0x13(%rbp), %ecx
andl $0x1, %ecx
shll %ecx
orl %ecx, %eax
movzbl -0x15(%rbp), %ecx
andl $0x1, %ecx
orl %ecx, %eax
movb %al, -0x17(%rbp)
cmpb $0x0, -0x17(%rbp)
je 0x2070
movzbl -0x17(%rbp), %eax
cmpl $0x1, %eax
jne 0x2051
movb $0x1, 0xa1e4(%rip)      # 0xc233
jmp 0x206e
movzbl -0x17(%rbp), %eax
cmpl $0x2, %eax
jne 0x2063
movb $0x0, 0xa1d2(%rip)      # 0xc233
jmp 0x206c
movb $0x1, -0x1(%rbp)
jmp 0x21f2
jmp 0x206e
jmp 0x20ed
movq 0xa1a1(%rip), %rax      # 0xc218
movb (%rax), %sil
movq 0xa18f(%rip), %rax      # 0xc210
movb 0x3(%rax), %al
leaq -0x16(%rbp), %rcx
movl $0x1, %edx
movzbl %sil, %edi
movzbl %al, %esi
callq 0x6df0
movsbl %al, %edx
movl $0x1, %eax
xorl %ecx, %ecx
cmpl $-0x1, %edx
cmovnel %ecx, %eax
cmpl $0x0, %eax
je 0x20b7
movb $0x1, -0x1(%rbp)
jmp 0x21f2
movzbl -0x16(%rbp), %eax
andl $0xf, %eax
movb %al, -0x17(%rbp)
cmpb $0x0, -0x17(%rbp)
jne 0x20d0
movb $0x1, -0x1(%rbp)
jmp 0x21f2
movzbl -0x17(%rbp), %eax
cmpl $0x4, %eax
jne 0x20e2
movb $0x1, 0xa153(%rip)      # 0xc233
jmp 0x20e9
movb $0x0, 0xa14a(%rip)      # 0xc233
jmp 0x20eb
jmp 0x20ed
movb $-0x1, 0xa136(%rip)     # 0xc22a
movb $-0x1, 0xa12d(%rip)     # 0xc228
movb $-0x1, 0xa127(%rip)     # 0xc229
movb $-0x1, 0xa122(%rip)     # 0xc22b
movw $0xffff, 0xa11c(%rip)   # imm = 0xFFFF
movb $-0x1, 0xa117(%rip)     # 0xc230
movb $-0x1, 0xa112(%rip)     # 0xc232
movb $0x1, 0xa105(%rip)      # 0xc22c
movb $0x1, 0xa114(%rip)      # 0xc242
movb $0x0, 0xa10e(%rip)      # 0xc243
movb $0x0, 0xa0f9(%rip)      # 0xc235
movb $0x0, 0xa0f1(%rip)      # 0xc234
leaq 0xa0c6(%rip), %rdi      # 0xc210
addq $0x18, %rdi
addq $0xe, %rdi
xorl %esi, %esi
movl $0xc, %edx
callq 0x1080
movb $0x0, 0xa0df(%rip)      # 0xc244
movb $0x0, 0xa0d9(%rip)      # 0xc245
movw $0x0, 0xa0d1(%rip)      # 0xc246
movl $0x7d0, %edi            # imm = 0x7D0
callq 0x23e0
cmpb $0x0, %al
je 0x2189
movb $0x1, -0x1(%rbp)
jmp 0x21f2
movl $0x2, %edi
callq 0x24c0
cmpb $0x0, %al
je 0x219d
movb $0x1, -0x1(%rbp)
jmp 0x21f2
movl $0x2a, %edi
callq 0x2590
cmpb $0x0, %al
je 0x21b1
movb $0x1, -0x1(%rbp)
jmp 0x21f2
movl $0x32, %edi
callq 0x2670
cmpb $0x0, %al
je 0x21c5
movb $0x1, -0x1(%rbp)
jmp 0x21f2
xorl %edi, %edi
callq 0x2780
cmpb $0x0, %al
je 0x21d6
movb $0x1, -0x1(%rbp)
jmp 0x21f2
xorl %edi, %edi
callq 0x2860
cmpb $0x0, %al
je 0x21e7
movb $0x1, -0x1(%rbp)
jmp 0x21f2
xorl %edi, %edi
callq 0x2aa0
movb $0x0, -0x1(%rbp)
movb -0x1(%rbp), %al
addq $0x20, %rsp
popq %rbp
retq
nopl (%rax,%rax)