Why load DMP firmware failed?

program list:

//(1)Resetting MPU6050...
I2C_writeBits(Address_MPU60X0, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1,1);

for(i=0;i<20000;i++)
wait5us();

//(2)disable sleep mode
I2C_writeBits(Address_MPU60X0, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1,0);
data = I2C_RegRead(Address_MPU60X0 , MPU6050_RA_PWR_MGMT_1); //bit6


//(3)get MPU hardware revision
setMemoryBank(0x70);

setMemoryStartAddress(0x06);
data = I2C_RegRead(Address_MPU60X0 , MPU6050_RA_MEM_R_W); //data is the hwRevision
setMemoryBank(0x00);

//(4)check OTP bank valid
//getOTPBankValid(); I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer);
//readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer);
data = I2C_RegRead(Address_MPU60X0 , MPU6050_RA_XG_OFFS_TC); //Bit0==0:Invalid,Bit0==1:Valid


//(5)get X/Y/Z gyro offsets
data = I2C_RegRead(Address_MPU60X0 , MPU6050_RA_XG_OFFS_TC);
xgOffsetTC = (data>>1)& 0x3F;
data = I2C_RegRead(Address_MPU60X0 , MPU6050_RA_YG_OFFS_TC);
ygOffsetTC = (data>>1)& 0x3F;
data = I2C_RegRead(Address_MPU60X0 , MPU6050_RA_ZG_OFFS_TC);
zgOffsetTC = (data>>1)& 0x3F;

//(6)setup weird slave stuff (?)
//setSlaveAddress(0, 0x7F);
//writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address);
I2C_RegWrite( Address_MPU60X0 , MPU6050_RA_I2C_SLV0_ADDR, 0x7F);

//setI2CMasterModeEnabled(false);
//writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
I2C_writeBits(Address_MPU60X0, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 1,1);

//setSlaveAddress(0, 0x68);
I2C_RegWrite( Address_MPU60X0 , MPU6050_RA_I2C_SLV0_ADDR, 0x68);

//resetI2CMaster();
//writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true);
I2C_writeBits(Address_MPU60X0, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1,1);

for(i=0;i<20000;i++)
wait5us();


//(7)load DMP code into memory banks
MPU6000_Load_Firmware(MPU6050_DMP_CODE_SIZE, dmpMemory);


int MPU6000_Load_Firmware(unsigned short length, const unsigned char *firmware)
{
unsigned char progBuffer[16],verifyBuffer[16];
unsigned char temp[2];
int i,j,dataSize;
unsigned char chunkSize;
unsigned short bank,address;
int k;

bank = 0;
address = 0;
dataSize = length;

//setMemoryBank(bank);
//setMemoryStartAddress(address);

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;
// write the chunk of data as specified
for (j = 0; j < chunkSize; j++) progBuffer[j] = *(firmware + i + j);

temp[0] = (unsigned char)(address >> 8);
temp[1] = (unsigned char)(address & 0xFF);
I2C_RegWriteN(Address_MPU60X0, MPU6050_RA_BANK_SEL, 2, temp);
I2C_RegWriteN(Address_MPU60X0, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);


// verify data if needed
//setMemoryBank(bank);
//setMemoryStartAddress(address);

temp[0] = (unsigned char)(address >> 8);
temp[1] = (unsigned char)(address & 0xFF);
I2C_RegWriteN(Address_MPU60X0, MPU6050_RA_BANK_SEL, 2, temp);
I2C_RegReadN(Address_MPU60X0, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);


if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0)
{
return 0; //
}

for(i=0;k<2000;k++) wait5us();

i += chunkSize;
address += chunkSize;
}

return 0;

}

I have solved this problem . The question is found out at the function I2C_RegReadN, the communication sequence with MPU6000 is not correct, so I received all oxff.