// File: /workspace/github_cpp_makefile_star2_O1/mrlordkaj[P]i2cdevlib-pi/MPU6050/MPU6050.cpp
// Function: MPU6050::writeDMPConfigurationSet(unsigned char const*, unsigned short, bool)
bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
    uint8_t *progBuffer = 0;
	uint8_t success, special;
    uint16_t i, j;
    if (useProgMem) {
        progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary
    }

    // config set data is a long string of blocks with the following structure:
    // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
    uint8_t bank, offset, length;
    for (i = 0; i < dataSize;) {
        if (useProgMem) {
            bank = pgm_read_byte(data + i++);
            offset = pgm_read_byte(data + i++);
            length = pgm_read_byte(data + i++);
        } else {
            bank = data[i++];
            offset = data[i++];
            length = data[i++];
        }

        // write data or perform special action
        if (length > 0) {
            // regular block of data to write
            /*Serial.print("Writing config block to bank ");
            Serial.print(bank);
            Serial.print(", offset ");
            Serial.print(offset);
            Serial.print(", length=");
            Serial.println(length);*/
            if (useProgMem) {
                if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length);
                for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j);
            } else {
                progBuffer = (uint8_t *)data + i;
            }
            success = writeMemoryBlock(progBuffer, length, bank, offset, true);
            i += length;
        } else {
            // special instruction
            // NOTE: this kind of behavior (what and when to do certain things)
            // is totally undocumented. This code is in here based on observed
            // behavior only, and exactly why (or even whether) it has to be here
            // is anybody's guess for now.
            if (useProgMem) {
                special = pgm_read_byte(data + i++);
            } else {
                special = data[i++];
            }
            /*Serial.print("Special command code ");
            Serial.print(special, HEX);
            Serial.println(" found...");*/
            if (special == 0x01) {
                // enable DMP-related interrupts
                
                //setIntZeroMotionEnabled(true);
                //setIntFIFOBufferOverflowEnabled(true);
                //setIntDMPEnabled(true);
                I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32);  // single operation

                success = true;
            } else {
                // unknown special command
                success = false;
            }
        }
        
        if (!success) {
            if (useProgMem) free(progBuffer);
            return false; // uh oh
        }
    }
    if (useProgMem) free(progBuffer);
    return true;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x28, %rsp
movl %edx, 0x1c(%rsp)
movq %rsi, %r14
movq %rdi, 0x20(%rsp)
movl %ecx, 0x18(%rsp)
testl %ecx, %ecx
je 0x2fded
movl $0x8, %edi
callq 0x1f7b0
movq %rax, %rbx
jmp 0x2fdef
xorl %ebx, %ebx
xorl %r15d, %r15d
movl %r15d, %ebp
cmpw 0x1c(%rsp), %r15w
jae 0x2fed7
leal 0x2(%rbp), %eax
movzwl %ax, %eax
movzbl (%r14,%rax), %r13d
leal 0x3(%rbp), %r15d
movzwl %r15w, %r12d
testq %r13, %r13
je 0x2fe73
movzwl %bp, %eax
movb (%r14,%rax), %dil
movb 0x1(%r14,%rax), %sil
cmpb $0x0, 0x18(%rsp)
je 0x2fe9b
cmpb $0x9, %r13b
jb 0x2fe4e
movb %dil, 0x17(%rsp)
movq %rbx, %rdi
movl %esi, %ebx
movq %r13, %rsi
callq 0x1f9a0
movb 0x17(%rsp), %dil
movl %ebx, %esi
movq %rax, %rbx
addq %r14, %r12
cmpb $0x2, %r13b
movl %r13d, %eax
movl $0x1, %ecx
cmovbl %ecx, %eax
xorl %ecx, %ecx
movb (%r12,%rcx), %dl
movb %dl, (%rbx,%rcx)
incq %rcx
cmpq %rcx, %rax
jne 0x2fe62
jmp 0x2fea1
leal 0x4(%rbp), %r15d
movb $0x1, %al
cmpb $0x1, (%r14,%r12)
jne 0x2fecf
movq 0x20(%rsp), %rax
movzbl (%rax), %edi
movl $0x38, %esi
movl $0x32, %edx
callq 0x1f5d0
xorl %eax, %eax
jmp 0x2fecf
addq %r14, %r12
movq %r12, %rbx
movl $0x0, (%rsp)
movzwl %r13w, %r13d
movzbl %dil, %ecx
movzbl %sil, %r8d
movq 0x20(%rsp), %rdi
movq %rbx, %rsi
movl %r13d, %edx
movl $0x1, %r9d
callq 0x1fae0
addl %r13d, %r15d
xorb $0x1, %al
testb %al, %al
je 0x2fdf2
cmpb $0x0, 0x18(%rsp)
je 0x2fee6
movq %rbx, %rdi
callq 0x1f6e0
cmpw 0x1c(%rsp), %bp
setae %al
addq $0x28, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop