Problems implementing AES in CBC+CTS mode

Question

I'm trying to implement an AES cipher in CBC+CTS mode, but I'm unable to get the second last plaintext block to decrypt properly. I've attached my code, it's based on the steps listed under CBC ciphertext stealing decryption using a standard CBC interface on Wikipedia.

void AES::encrypt(const char* src, char* dest, size_t length)
{
    //Check if the length is less than 1 block
    if (length < 16) {
        //Pad the data to a complete block with zeroes
        char paddedBlock[16];
        memcpy(paddedBlock, src, length);
        memset(paddedBlock + length, 0, 16 - length);

        //Encrypt the padded block, and return early
        encryptBlock(paddedBlock, dest);
        return;
    }

    //Encypt all of the complete blocks
    while (length >= 16) {
        //Encrypt the next block
        encryptBlock(src, dest);
        src += 16;
        dest += 16;
        length -= 16;
    }

    //Check if a partial block remains
    if (length > 0) {
        //Perform CTS processing on the partial block
        if (m_CipherMode == MODE_ECB) {
            //Rewind the destination pointer to the second last block
            dest -= 16;

            //Copy the leading bytes of the second last block to the last block
            memcpy(dest + 16, dest, length);

            //Assemble the last block
            char lastBlock[16];
            memcpy(lastBlock, src, length);
            memcpy(lastBlock + length, dest + length, 16 - length);

            //Encrypt the last block
            encryptBlock(lastBlock, dest);
        } else if (m_CipherMode == MODE_CBC) {
            //Rewind the destination pointer to the second last block
            dest -= 16;

            //Copy the leading bytes of the second last block to the last block
            memcpy(dest + 16, dest, length);

            //Pad the partial last block to a complete block with zeroes
            char lastBlock[16];
            memcpy(lastBlock, src, length);
            memset(lastBlock + length, 0, 16 - length);

            //Encrypt the last block
            encryptBlock(lastBlock, dest);
        }
    }
}

void AES::decrypt(const char* src, char* dest, size_t length)
{
    //Check if the length is less than 1 block
    if (length < 16) {
        //Nothing we can really do here...
        memcpy(dest, src, length);
        return;
    }

    //Decrypt all of the complete blocks
    while (length >= 16) {
        //Decrypt the next block
        decryptBlock(src, dest);
        src += 16;
        dest += 16;
        length -= 16;
    }

    //Check if a partial block remains
    if (length > 0) {
        //Perform CTS processing on the partial block
        if (m_CipherMode == MODE_ECB) {
            //Rewind the destination pointer to the second last block
            dest -= 16;

            //Assemble the last block
            char lastBlock[16];
            memcpy(lastBlock, src, length);
            memcpy(lastBlock + length, dest + length, 16 - length);

            //Copy the leading bytes of the second last block to the last block
            memcpy(dest + 16, dest, length);

            //Decrypt the last block
            decryptBlock(lastBlock, dest);
        } else if (m_CipherMode == MODE_CBC) {
            //Rewind the destination pointer to the second last block
            dest -= 16;

            //Assemble the second last block
            char secondLastBlock[16];
            memcpy(secondLastBlock, src, length);
            memcpy(secondLastBlock + length, dest + length, 16 - length);

            //Assemble the last block
            char lastBlock[16];
            memcpy(lastBlock, src - 16, 16);

            //Decrypt the last two blocks
            decryptBlock(secondLastBlock, dest);
            decryptBlock(lastBlock);
            memcpy(dest + 16, lastBlock, length);
        }
    }
}

Note that encryptBlock() and decryptBlock() are performing the CBC chaining and unchaining.

Update

Here is some example output to demonstrate the problem:

//AES-128 encryption key
const char key[16] = {
    0x2B, 0x7E, 0x15, 0x16, 0x28, 0xAE, 0xD2, 0xA6,
    0xAB, 0xF7, 0x15, 0x88, 0x09, 0xCF, 0x4F, 0x3C
};

//C string "My awkward 55B test string to test ciphertext stealing"
const char inputPlaintext[55] = {
    //Block 1
    0x4D, 0x79, 0x20, 0x61, 0x77, 0x6B, 0x77, 0x61,
    0x72, 0x64, 0x20, 0x35, 0x35, 0x42, 0x20, 0x74,

    //Block 2
    0x65, 0x73, 0x74, 0x20, 0x73, 0x74, 0x72, 0x69,
    0x6E, 0x67, 0x20, 0x74, 0x6F, 0x20, 0x74, 0x65,

    //Block 3
    0x73, 0x74, 0x20, 0x63, 0x69, 0x70, 0x68, 0x65,
    0x72, 0x74, 0x65, 0x78, 0x74, 0x20, 0x73, 0x74,

    //Block 4
    0x65, 0x61, 0x6C, 0x69, 0x6E, 0x67, 0x00
};

