CC32xxAes.hxx

 
#ifndef _CC32xxAESCCM_HXX_
#define _CC32xxAESCCM_HXX_
 
#include <stdint.h>
 
#include "inc/hw_types.h"
#include "inc/hw_memmap.h"
#include "inc/hw_aes.h"
#include "inc/hw_dthe.h"
#include "driverlib/prcm.h"
#include "driverlib/rom.h"
#include "driverlib/rom_map.h"
#include "driverlib/aes.h"
 
#include "utils/SyncStream.hxx"
 
class CCMHelper
{
public:
    static void decrypt(const std::string &aes_key, const std::string &nonce,
        const std::string &auth_data, const std::string &cipher,
        std::string *plain, std::string *tag)
    {
        CCMHelper h;
        h.decrypt_init(aes_key, nonce, auth_data, cipher.size(), tag->size());
        unsigned ofs = cipher.size() / 3;
        h.data_process(cipher.substr(0, ofs), plain);
        std::string part;
        h.data_process(cipher.substr(ofs), &part);
        *plain += part;
        h.data_finalize(&part, tag);
        *plain += part;
    }
 
    static void encrypt(const std::string &aes_key, const std::string &nonce,
        const std::string &auth_data, const std::string &plain,
        unsigned tag_len, std::string *cipher, std::string *tag)
    {
        static_encrypt_init(aes_key, nonce, auth_data, plain.size(), tag_len);
        cipher->resize(plain.size() + 15);
        int len = plain.size();
        unsigned ofs = 0;
        while (len >= 16)
        {
            process_block(
                (const uint8_t *)&plain[ofs], (uint8_t *)&(*cipher)[ofs]);
            ofs += 16;
            len -= 16;
        }
        if (len > 0)
        {
            uint8_t buf[16];
            memset(buf, 0, sizeof(buf));
            memcpy(buf, &plain[ofs], len);
            process_block(buf, (uint8_t *)&(*cipher)[ofs]);
        }
        cipher->resize(plain.size());
        tag->resize(tag_len);
        read_tag(tag);
    }
 
    void decrypt_init(const std::string &aes_key, const std::string &nonce,
        const std::string &auth_data, unsigned data_len, uint8_t tag_len)
    {
        // will set the strings on each write.
        outputStream_ = new StringAppendStream(nullptr); 
        decryptorStream_.reset(create_decryptor_stream(aes_key, nonce, auth_data, data_len, tag_len, outputStream_, &tagOut_, nullptr));
    }
 
    static void static_decrypt_init(const std::string &aes_key,
        const std::string &nonce, const std::string &auth_data,
        unsigned data_len, uint8_t tag_len)
    {
        reset();
        // Sets configuration
        unsigned mode = AES_CFG_DIR_DECRYPT | AES_CFG_MODE_CCM |
            AES_CFG_CTR_WIDTH_32 |
            get_mode(aes_key.size(), nonce.size(), tag_len);
        MAP_AESConfigSet(AES_BASE, mode);
 
        // Computes the initialization vector form the nonce.
        string real_iv;
        real_iv.push_back(nonce_length_to_l(nonce.size()) - 1);
        real_iv += nonce;
        while (real_iv.size() < 16)
        {
            real_iv.push_back(0);
        }
        MAP_AESIVSet(AES_BASE, (uint8_t *)real_iv.data());
 
        // Sets key and length variables needed before starting the decryption.
        MAP_AESKey1Set(AES_BASE, (uint8_t *)aes_key.data(),
            interpret_key_size(aes_key.size()));
        MAP_AESDataLengthSet(AES_BASE, data_len);
        MAP_AESAuthDataLengthSet(AES_BASE, auth_data.size());
 
        // Writes the pre-authenticated data.
        for (unsigned ofs = 0; ofs < auth_data.size(); ofs += 16)
        {
            MAP_AESDataWrite(AES_BASE, (uint8_t *)&auth_data[ofs],
                std::min(16u, auth_data.size() - ofs));
        }
    }
 
    static void static_encrypt_init(const std::string &aes_key,
        const std::string &nonce, const std::string &auth_data,
        unsigned data_len, uint8_t tag_len)
    {
        reset();
        // Sets configuration
        unsigned mode = AES_CFG_DIR_ENCRYPT | AES_CFG_MODE_CCM |
            AES_CFG_CTR_WIDTH_32 |
            get_mode(aes_key.size(), nonce.size(), tag_len);
        MAP_AESConfigSet(AES_BASE, mode);
 
        // Computes the initialization vector form the nonce.
        string real_iv;
        real_iv.push_back(nonce_length_to_l(nonce.size()) - 1);
        real_iv += nonce;
        while (real_iv.size() < 16)
        {
            real_iv.push_back(0);
        }
        MAP_AESIVSet(AES_BASE, (uint8_t *)real_iv.data());
 
        // Sets key and length variables needed before starting the decryption.
        MAP_AESKey1Set(AES_BASE, (uint8_t *)aes_key.data(),
            interpret_key_size(aes_key.size()));
        MAP_AESDataLengthSet(AES_BASE, data_len);
        MAP_AESAuthDataLengthSet(AES_BASE, auth_data.size());
 
        // Writes the pre-authenticated data.
        for (unsigned ofs = 0; ofs < auth_data.size(); ofs += 16)
        {
            MAP_AESDataWrite(AES_BASE, (uint8_t *)&auth_data[ofs],
                std::min(16u, auth_data.size() - ofs));
        }
    }
 
