// File: /workspace/github_cpp_makefile_star2_O3/mrlordkaj[P]i2cdevlib-pi/MPU6050/MPU6050.cpp
// Function: MPU6050::writeMemoryBlock(unsigned char const*, unsigned short, unsigned char, unsigned char, bool, bool)
bool MPU6050::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=0;
    uint8_t *progBuffer=0;
    uint16_t i;
    uint8_t j;
    if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    for (i = 0; i < dataSize;) {
        // determine correct chunk size according to bank position and data size
        chunkSize = MPU6050_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, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);

        // verify data if needed
        if (verify && verifyBuffer) {
            setMemoryBank(bank);
            setMemoryStartAddress(address);
            I2Cdev::readBytes(devAddr, MPU6050_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, %r15d
movl %edx, %ebx
movq %rsi, 0x28(%rsp)
movq %rdi, %r14
movzbl (%rdi), %edi
movl %ecx, 0xc(%rsp)
movl %ecx, %edx
andl $0x1f, %edx
movl $0x6d, %esi
callq 0x1f5d0
movzbl (%r14), %edi
movl $0x6e, %esi
movl %r15d, %edx
callq 0x1f5d0
testl %ebp, %ebp
je 0x2faf6
movl $0x10, %edi
callq 0x1f7b0
movq %rax, 0x10(%rsp)
jmp 0x2faff
movq $0x0, 0x10(%rsp)
cmpb $0x0, 0x70(%rsp)
je 0x2fb15
movl $0x10, %edi
callq 0x1f7b0
movq %rax, %r13
jmp 0x2fb18
xorl %r13d, %r13d
testw %bx, %bx
movl %ebp, 0x1c(%rsp)
je 0x2fc80
movq %r14, 0x30(%rsp)
movzwl %bx, %eax
cmpq $0x0, 0x10(%rsp)
setne %cl
andb %bpl, %cl
movb %cl, 0xb(%rsp)
movl %eax, %ebp
xorl %r12d, %r12d
xorl %ebx, %ebx
movl %eax, 0x20(%rsp)
leal 0x10(%r12), %eax
movl %ebp, %ecx
subl %ebx, %ecx
cmpl %ebp, %eax
movl $0x10, %eax
cmovbel %eax, %ecx
movzbl %cl, %eax
movl %r15d, 0x24(%rsp)
movzbl %r15b, %ebx
movl $0x100, %ebp            # imm = 0x100
subl %ebx, %ebp
cmpl %eax, %ebp
cmovael %ecx, %ebp
movzbl %bpl, %r15d
cmpb $0x0, 0x70(%rsp)
je 0x2fb9f
testb %bpl, %bpl
je 0x2fba7
movl %r12d, %eax
addq 0x28(%rsp), %rax
xorl %ecx, %ecx
movb (%rax,%rcx), %dl
movb %dl, (%r13,%rcx)
incq %rcx
cmpq %rcx, %r15
jne 0x2fb8d
jmp 0x2fba7
movl %r12d, %r13d
addq 0x28(%rsp), %r13
movq 0x30(%rsp), %r14
movzbl (%r14), %edi
movl $0x6f, %esi
movl %r15d, %edx
movq %r13, %rcx
callq 0x1f150
cmpb $0x0, 0xb(%rsp)
je 0x2fc24
movl 0xc(%rsp), %eax
andb $0x1f, %al
movzbl (%r14), %edi
movzbl %al, %edx
movl $0x6d, %esi
callq 0x1f5d0
movzbl (%r14), %edi
movl $0x6e, %esi
movl %ebx, %edx
callq 0x1f5d0
movzbl (%r14), %edi
movq 0x1734f(%rip), %rax     # 0x46f48
movzwl (%rax), %r8d
movl $0x6f, %esi
movl %r15d, %edx
movq 0x10(%rsp), %rbx
movq %rbx, %rcx
callq 0x1f630
movq %r13, %rdi
movq %rbx, %rsi
movq %r15, %rdx
callq 0x1f640
testl %eax, %eax
jne 0x2fc95
movl %r12d, %ebx
addl %r15d, %ebx
movl 0x24(%rsp), %r15d
addb %bpl, %r15b
movzwl %bx, %r12d
movl 0x20(%rsp), %ebp
cmpl %ebp, %r12d
jae 0x2fc77
cmpb $0x1, %r15b
movl 0xc(%rsp), %eax
adcb $0x0, %al
movl %eax, 0xc(%rsp)
andb $0x1f, %al
movq 0x30(%rsp), %r14
movzbl (%r14), %edi
movzbl %al, %edx
movl $0x6d, %esi
callq 0x1f5d0
movzbl (%r14), %edi
movzbl %r15b, %edx
movl $0x6e, %esi
callq 0x1f5d0
cmpl %ebp, %r12d
jb 0x2fb48
cmpb $0x0, 0x1c(%rsp)
je 0x2fc91
movq 0x10(%rsp), %rdi
callq 0x1f6e0
movb $0x1, %bl
jmp 0x2fca1
movq 0x10(%rsp), %rdi
callq 0x1f6e0
xorl %ebx, %ebx
cmpb $0x0, 0x70(%rsp)
je 0x2fcb0
movq %r13, %rdi
callq 0x1f6e0
movl %ebx, %eax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop