MSP430F5249: Problem with reading over I2C

Part Number:MSP430F5249

Hi,

I've got a really strange problem when trying to read over I2C. The first write and read work fine. However, the second write fails.

I am writing the two byte register address one byte at a time, then changing to read mode to do the reading. My code is shown below.

static void StoreRegisterAddress(const I2CAddressSize register_size, const uint16_t register_address)
{
    uint8_t index = 0;
    regAddressSize = register_size;

    // MSB of register address first
    switch (register_size)
    {
        case I2CAddressSizeTwoBytes:
            regAddress[index++] = MSB16(register_address);
            // no break - intentional
        case I2CAddressSizeOneByte:
            regAddress[index++] = LSB16(register_address);
            break;
        default:
            // Nothing is written
            break;
   }
}

void WriteToI2CSlave(const uint8_t slave_address, const I2CAddressSize register_size, const uint16_t register_address, const uint8_t *data, const uint8_t size)
{
    txMasterSize = (size <= MaxI2cBufferLength) ? size : MaxI2cBufferLength;
    txMasterCount = 0;
    memcpy(txMasterData, data, txMasterSize);
    UCB0I2CSA = slave_address;    // Set slave address
    while (UCB0CTL1 & UCTXSTP);   // Ensure stop condition got sent
    StoreRegisterAddress(register_size, register_address);
    UCB0CTL1 |= UCTR + UCTXSTT;   // I2C TX, start condition
    __no_operation();
}

void ReadFromI2CSlave(const uint8_t slave_address, const I2CAddressSize register_size, const uint16_t register_address, const uint8_t size)
{
    rxMasterSize = (size <= MaxI2cBufferLength) ? size : MaxI2cBufferLength;
    rxMasterCount = rxMasterSize;
    pRxMasterData = rxMasterData;
    UCB0I2CSA = slave_address;    // Set slave address
    while (UCB0CTL1 & UCTXSTP);   // Ensure stop condition got sent
    StoreRegisterAddress(register_size, register_address);
    UCB0CTL1 |= UCTR + UCTXSTT;   // Enter writing mode and send start
    __no_operation();
}

// USCI_B0 Data ISR - this is the interrupt service routine for PMU <--> slave devices
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
    #pragma vector = USCI_B0_VECTOR
        __interrupt void USCI_B0_ISR(void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(USCI_B0_VECTOR))) USCI_B0_ISR (void)
#else
#error Compiler not supported!
#endif
{
    switch(__even_in_range(UCB0IV,12))
    {
        case  0: break;                                          // Vector  0: No interrupts
        case  2: break;                                          // Vector  2: ALIFG
        case  4:                                                 // Vector  4: NACKIFG
        {
            UCB0CTL1 |= UCTXSTP;                                 // When an acknowledge is missing the slave gets a stop condition
            UCB0IFG &= ~UCTXIFG;
            break;
        }
        case  6: break;                                          // Vector  6: STTIFG
        case  8: break;                                          // Vector  8: STPIFG
        case 10:                                                 // Vector 10: RXIFG
        {
            if (rxMasterSize == 1)
            {
                if (rxMasterCount > 0)
                {
                    UCB0CTL1 |= UCTXSTP;                         // Generate I2C stop condition
                    *pRxMasterData = UCB0RXBUF;                  // Move final RX data to rxMasterData
                    --rxMasterCount;
                }
                else
                {
                    UCB0IFG &= ~UCRXIFG;                          // Clear USCI_B0 RX int flag
                    readMasterData = TRUE;
                    __bic_SR_register_on_exit(LPM0_bits + GIE);   // Exit low power mode
                }
            }
            else
            {
                --rxMasterCount;

                if (rxMasterCount)
                {
                    *pRxMasterData++ = UCB0RXBUF;                 // Move RX data to address rxMasterData
                    if (rxMasterCount == 1)                       // Only one byte left?
                    {
                        UCB0CTL1 |= UCTXSTP;                      // Generate I2C stop condition
                    }
                }
                else
                {
                    *pRxMasterData = UCB0RXBUF;                   // Move final RX data to PRxData
                    UCB0IFG &= ~UCRXIFG;                          // Clear USCI_B0 RX int flag
                    readMasterData = TRUE;
                    __bic_SR_register_on_exit(LPM0_bits + GIE);   // Exit low power mode
                }
            }

            break;
        }
        case 12:                                                   // Vector 12: TXIFG
        {
            if (regAddressSize > 0)
            {
                // Send register address
                UCB0TXBUF = regAddress[I2cMaxRegisterSize - regAddressSize];
                --regAddressSize;
            }
            else
            {
                if (rxMasterCount > 0)
                {
                    UCB0IFG &= ~UCTXIFG;
                    UCB0CTL1 &= ~UCTR;                              // Enter reading mode
                    UCB0CTL1 |= UCTXSTT;                            // I2C start condition
                }
                else
                {
                    if (txMasterCount < txMasterSize)               // Check TX byte counter
                    {
                        UCB0TXBUF = txMasterData[txMasterCount++];  // Load TX buffer and increment TX byte counter
                    }
                    else
                    {
                        UCB0CTL1 |= UCTXSTP;                        // I2C stop condition
                        UCB0IFG &= ~UCTXIFG;                        // Clear USCI_B0 TX int flag
                        sentMasterData = TRUE;
                        __bic_SR_register_on_exit(LPM0_bits + GIE); // Exit low power mode
                    }
                }
            }
            break;
        }
        default: break;
    }
}

The program always seems to fail after the following lines in the TXIFG case block:

if (rxMasterCount > 0)
{
    UCB0IFG &= ~UCTXIFG;
    UCB0CTL1 &= ~UCTR;                              // Enter reading mode
    UCB0CTL1 |= UCTXSTT;                            // I2C start condition
...

I don't understand how this could first time but not the second!

Can anyone tell me what is wrong?