// File: /workspace/github_c_makefile_star2_O2/Openvario[P]sensord/ms5611.c
// Function: ms5611_read_temp
int ms5611_read_temp(t_ms5611 *sensor, int glitch)
{
	//variables
	uint8_t buf[10]={0x00};
	//variables
	//long d2;
	int64_t OFF2=0;
	int64_t SENS2=0;
	int64_t T2=0;

	// read result
	buf[0] = 0x00;
	if ((write(sensor->fd, buf, 1)) != 1) {								// Send register we want to read from
		fprintf(stderr, "Error writing to i2c slave(%s)\n", __func__);
		return(1);
	}

	if (read(sensor->fd, buf, 3) != 3) {								// Read back data into buf[]
		fprintf(stderr, "Unable to read from slave(%s)\n", __func__);
		return(1);
	}

	// Put temperature readings together
	sensor->D2l = sensor->D2;
	sensor->D2 = (buf[0] << 16) + (buf[1] << 8) + buf[2];
	if (glitch==0)
		if ((sensor->D2<=sensor->D2l+100e3) && (sensor->D2l<=sensor->D2+300))
		{
			sensor->D2f = (sensor->D2f*7+sensor->D2)/8;

			// calculate dT and absolute temperature
			sensor->dT = sensor->D2f - sensor->C5s;
			sensor->temp = 2000 + (((int64_t)sensor->dT * sensor->C6) / 8388608);

			// these calculations are copied from the data sheet
			//OFF = C2 * 2**16 + (C4 * dT) / 2**7
			//SENS = C1 * 2**15 + (C3 * dT) / 2**8
			//P = (D1 * SENS / 2**21 - OFF) / 2**15

			sensor->off = (sensor->C2s + (((int64_t)sensor->C4 * sensor->dT) >> 7));
			sensor->sens = (sensor->C1s + (((int64_t)sensor->C3 * sensor->dT) >> 8));

			if (sensor->secordcomp)
			{
				// second order correction
				if (sensor->temp < 2000)
				{
					T2 = (sensor->dT * sensor->dT) >> 31;
					OFF2 = 5 * ((int64_t)(sensor->temp - 2000) * (sensor->temp - 2000)) >> 1;
					SENS2 = 5 * ((int64_t)(sensor->temp - 2000) * (sensor->temp - 2000)) >> 2;

					if (sensor->temp < -1500)
					{
						OFF2 = OFF2 + 7 * ((int64_t)(sensor->temp + 1500) * (sensor->temp + 1500));
						SENS2 = SENS2 + ((11 * ((int64_t)(sensor->temp + 1500) * (sensor->temp + 1500))) >> 1);
					}

					sensor->temp = sensor->temp - T2;
					sensor->off = sensor->off - OFF2;
					sensor->sens = sensor->sens - SENS2;
				}
			}
		}


	// debug print
	ddebug_print("%s @ 0x%x: D2 = %u\n", __func__, sensor->address, sensor->D2);
	ddebug_print("%s @ 0x%x: dT = %d\n", __func__, sensor->address, sensor->dT);
	debug_print("%s @ 0x%x: temp = %d\n", __func__, sensor->address, sensor->temp);

	return(0);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %rbx
subq $0x18, %rsp
movl %esi, %ebp
movq %rdi, %rbx
leaq 0x8(%rsp), %rsi
andw $0x0, 0x8(%rsi)
andq $0x0, (%rsi)
movl (%rdi), %edi
pushq $0x1
popq %rdx
callq 0x20b0
cmpq $0x1, %rax
jne 0x2c3c
movl (%rbx), %edi
leaq 0x8(%rsp), %rsi
pushq $0x3
popq %rdx
callq 0x2180
cmpq $0x3, %rax
jne 0x2c4f
movl 0x28(%rbx), %eax
movl %eax, 0x2c(%rbx)
movzbl 0x8(%rsp), %ecx
shll $0x10, %ecx
movzbl 0x9(%rsp), %edx
shll $0x8, %edx
orl %ecx, %edx
movzbl 0xa(%rsp), %r8d
orl %edx, %r8d
movl %r8d, 0x28(%rbx)
testl %ebp, %ebp
jne 0x2c16
movl %r8d, %ecx
movq %rax, %rdx
addq $0x186a0, %rdx          # imm = 0x186A0
cmpq %rcx, %rdx
setb %cl
leal 0x12c(%r8), %edx
cmpl %edx, %eax
seta %al
orb %cl, %al
jne 0x2c16
imull $0x7, 0x30(%rbx), %eax
addl %r8d, %eax
shrl $0x3, %eax
movl %eax, 0x30(%rbx)
subl 0x14(%rbx), %eax
movl %eax, 0x34(%rbx)
movslq %eax, %rdi
movzwl 0x18(%rbx), %ecx
imulq %rdi, %rcx
movl $0x800000, %esi         # imm = 0x800000
movq %rcx, %rax
cqto
idivq %rsi
leal 0x7d0(%rax), %edx
movl %edx, 0x38(%rbx)
movl 0x8(%rbx), %r9d
movl 0xc(%rbx), %esi
movzwl 0x12(%rbx), %edx
imulq %rdi, %rdx
sarq $0x7, %rdx
addq %rsi, %rdx
movq %rdx, 0x40(%rbx)
movzwl 0x10(%rbx), %esi
imulq %rdi, %rsi
sarq $0x8, %rsi
addq %r9, %rsi
movq %rsi, 0x48(%rbx)
cmpl $0x0, 0x98(%rbx)
je 0x2c16
cmpq $-0x800000, %rcx        # imm = 0xFF800000
jg 0x2c16
movq %rax, %rdi
imulq %rax, %rdi
leaq (%rdi,%rdi,4), %rdi
movq %rdi, %r9
shrq %r9
shrq $0x2, %rdi
movabsq $-0x6d6800000, %r10     # imm = 0xFFFFFFF929800000
cmpq %r10, %rcx
ja 0x2c08
addq $0xdac, %rax            # imm = 0xDAC
imulq %rax, %rax
imulq $0x7, %rax, %rcx
addq %rcx, %r9
imulq $0xb, %rax, %rax
shrq %rax
addq %rax, %rdi
subq %r9, %rdx
movq %rdx, 0x40(%rbx)
subq %rdi, %rsi
movq %rsi, 0x48(%rbx)
leaq 0xa857(%rip), %r14      # 0xd474
movl (%r14), %eax
cmpl $0x2, %eax
jge 0x2c73
xorl %ebp, %ebp
testl %eax, %eax
jg 0x2cc6
movl %ebp, %eax
addq $0x18, %rsp
popq %rbx
popq %r14
popq %r15
popq %rbp
retq
movq 0xa3b5(%rip), %rax      # 0xcff8
movq (%rax), %rdi
leaq 0x55f8(%rip), %rsi      # 0x8245
jmp 0x2c60
movq 0xa3a2(%rip), %rax      # 0xcff8
movq (%rax), %rdi
leaq 0x5616(%rip), %rsi      # 0x8276
leaq 0x55fe(%rip), %rdx      # 0x8265
xorl %eax, %eax
callq 0x21c0
pushq $0x1
popq %rbp
jmp 0x2c2f
movq 0xa37e(%rip), %r15      # 0xcff8
movq (%r15), %rdi
movzbl 0x4(%rbx), %ecx
leaq 0x560d(%rip), %rsi      # 0x8295
leaq 0x55d6(%rip), %rdx      # 0x8265
xorl %eax, %eax
callq 0x21c0
movl (%r14), %eax
cmpl $0x2, %eax
jl 0x2c25
movq (%r15), %rdi
movzbl 0x4(%rbx), %ecx
movl 0x34(%rbx), %r8d
leaq 0x55f9(%rip), %rsi      # 0x82a9
leaq 0x55ae(%rip), %rdx      # 0x8265
xorl %eax, %eax
callq 0x21c0
movl (%r14), %eax
jmp 0x2c25
movq 0xa32b(%rip), %rax      # 0xcff8
movq (%rax), %rdi
movzbl 0x4(%rbx), %ecx
movl 0x38(%rbx), %r8d
leaq 0x55de(%rip), %rsi      # 0x82bd
leaq 0x557f(%rip), %rdx      # 0x8265
xorl %ebp, %ebp
xorl %eax, %eax
callq 0x21c0
jmp 0x2c2f