// File: /workspace/github_cpp_makefile_star2_O1/mrlordkaj[P]i2cdevlib-pi/MPU9150/MPU9150.cpp
// Function: MPU9150::writeMemoryBlock(unsigned char const*, unsigned short, unsigned char, unsigned char, bool, bool)
bool MPU9150::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
    setMemoryBank(bank);
    setMemoryStartAddress(address);
    uint8_t chunkSize;
    uint8_t *verifyBuffer;
    uint8_t *progBuffer;
    uint16_t i;
    uint8_t j;
    if (verify) verifyBuffer = (uint8_t *)malloc(MPU9150_DMP_MEMORY_CHUNK_SIZE);
    if (useProgMem) progBuffer = (uint8_t *)malloc(MPU9150_DMP_MEMORY_CHUNK_SIZE);
    else progBuffer = NULL;
    for (i = 0; i < dataSize;) {
        // determine correct chunk size according to bank position and data size
        chunkSize = MPU9150_DMP_MEMORY_CHUNK_SIZE;

        // make sure we don't go past the data size
        if (i + chunkSize > dataSize) chunkSize = dataSize - i;

        // make sure this chunk doesn't go past the bank boundary (256 bytes)
        if (chunkSize > 256 - address) chunkSize = 256 - address;
        
        if (useProgMem) {
            // write the chunk of data as specified
            for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
        } else {
            // write the chunk of data as specified
            progBuffer = (uint8_t *)data + i;
        }

        I2Cdev::writeBytes(devAddr, MPU9150_RA_MEM_R_W, chunkSize, progBuffer);

        // verify data if needed
        if (verify && verifyBuffer) {
            setMemoryBank(bank);
            setMemoryStartAddress(address);
            I2Cdev::readBytes(devAddr, MPU9150_RA_MEM_R_W, chunkSize, verifyBuffer);
            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
                /*Serial.print("Block write verification error, bank ");
                Serial.print(bank, DEC);
                Serial.print(", address ");
                Serial.print(address, DEC);
                Serial.print("!\nExpected:");
                for (j = 0; j < chunkSize; j++) {
                    Serial.print(" 0x");
                    if (progBuffer[j] < 16) Serial.print("0");
                    Serial.print(progBuffer[j], HEX);
                }
                Serial.print("\nReceived:");
                for (uint8_t j = 0; j < chunkSize; j++) {
                    Serial.print(" 0x");
                    if (verifyBuffer[i + j] < 16) Serial.print("0");
                    Serial.print(verifyBuffer[i + j], HEX);
                }
                Serial.print("\n");*/
                free(verifyBuffer);
                if (useProgMem) free(progBuffer);
                return false; // uh oh.
            }
        }

        // increase byte index by [chunkSize]
        i += chunkSize;

        // uint8_t automatically wraps to 0 at 256
        address += chunkSize;

        // if we aren't done, update bank (if necessary) and address
        if (i < dataSize) {
            if (address == 0) bank++;
            setMemoryBank(bank);
            setMemoryStartAddress(address);
        }
    }
    if (verify) free(verifyBuffer);
    if (useProgMem) free(progBuffer);
    return true;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movl %r9d, %ebp
