// File: /workspace/github_cpp_makefile_star2_O2/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, %ebx
movl %r8d, %ebp
movl %edx, %r14d
movq %rsi, 0x30(%rsp)
movq %rdi, %r12
movq $0x0, 0x10(%rsp)
movl %ecx, %r15d
movl %ecx, %esi
xorl %edx, %edx
xorl %ecx, %ecx
callq 0x1fb40
movq %r12, %rdi
movl %ebp, %esi
callq 0x1fdd0
movl %ebx, 0x18(%rsp)
testl %ebx, %ebx
je 0x2dffd
pushq $0x10
popq %rdi
callq 0x1fab0
movq %rax, 0x10(%rsp)
xorl %r13d, %r13d
cmpb $0x0, 0x70(%rsp)
movq %r12, 0x28(%rsp)
je 0x2e019
pushq $0x10
popq %rdi
callq 0x1fab0
movq %rax, %rbx
jmp 0x2e01b
xorl %ebx, %ebx
movzwl %r14w, %r12d
cmpq $0x0, 0x10(%rsp)
setne %al
andb 0x18(%rsp), %al
movb %al, 0xf(%rsp)
movl %r12d, 0x1c(%rsp)
movl %r15d, 0x20(%rsp)
movzbl %r15b, %eax
movl %eax, 0x24(%rsp)
movl %r13d, %eax
movzwl %ax, %r13d
cmpl %r12d, %r13d
jae 0x2e161
leal 0x10(%r13), %ecx
movl %r12d, %edx
subl %eax, %edx
cmpl %r12d, %ecx
pushq $0x10
popq %rax
cmovbel %eax, %edx
movzbl %dl, %eax
movzbl %bpl, %r15d
movl $0x100, %ecx            # imm = 0x100
subl %r15d, %ecx
cmpl %eax, %ecx
cmovael %edx, %ecx
movl %r13d, %eax
addq 0x30(%rsp), %rax
movzbl %cl, %r14d
cmpb $0x0, 0x70(%rsp)
pushq $0x6f
popq %rsi
je 0x2e0a0
xorl %ecx, %ecx
cmpq %rcx, %r14
je 0x2e0a3
movb (%rax,%rcx), %dl
movb %dl, (%rbx,%rcx)
incq %rcx
jmp 0x2e090
movq %rax, %rbx
movq 0x28(%rsp), %r12
movzbl (%r12), %edi
movl %r14d, %edx
movq %rbx, %rcx
callq 0x1f1f0
cmpb $0x0, 0xf(%rsp)
je 0x2e113
movq %r12, %rdi
movl 0x24(%rsp), %esi
xorl %edx, %edx
xorl %ecx, %ecx
callq 0x1fb40
movq %r12, %rdi
movl %r15d, %esi
callq 0x1fdd0
movzbl (%r12), %edi
movq 0x1ae62(%rip), %rax     # 0x48f48
movzwl (%rax), %r8d
pushq $0x6f
popq %rsi
movl %r14d, %edx
movq 0x10(%rsp), %r15
movq %r15, %rcx
callq 0x1f870
movq %rbx, %rdi
movq %r15, %rsi
movq %r14, %rdx
callq 0x1f880
testl %eax, %eax
jne 0x2e196
addl %r14d, %r13d
addb %r14b, %bpl
movzwl %r13w, %ecx
movl %r13d, %eax
movl 0x1c(%rsp), %r12d
cmpl %r12d, %ecx
jae 0x2e045
cmpb $0x1, %bpl
movl 0x20(%rsp), %r15d
adcb $0x0, %r15b
movzbl %r15b, %esi
movq 0x28(%rsp), %r14
movq %r14, %rdi
xorl %edx, %edx
xorl %ecx, %ecx
callq 0x1fb40
movzbl %bpl, %esi
movq %r14, %rdi
callq 0x1fdd0
jmp 0x2e035
cmpb $0x0, 0x18(%rsp)
je 0x2e172
movq 0x10(%rsp), %rdi
callq 0x1f9c0
cmpb $0x0, 0x70(%rsp)
je 0x2e181
movq %rbx, %rdi
callq 0x1f9c0
cmpl %r12d, %r13d
setae %al
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x10(%rsp), %rdi
callq 0x1f9c0
cmpb $0x0, 0x70(%rsp)
movl 0x1c(%rsp), %r12d
jne 0x2e179
jmp 0x2e181