// File: lucaong[P]elixir_mlx90640/src/MLX90640_LINUX_I2C_Driver.cpp
// Function: MLX90640_I2CRead(unsigned char, unsigned short, unsigned short, unsigned short*)
int MLX90640_I2CRead(uint8_t slaveAddr, uint16_t startAddress, uint16_t nMemAddressRead, uint16_t *data)
{
  if(!i2c_fd){
    i2c_fd = open(i2c_device, O_RDWR);
  }

  int result;
  char cmd[2] = {(char)(startAddress >> 8), (char)(startAddress & 0xFF)};
  char buf[1664];
  uint16_t *p = data;
  struct i2c_msg i2c_messages[2];
  struct i2c_rdwr_ioctl_data i2c_messageset[1];

  i2c_messages[0].addr = slaveAddr;
  i2c_messages[0].flags = 0;
  i2c_messages[0].len = 2;
  i2c_messages[0].buf = cmd;

  i2c_messages[1].addr = slaveAddr;
  i2c_messages[1].flags = I2C_M_RD | I2C_M_NOSTART;
  i2c_messages[1].len = nMemAddressRead * 2;
  i2c_messages[1].buf = buf;

  i2c_messageset[0].msgs = i2c_messages;
  i2c_messageset[0].nmsgs = 2;

  memset(buf, 0, nMemAddressRead * 2);

  if (ioctl(i2c_fd, I2C_RDWR, &i2c_messageset) < 0) {
    printf("I2C Read Error!\n");
    return -1;
  }

  for(int count = 0; count < nMemAddressRead; count++){
    int i = count << 1;
    *p++ = ((uint16_t)buf[i] << 8) | buf[i+1];
  }

  return 0;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x6f0, %rsp            # imm = 0x6F0
movw %dx, %ax
movw %si, %dx
movb %dil, %sil
movb %sil, -0x5(%rbp)
movw %dx, -0x8(%rbp)
movw %ax, -0xa(%rbp)
movq %rcx, -0x18(%rbp)
cmpl $0x0, 0x4839(%rip)      # 0xa374
jne 0x5b56
movq 0x357c(%rip), %rdi      # 0x90c0
movl $0x2, %esi
movb $0x0, %al
callq 0x10a0
movl %eax, 0x481e(%rip)      # 0xa374
movzwl -0x8(%rbp), %eax
sarl $0x8, %eax
movb %al, -0x1e(%rbp)
movzwl -0x8(%rbp), %eax
andl $0xff, %eax
movb %al, -0x1d(%rbp)
movq -0x18(%rbp), %rax
movq %rax, -0x6a8(%rbp)
movzbl -0x5(%rbp), %eax
movw %ax, -0x6d0(%rbp)
movw $0x0, -0x6ce(%rbp)
movw $0x2, -0x6cc(%rbp)
leaq -0x1e(%rbp), %rax
movq %rax, -0x6c8(%rbp)
movzbl -0x5(%rbp), %eax
movw %ax, -0x6c0(%rbp)
movw $0x4001, -0x6be(%rbp)   # imm = 0x4001
movzwl -0xa(%rbp), %eax
shll %eax
movw %ax, -0x6bc(%rbp)
leaq -0x6a0(%rbp), %rax
movq %rax, -0x6b8(%rbp)
leaq -0x6d0(%rbp), %rax
movq %rax, -0x6e0(%rbp)
movl $0x2, -0x6d8(%rbp)
leaq -0x6a0(%rbp), %rdi
movzwl -0xa(%rbp), %eax
shll %eax
movslq %eax, %rdx
xorl %esi, %esi
callq 0x1090
movl 0x4771(%rip), %edi      # 0xa374
movl $0x707, %esi            # imm = 0x707
leaq -0x6e0(%rbp), %rdx
movb $0x0, %al
callq 0x10e0
cmpl $0x0, %eax
jge 0x5c35
leaq 0x4d9(%rip), %rdi       # 0x60fb
movb $0x0, %al
callq 0x1040
movl $0xffffffff, -0x4(%rbp) # imm = 0xFFFFFFFF
jmp 0x5cb9
movl $0x0, -0x6e4(%rbp)
movl -0x6e4(%rbp), %eax
movzwl -0xa(%rbp), %ecx
cmpl %ecx, %eax
jge 0x5cb2
movl -0x6e4(%rbp), %eax
shll %eax
movl %eax, -0x6e8(%rbp)
movslq -0x6e8(%rbp), %rax
movsbl -0x6a0(%rbp,%rax), %eax
movzwl %ax, %eax
shll $0x8, %eax
movl -0x6e8(%rbp), %ecx
addl $0x1, %ecx
movslq %ecx, %rcx
movsbl -0x6a0(%rbp,%rcx), %ecx
orl %ecx, %eax
movw %ax, %cx
movq -0x6a8(%rbp), %rax
movq %rax, %rdx
addq $0x2, %rdx
movq %rdx, -0x6a8(%rbp)
movw %cx, (%rax)
movl -0x6e4(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x6e4(%rbp)
jmp 0x5c3f
movl $0x0, -0x4(%rbp)
movl -0x4(%rbp), %eax
addq $0x6f0, %rsp            # imm = 0x6F0
popq %rbp
retq
nopw %cs:(%rax,%rax)