movl %r8d, %r14d
movl %edx, %ebx
movq %rsi, 0x28(%rsp)
movq %rdi, %r15
movzbl (%rdi), %edi
movl %ecx, 0x10(%rsp)
movl %ecx, %edx
andl $0x1f, %edx
movl $0x6d, %esi
callq 0x1f5d0
movzbl (%r15), %edi
movl $0x6e, %esi
movl %r14d, 0x14(%rsp)
movl %r14d, %edx
callq 0x1f5d0
testl %ebp, %ebp
je 0x32dcd
movl $0x10, %edi
callq 0x1f7b0
movq %rax, 0x18(%rsp)
cmpb $0x0, 0x70(%rsp)
je 0x32de3
movl $0x10, %edi
callq 0x1f7b0
movq %rax, %r13
jmp 0x32de6
xorl %r13d, %r13d
testw %bx, %bx
sete %r12b
movl %ebp, 0x20(%rsp)
je 0x32f67
movq %r15, 0x30(%rsp)
movzwl %bx, %eax
cmpq $0x0, 0x18(%rsp)
setne %cl
andb %bpl, %cl
movb %cl, 0xf(%rsp)
movl %eax, %ebp
xorl %r15d, %r15d
xorl %r14d, %r14d
movl 0x14(%rsp), %ebx
movl %eax, 0x24(%rsp)
leal 0x10(%r15), %eax
movl %ebp, %ecx
subl %r14d, %ecx
cmpl %ebp, %eax
movl $0x10, %eax
cmovbel %eax, %ecx
movzbl %cl, %eax
movl %ebx, 0x14(%rsp)
movzbl %bl, %r14d
movl $0x100, %ebp            # imm = 0x100
subl %r14d, %ebp
cmpl %eax, %ebp
cmovael %ecx, %ebp
movzbl %bpl, %ebx
cmpb $0x0, 0x70(%rsp)
movb %r12b, 0xe(%rsp)
je 0x32e7b
testb %bpl, %bpl
je 0x32e83
movl %r15d, %eax
addq 0x28(%rsp), %rax
xorl %ecx, %ecx
movb (%rax,%rcx), %dl
movb %dl, (%r13,%rcx)
incq %rcx
cmpq %rcx, %rbx
jne 0x32e69
jmp 0x32e83
movl %r15d, %r13d
addq 0x28(%rsp), %r13
movq 0x30(%rsp), %r12
movzbl (%r12), %edi
movl $0x6f, %esi
movl %ebx, %edx
movq %r13, %rcx
callq 0x1f150
cmpb $0x0, 0xf(%rsp)
je 0x32f07
movl 0x10(%rsp), %eax
andb $0x1f, %al
movzbl (%r12), %edi
movzbl %al, %edx
movl $0x6d, %esi
callq 0x1f5d0
movzbl (%r12), %edi
movl $0x6e, %esi
movl %r14d, %edx
callq 0x1f5d0
movzbl (%r12), %edi
movq 0x1406f(%rip), %rax     # 0x46f48
movzwl (%rax), %r8d
movl $0x6f, %esi
movl %ebx, %edx
movq 0x18(%rsp), %r14
movq %r14, %rcx
callq 0x1f630
movq %r13, %rdi
movq %r14, %rsi
movq %rbx, %rdx
callq 0x1f640
testl %eax, %eax
jne 0x32f9d
movl %r15d, %r14d
addl %ebx, %r14d
movl 0x14(%rsp), %ebx
addb %bpl, %bl
movzwl %r14w, %eax
movl 0x24(%rsp), %ebp
cmpl %ebp, %eax
jae 0x32f56
cmpb $0x1, %bl
movl 0x10(%rsp), %eax
adcb $0x0, %al
movl %eax, 0x10(%rsp)
andb $0x1f, %al
movq 0x30(%rsp), %r15
movzbl (%r15), %edi
movzbl %al, %edx
movl $0x6d, %esi
callq 0x1f5d0
movzbl (%r15), %edi
movzbl %bl, %edx
movl $0x6e, %esi
callq 0x1f5d0
movzwl %r14w, %r15d
cmpl %ebp, %r15d
setae %r12b
jb 0x32e1f
cmpb $0x0, 0x20(%rsp)
je 0x32f78
movq 0x18(%rsp), %rdi
callq 0x1f6e0
cmpb $0x0, 0x70(%rsp)
je 0x32f87
movq %r13, %rdi
callq 0x1f6e0
andb $0x1, %r12b
movl %r12d, %eax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x18(%rsp), %rdi
callq 0x1f6e0
cmpb $0x0, 0x70(%rsp)
movb 0xe(%rsp), %r12b
jne 0x32f7f
jmp 0x32f87
nop