TWI code for HillCrest SHTP

Question

I'm developing TWI code for Bosch BNO080. They use SHTP protocol on top of TWI. A working version using Wire library of Arduino is posted here.

I ported the code, and replace read/write with nrf_drv_twi_rx/tx. Code still hasn't worked and I wasn't able to find the problem. When debugging, it seems that whatever is sent through sendPacket will be be the same as in receivePacket (i.e., data on the bus not changed after calling sendPacket and receivePacket).

//Check to see if there is any new data available
//Read the contents of the incoming packet into the shtpData array
bool receivePacket(void)
{
    NRF_LOG_INFO("Receive Packet\r\n");
    NRF_LOG_FLUSH();
    
    uint8_t readValue[HEADER_BYTES];
    ret_code_t err_code;
    
    err_code = nrf_drv_twi_rx(&m_twi, BNO080_DEFAULT_ADDRESS, readValue, sizeof(readValue));
    APP_ERROR_CHECK(err_code);

    //Get the first four bytes, aka the packet header
    uint8_t packetLSB = readValue[0];
    uint8_t packetMSB = readValue[1];
    uint8_t channelNumber = readValue[2];
    uint8_t sequenceNumber = readValue[3]; //Not sure if we need to store this or not

    //Store the header info.
    shtpHeader[0] = packetLSB;
    shtpHeader[1] = packetMSB;
    shtpHeader[2] = channelNumber;
    shtpHeader[3] = sequenceNumber;

    //Calculate the number of data bytes in this packet
    int16_t dataLength = ((uint16_t)packetMSB << 8 | packetLSB);
    dataLength &= ~(1 << 15); //Clear the MSbit.
    
    NRF_LOG_INFO("SHTP HEADER: %x %x %x %x \r\n", shtpHeader[0], shtpHeader[1], shtpHeader[2], shtpHeader[3]);
    NRF_LOG_INFO("data Length %d \r\n", dataLength);
    NRF_LOG_FLUSH();
    
    //This bit indicates if this package is a continuation of the last. Ignore it for now.
    //TODO catch this as an error and exit
    if (dataLength == 0) {
        //Packet is empty
        return (false); //All done
    }
    dataLength -= 4; //Remove the header bytes from the data count
    getData(dataLength);

    return (true); //We're done!
}

bool getData(uint16_t bytesRemaining)
{
    uint16_t dataSpot = 0; //Start at the beginning of shtpData array

    //Setup a series of chunked 32 byte reads
    while (bytesRemaining > 0) {        
        uint16_t numberOfBytesToRead = bytesRemaining;
        if (numberOfBytesToRead > (I2C_BUFFER_LENGTH-4)) numberOfBytesToRead = (I2C_BUFFER_LENGTH-4);
        
        ret_code_t err_code;       
        uint8_t* readValue =  (uint8_t*)malloc(numberOfBytesToRead + HEADER_BYTES);

        err_code = nrf_drv_twi_rx(&m_twi, BNO080_DEFAULT_ADDRESS, readValue, sizeof(readValue));
        
        if (err_code != NRF_SUCCESS)
            return false;
        
        APP_ERROR_CHECK(err_code);
        
//        NRF_LOG_INFO("Number of bytes to read: %d\r\n", numberOfBytesToRead);
//        NRF_LOG_FLUSH();
        
        for (uint8_t i = 0; i < numberOfBytesToRead ; i++)
        {
            if (dataSpot < MAX_PACKET_SIZE) {
                // discard first 4 bytes in readValue
                shtpData[dataSpot++] = readValue[i + HEADER_BYTES]; //Store data into the shtpData array
                NRF_LOG_RAW_INFO("%x ", readValue[i + HEADER_BYTES]);
            }
            else {
                //Do nothing with the data
            }
        }
        NRF_LOG_INFO("\r\n");
        
        bytesRemaining -= numberOfBytesToRead;
        free(readValue);
        NRF_LOG_FLUSH();
    }
    return (true);
}

bool sendPacket(uint8_t channelNumber, uint8_t dataLength)
{
    NRF_LOG_INFO("Send Packet\r\n");
    NRF_LOG_FLUSH();
    
    ret_code_t err_code;
    uint8_t packetLength = dataLength + HEADER_BYTES; //Add four bytes for the header
    
    uint8_t* txbuffer = malloc(packetLength);
    
    txbuffer[0] = packetLength & 0xFF;
    txbuffer[1] = packetLength >> 8;
    txbuffer[2] = channelNumber;
    txbuffer[3] = sequenceNumber[channelNumber]++;
    
    for (uint8_t i = 0; i < dataLength; i++) {
        txbuffer[i + HEADER_BYTES] = shtpData[i];
    }
    
    err_code = nrf_drv_twi_tx(&m_twi, BNO080_DEFAULT_ADDRESS, txbuffer, sizeof(txbuffer), false);
    free(txbuffer);
    
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_INFO("Send data error\r\n");
        NRF_LOG_INFO("%d", err_code);
        NRF_LOG_FLUSH();
        return false;
    } else {
        NRF_LOG_INFO("Send packet successfully\r\n");
        NRF_LOG_FLUSH();
    }

    APP_ERROR_CHECK(err_code);
    return err_code;
}