//Encrypted ciphertext
const char encryptedCiphertext[55] = {
    //Block 1
    0x40, 0x6A, 0x5D, 0x3A, 0xF5, 0x98, 0x9D, 0xEA,
    0x6E, 0x70, 0x33, 0xA5, 0x6E, 0xBE, 0xAC, 0x38,

    //Block 2
    0xBD, 0x3F, 0xB0, 0xE0, 0xF5, 0xB7, 0x07, 0xE6,
    0xAA, 0xF5, 0x01, 0xC7, 0xEF, 0x92, 0x28, 0x50,

    //Block 3
    0xAD, 0x88, 0xEC, 0x34, 0x3F, 0x7E, 0x02, 0xF1,
    0x12, 0xD0, 0x86, 0x7E, 0x67, 0xCA, 0x09, 0x6D,

    //Block 4
    0xE6, 0x87, 0x54, 0x14, 0xF6, 0xC5, 0x75
};

//Decrypted plaintext
const char decryptedPlaintext[55] = {
    //Block 1 - correct
    0x4D, 0x79, 0x20, 0x61, 0x77, 0x6B, 0x77, 0x61,
    0x72, 0x64, 0x20, 0x35, 0x35, 0x42, 0x20, 0x74,

    //Block 2 - correct
    0x65, 0x73, 0x74, 0x20, 0x73, 0x74, 0x72, 0x69,
    0x6E, 0x67, 0x20, 0x74, 0x6F, 0x20, 0x74, 0x65,

    //Block 3 - garbage
    0x7C, 0x61, 0xE3, 0x6D, 0x00, 0xFB, 0x59, 0x8D,
    0xB9, 0x64, 0x2D, 0x38, 0x70, 0xAA, 0xB0, 0xD9,

    //Block 4 - correct
    0x65, 0x61, 0x6C, 0x69, 0x6E, 0x67, 0x00
};

Answer 1

Solved it by separating the CBC logic from the block encryption logic. Here is the revised code:

void AES::encrypt(const char* src, char* dest, size_t length)
{
    //TODO: Handle data lengths < 1 block!

    //Encrypt all of the data
    while (length > 0) {
        //Copy the next source block to the state array
        memcpy(m_State, src, 16);
        src += 16;
        length -= 16;

        //Perform CBC pre-processing if necessary
        if (m_CipherMode == MODE_CBC) {
            //XOR the state array with the carry vector
            for (int i = 0; i < 16; i++)
                m_State[i] = m_State[i] ^ m_CarryVector[i];
        }

        //Encrypt the state array
        aesEncrypt();

        //Perform CBC post-processing if necessary
        if (m_CipherMode == MODE_CBC) {
            //Save the state array as the next carry vector
            memcpy(m_CarryVector, m_State, 16);
        }

        //Perform ciphertext stealing if the next block is a partial block
        if (length > 0 && length < 16) {
            //Copy the last partial source block to a temporary buffer (in case of in-place encryption)
            char temp[length];
            memcpy(temp, src, length);

            //Copy the leading bytes of the state array to the last partial destination block
            memcpy(dest + 16, m_State, length);

            //Copy the temporary buffer to the state array
            memcpy(m_State, temp, length);

            //Perform CBC processing if necessary
            if (m_CipherMode == MODE_CBC) {
                //Pad the state array with zeroes
                memset(m_State + length, 0, 16 - length);

                //XOR the state array with the carry vector
                for (int i = 0; i < 16; i++)
                    m_State[i] = m_State[i] ^ m_CarryVector[i];
            }

            //Encrypt the state array
            aesEncrypt();
            length = 0;
        }

        //Copy the state array to the destination block
        memcpy(dest, m_State, 16);
        dest += 16;
    }
}

void AES::decrypt(const char* src, char* dest, size_t length)
{
    //TODO: Handle data lengths < 1 block!

    //Encrypt all of the data
    while (length > 0) {
        //Copy the next source block to the state array
        memcpy(m_State, src, 16);
        src += 16;
        length -= 16;

        //Decrypt the state array
        aesDecrypt();

        //Reverse ciphertext stealing if the next block is a partial block
        if (length > 0 && length < 16) {
            //Perform CBC processing if necessary
            if (m_CipherMode == MODE_CBC) {
                //XOR the state array with the last partial source block
                for (int i = 0; i < length; i++)
                    m_State[i] = m_State[i] ^ src[i];
            }

            //Copy the last partial source block to a temporary buffer (in case of in-place decryption)
            char temp[length];
            memcpy(temp, src, length);

            //Copy the leading bytes of the state array to the last partial destination block
            memcpy(dest + 16, m_State, length);

            //Copy the temporary buffer to the state array
            memcpy(m_State, temp, length);

            //Decrypt the state array
            aesDecrypt();
            length = 0;
        }

        //Perform CBC processing if necessary
        if (m_CipherMode == MODE_CBC) {
            //XOR the state array with the carry vector
            for (int i = 0; i < 16; i++)
                m_State[i] = m_State[i] ^ m_CarryVector[i];

            //Save the source block as the next carry vector
            memcpy(m_CarryVector, src - 16, 16);
        }

        //Copy the state array to the destination block
        memcpy(dest, m_State, 16);
        dest += 16;
    }
}