    void data_process(const std::string &payload, std::string *payload_out)
    {
        payload_out->clear();
        payload_out->reserve(payload.size() + 16);
        outputStream_->set_output(payload_out);
        auto ret = decryptorStream_->write_all(payload.data(), payload.size());
        HASSERT(ret == (ssize_t)payload.size());
    }
 
    void data_finalize(std::string *payload_out, std::string *tag_out)
    {
        payload_out->clear();
        outputStream_->set_output(payload_out);
        auto ret = decryptorStream_->finalize(0);
        HASSERT(ret == 0);
        tag_out->swap(tagOut_);
    }
 
    static SyncStream *create_decryptor_stream(const std::string &aes_key,
        const std::string &nonce, const std::string &auth_data,
        unsigned data_len, uint8_t tag_len, SyncStream *consumer,
                                               std::string *tag_out, const std::string* expected_tag)
    {
        static_decrypt_init(aes_key, nonce, auth_data, data_len, tag_len);
        tag_out->resize(tag_len);
        return new MinWriteStream(
            16, new DecryptorStream(
                tag_out, expected_tag, new MaxLengthStream(data_len, consumer)));
    }
 
    class DecryptorStream : public WrappedStream
    {
    public:
        DecryptorStream(std::string *tag_out, const std::string *expected_tag,
            SyncStream *consumer)
            : WrappedStream(consumer)
            , tagOut_(tag_out)
            , expectedTag_(expected_tag)
        {
        }
 
        ssize_t write(const void *data, size_t len) override
        {
            if (len < 16)
                return -1;
            unsigned ll = 0;
            auto rp = (const uint8_t *)(data);
            auto wp = (uint8_t *)(data);
            while (len >= 16)
            {
                process_block(rp, wp);
                rp += 16;
                wp += 16;
                ll += 16;
                len -= 16;
            }
            delegate_->write_all(data, ll);
            return ll;
        }
 
        int finalize(int status) override
        {
            read_tag(tagOut_);
            if (expectedTag_ && (*expectedTag_ != *tagOut_)) {
                status = -2;
            }
            return WrappedStream::finalize(status);
        }
 
    private:
        std::string *tagOut_;
        const std::string *expectedTag_;
    };
 
private:
    static void reset()
    {
        MAP_PRCMPeripheralClkDisable(PRCM_DTHE, PRCM_RUN_MODE_CLK);
        usleep(10);
        MAP_PRCMPeripheralClkEnable(PRCM_DTHE, PRCM_RUN_MODE_CLK);
        usleep(10);
        while (!MAP_PRCMPeripheralStatusGet(PRCM_DTHE))
            ;
    }
 
    static uint32_t interpret_key_size(unsigned key_len)
    {
        switch (key_len)
        {
            case 16:
                return AES_CFG_KEY_SIZE_128BIT;
            case 24:
                return AES_CFG_KEY_SIZE_192BIT;
            case 32:
                return AES_CFG_KEY_SIZE_256BIT;
            default:
                DIE("Unknown key length");
        }
        return 0;
    }
 
    static int nonce_length_to_l(unsigned nonce_len)
    {
        switch (nonce_len)
        {
            case 15 - 2:
                return 2;
            case 15 - 4:
                return 4;
            case 15 - 8:
                return 8;
        }
        return -1;
    }
 
    static unsigned get_mode(
        unsigned key_len, unsigned nonce_len, unsigned tag_len)
    {
        unsigned ret = interpret_key_size(key_len);
 
        switch (nonce_len)
        {
            case 15 - 2:
                ret |= AES_CFG_CCM_L_2;
                break;
            case 15 - 4:
                ret |= AES_CFG_CCM_L_4;
                break;
            case 15 - 8:
                ret |= AES_CFG_CCM_L_8;
                break;
            default:
                DIE("unsupported nonce length");
        }
 
        switch (tag_len)
        {
            case 4:
                ret |= AES_CFG_CCM_M_4;
                break;
            case 6:
                ret |= AES_CFG_CCM_M_6;
                break;
            case 8:
                ret |= AES_CFG_CCM_M_8;
                break;
            case 10:
                ret |= AES_CFG_CCM_M_10;
                break;
            case 12:
                ret |= AES_CFG_CCM_M_12;
                break;
            case 14:
                ret |= AES_CFG_CCM_M_14;
                break;
            case 16:
                ret |= AES_CFG_CCM_M_16;
                break;
            default:
                DIE("Unsupported tag length");
        }
        return ret;
    }
 
    static void process_block(const uint8_t *din, uint8_t *dout)
    {
        MAP_AESDataWrite(AES_BASE, (uint8_t *)din, 16);
        MAP_AESDataRead(AES_BASE, dout, 16);
    }
 
    static void read_tag(std::string* tag_out) {
        uint8_t tag[16];
        //
        // Wait for the context data registers to be ready.
        //
        while ((AES_CTRL_SVCTXTRDY & (HWREG(AES_BASE + AES_O_CTRL))) == 0)
        {
        }
 
        MAP_AESTagRead(AES_BASE, tag);
        tag_out->assign((const char*)tag, tag_out->size());
    }
 
    std::unique_ptr<SyncStream> decryptorStream_;
    StringAppendStream* outputStream_;
    std::string tagOut_;
};
 
#endif // _CC32xxAESCCM_HXX_