Xin giúp đỡ về I2C cua dsPIC 4011

[pactache]

Mình đang thực hiện giao tiếp I2C giữa 2 con dsPIC 4011 nhưng con master chỉ chạy đến dòng while(!IFS0bits.MI2CIF); thì bị đứng ở đó không chạy tiếp. Mình đã nhìn theo waveform để viết nhưng vẫn không chạy, mong mọi người xem giúp mình với, dưới đây là code master và slave.
Code Master

/*;------------------------------------------------------------------*/
#include "main.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>

#define MAXSPEED        2000
#define HALFMAXSPEED    1000
#define driver_x         0x10
#define driver_y        0x20

  //Khai bao bien

  /* Baud rate is set for 100 Khz */

  config2 = 0x90;

  /* Configure I2C for 7 bit address mode */

  config1 = (I2C_ON & I2C_IDLE_CON & I2C_CLK_HLD &

             I2C_IPMI_DIS & I2C_7BIT_ADD &

             I2C_SLW_DIS & I2C_SM_DIS &

             I2C_GCALL_DIS & I2C_STR_DIS &

             I2C_ACK & I2C_ACK_DIS & I2C_RCV_DIS &

             I2C_STOP_DIS & I2C_RESTART_DIS &

             I2C_START_DIS);    // config = 0x8200;  */ Enable I2C and Disable Slew Rate Control */

//Khai bao ham

 
//Chuong trinh chinh

void main(void)
{
    _TRISE5 = 0;
    _LATE5    = 0;
    _TRISE4 = 0;
    _LATE4    = 0;
    _TRISE3 = 0;
    _LATE3    = 0;
    _TRISE2 = 0;
    _LATE2    = 0;

    while(1)
    {
          OpenI2CMaster(config1,config2);

          IdleI2C();
        
          StartI2C();
        
          /* Wait till Start sequence is completed */
        
          while(I2CCONbits.SEN);
        
          /* Clear interrupt flag */
        
          IFS0bits.MI2CIF = 0;
        
          /* Write Slave address and set master for transmission */
        
          MasterWriteI2C(0x40);
        
          /* Wait till address is transmitted */
        
          while(I2CSTATbits.TBF);  // 8 clock cycles
_LATE5    = 1;
          while(I2CSTATbits.ACKSTAT);
_LATE4    = 1;
          while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
        
          IFS0bits.MI2CIF = 0;     // Clear interrupt flag
_LATE3    = 1;
        
          /* Transmit string of data */
        
          MasterWriteI2C(0xAA);               // write a byte

          while(I2CSTATbits.TBF);          //Wait till data is transmitted.
_LATE2    = 1;
          IdleI2C();
        
          StopI2C();
        
          /* Wait till stop sequence is completed */
        
          while(I2CCONbits.PEN);
        
          CloseI2C();
    }
}

//------------------------------------------------------------------------------
// Cac chuong trinh con va chuong trinh phuc vu ngat neu co
//------------------------------------------------------------------------------

Code Slave

/*;------------------------------------------------------------------*/
#include "main.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>

#define MAXSPEED        2000
#define HALFMAXSPEED    1000
#define driver_x         0x10
#define driver_y        0x20

  //Khai bao bien

  /* Baud rate is set for 100 Khz */

  config2 = 0x20;

  /* Configure I2C for 7 bit address mode */

  config1 = (I2C_ON & I2C_IDLE_CON & I2C_CLK_REL &

             I2C_IPMI_DIS & I2C_7BIT_ADD &

             I2C_SLW_DIS & I2C_SM_DIS &

             I2C_GCALL_DIS & I2C_STR_DIS &

             I2C_ACK & I2C_ACK_DIS & I2C_RCV_DIS &

             I2C_STOP_DIS & I2C_RESTART_DIS &

             I2C_START_DIS);    // config1 = 0x9200;  */ Enable I2C , Disable Slew Rate Control and Release SCL Clock */

//Khai bao ham

 
//Chuong trinh chinh

void main(void)
{
    _TRISE5 = 0;
    _LATE5    = 0;
    _TRISE4 = 0;
    _LATE4    = 0;
    _TRISE3 = 0;
    _LATE3    = 0;
    _TRISE2 = 0;
    _LATE2    = 0;

    while(1)
    {
          OpenI2CSlave(config1,config2);

          while(!I2CSTATbits.S||I2CSTATbits.P);    /* Wait till I2C bus active */
_LATE5 = 1;        
          while(I2CSTATbits.D_A||I2CSTATbits.R_W);    /* Wait till slave ready to receive data */
_LATE4    = 1;
          //while(!IFS0bits.SI2CIF);    /* Wait till interrupt flag set */

          IFS0bits.SI2CIF = 0;        /* Clear interrupt flag */

          I2CSTATbits.I2COV = 0;    /* Clear Receive Overflow Flag bit */

          while(!DataRdyI2C);    /* Wait till I2CRCV is full */
_LATE3    = 1;
          if(SlaveReadI2C == 0xAA)    _LATE2 = 1;    /* Read data */

          IFS0bits.SI2CIF = 0;        /* Clear interrupt flag */
    }
}

//------------------------------------------------------------------------------
// Cac chuong trinh con va chuong trinh phuc vu ngat neu co
//------------------------------------------------------------------------------

Và 2 file i2c.c , i2c.h trong file đính kèm