/space/home/slher/rel/Beken/v6_4_5/macsw/ip/umac/src/mfp/mfp_bip.c

Go to the documentation of this file.

#include "rwnx_config.h"

#if NX_MFP

#include "dbg_assert.h"
#include "mac_frame.h"
#include "co_utils.h"
#include "mfp_bip.h"
#if NX_HSU
#include "hsu.h"
#endif

#if NX_HSU < 2

static const uint8_t sbox_t[256] =   {
    0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
    0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
    0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
    0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
    0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
    0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
    0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
    0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
    0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
    0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
    0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
    0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
    0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
    0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
    0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
    0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
};

#define S(a, s) sbox_t[(a >> s) & 0xff]

static const uint8_t x_time2_sbox_t[256] = {
    0xc6, 0xf8, 0xee, 0xf6, 0xff, 0xd6, 0xde, 0x91, 0x60, 0x02, 0xce, 0x56, 0xe7, 0xb5, 0x4d, 0xec,
    0x8f, 0x1f, 0x89, 0xfa, 0xef, 0xb2, 0x8e, 0xfb, 0x41, 0xb3, 0x5f, 0x45, 0x23, 0x53, 0xe4, 0x9b,
    0x75, 0xe1, 0x3d, 0x4c, 0x6c, 0x7e, 0xf5, 0x83, 0x68, 0x51, 0xd1, 0xf9, 0xe2, 0xab, 0x62, 0x2a,
    0x08, 0x95, 0x46, 0x9d, 0x30, 0x37, 0x0a, 0x2f, 0x0e, 0x24, 0x1b, 0xdf, 0xcd, 0x4e, 0x7f, 0xea,
    0x12, 0x1d, 0x58, 0x34, 0x36, 0xdc, 0xb4, 0x5b, 0xa4, 0x76, 0xb7, 0x7d, 0x52, 0xdd, 0x5e, 0x13,
    0xa6, 0xb9, 0x00, 0xc1, 0x40, 0xe3, 0x79, 0xb6, 0xd4, 0x8d, 0x67, 0x72, 0x94, 0x98, 0xb0, 0x85,
    0xbb, 0xc5, 0x4f, 0xed, 0x86, 0x9a, 0x66, 0x11, 0x8a, 0xe9, 0x04, 0xfe, 0xa0, 0x78, 0x25, 0x4b,
    0xa2, 0x5d, 0x80, 0x05, 0x3f, 0x21, 0x70, 0xf1, 0x63, 0x77, 0xaf, 0x42, 0x20, 0xe5, 0xfd, 0xbf,
    0x81, 0x18, 0x26, 0xc3, 0xbe, 0x35, 0x88, 0x2e, 0x93, 0x55, 0xfc, 0x7a, 0xc8, 0xba, 0x32, 0xe6,
    0xc0, 0x19, 0x9e, 0xa3, 0x44, 0x54, 0x3b, 0x0b, 0x8c, 0xc7, 0x6b, 0x28, 0xa7, 0xbc, 0x16, 0xad,
    0xdb, 0x64, 0x74, 0x14, 0x92, 0x0c, 0x48, 0xb8, 0x9f, 0xbd, 0x43, 0xc4, 0x39, 0x31, 0xd3, 0xf2,
    0xd5, 0x8b, 0x6e, 0xda, 0x01, 0xb1, 0x9c, 0x49, 0xd8, 0xac, 0xf3, 0xcf, 0xca, 0xf4, 0x47, 0x10,
    0x6f, 0xf0, 0x4a, 0x5c, 0x38, 0x57, 0x73, 0x97, 0xcb, 0xa1, 0xe8, 0x3e, 0x96, 0x61, 0x0d, 0x0f,
    0xe0, 0x7c, 0x71, 0xcc, 0x90, 0x06, 0xf7, 0x1c, 0xc2, 0x6a, 0xae, 0x69, 0x17, 0x99, 0x3a, 0x27,
    0xd9, 0xeb, 0x2b, 0x22, 0xd2, 0xa9, 0x07, 0x33, 0x2d, 0x3c, 0x15, 0xc9, 0x87, 0xaa, 0x50, 0xa5,
    0x03, 0x59, 0x09, 0x1a, 0x65, 0xd7, 0x84, 0xd0, 0x82, 0x29, 0x5a, 0x1e, 0x7b, 0xa8, 0x6d, 0x2c
};
#define SX2(a, s) x_time2_sbox_t[(a >> s) & 0xff]

static const uint8_t x_time3_sbox_t[256] = {
    0xa5, 0x84, 0x99, 0x8d, 0x0d, 0xbd, 0xb1, 0x54, 0x50, 0x03, 0xa9, 0x7d, 0x19, 0x62, 0xe6, 0x9a,
    0x45, 0x9d, 0x40, 0x87, 0x15, 0xeb, 0xc9, 0x0b, 0xec, 0x67, 0xfd, 0xea, 0xbf, 0xf7, 0x96, 0x5b,
    0xc2, 0x1c, 0xae, 0x6a, 0x5a, 0x41, 0x02, 0x4f, 0x5c, 0xf4, 0x34, 0x08, 0x93, 0x73, 0x53, 0x3f,
    0x0c, 0x52, 0x65, 0x5e, 0x28, 0xa1, 0x0f, 0xb5, 0x09, 0x36, 0x9b, 0x3d, 0x26, 0x69, 0xcd, 0x9f,
    0x1b, 0x9e, 0x74, 0x2e, 0x2d, 0xb2, 0xee, 0xfb, 0xf6, 0x4d, 0x61, 0xce, 0x7b, 0x3e, 0x71, 0x97,
    0xf5, 0x68, 0x00, 0x2c, 0x60, 0x1f, 0xc8, 0xed, 0xbe, 0x46, 0xd9, 0x4b, 0xde, 0xd4, 0xe8, 0x4a,
    0x6b, 0x2a, 0xe5, 0x16, 0xc5, 0xd7, 0x55, 0x94, 0xcf, 0x10, 0x06, 0x81, 0xf0, 0x44, 0xba, 0xe3,
    0xf3, 0xfe, 0xc0, 0x8a, 0xad, 0xbc, 0x48, 0x04, 0xdf, 0xc1, 0x75, 0x63, 0x30, 0x1a, 0x0e, 0x6d,
    0x4c, 0x14, 0x35, 0x2f, 0xe1, 0xa2, 0xcc, 0x39, 0x57, 0xf2, 0x82, 0x47, 0xac, 0xe7, 0x2b, 0x95,
    0xa0, 0x98, 0xd1, 0x7f, 0x66, 0x7e, 0xab, 0x83, 0xca, 0x29, 0xd3, 0x3c, 0x79, 0xe2, 0x1d, 0x76,
    0x3b, 0x56, 0x4e, 0x1e, 0xdb, 0x0a, 0x6c, 0xe4, 0x5d, 0x6e, 0xef, 0xa6, 0xa8, 0xa4, 0x37, 0x8b,
    0x32, 0x43, 0x59, 0xb7, 0x8c, 0x64, 0xd2, 0xe0, 0xb4, 0xfa, 0x07, 0x25, 0xaf, 0x8e, 0xe9, 0x18,
    0xd5, 0x88, 0x6f, 0x72, 0x24, 0xf1, 0xc7, 0x51, 0x23, 0x7c, 0x9c, 0x21, 0xdd, 0xdc, 0x86, 0x85,
    0x90, 0x42, 0xc4, 0xaa, 0xd8, 0x05, 0x01, 0x12, 0xa3, 0x5f, 0xf9, 0xd0, 0x91, 0x58, 0x27, 0xb9,
    0x38, 0x13, 0xb3, 0x33, 0xbb, 0x70, 0x89, 0xa7, 0xb6, 0x22, 0x92, 0x20, 0x49, 0xff, 0x78, 0x7a,
    0x8f, 0xf8, 0x80, 0x17, 0xda, 0x31, 0xc6, 0xb8, 0xc3, 0xb0, 0x77, 0x11, 0xcb, 0xfc, 0xd6, 0x3a
};
#define SX3(a, s) x_time3_sbox_t[(a >> s) & 0xff]

const uint32_t rcon_t[] = {
    0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36,
};

__INLINE uint32_t sched_core(uint32_t val, int r)
{
    return (S(val, 0) <<  24)      |
           (S(val, 8) ^ rcon_t[r]) |
           (S(val, 16) << 8)       |
           (S(val, 24) << 16);
}

static void aes_expand_key(uint32_t *key, uint32_t *rk)
{
    int r;

    /* first round is the key itself */
    rk[0] = key[0];
    rk[1] = key[1];
    rk[2] = key[2];
    rk[3] = key[3];

    /* Process remaining rounds */
    for (r = 0; r < 10; r++)
    {
        rk[4] = rk[0] ^ sched_core(rk[3], r);
        rk[5] = rk[1] ^ rk[4];
        rk[6] = rk[2] ^ rk[5];
        rk[7] = rk[3] ^ rk[6];
        rk += 4;
    }
}

__INLINE void add_round_key(uint32_t *rk, uint32_t *data, int r)
{
    data[0] ^= rk[r*4 + 0];
    data[1] ^= rk[r*4 + 1];
    data[2] ^= rk[r*4 + 2];
    data[3] ^= rk[r*4 + 3];
}


__INLINE void shift_sub(uint32_t *data)
{
    uint32_t temp[3];

    temp[0] = data[0];
    data[0] = S(data[0], 0) | S(data[1], 8) << 8 | S(data[2], 16) << 16 | S(data[3], 24) << 24;
    temp[1] = data[1];
    data[1] = S(data[1], 0) | S(data[2], 8) << 8 | S(data[3], 16) << 16 | S(temp[0], 24) << 24;
    temp[2] = data[2];
    data[2] = S(data[2], 0) | S(data[3], 8) << 8 | S(temp[0], 16) << 16 | S(temp[1], 24) << 24;
    data[3] = S(data[3], 0) | S(temp[0], 8) << 8 | S(temp[1], 16) << 16 | S(temp[2], 24) << 24;
}

__INLINE void mix_shift_sub(uint32_t *data)
{
    uint32_t temp[4];

    temp[0] = data[0];
    data[0]  = (SX2(temp[0], 0) ^ SX3(data[1], 8) ^ S(data[2], 16) ^ S(data[3], 24));
    data[0] |= (S(temp[0], 0) ^ SX2(data[1], 8) ^ SX3(data[2], 16) ^ S(data[3], 24)) << 8;
    data[0] |= (S(temp[0], 0) ^ S(data[1], 8) ^ SX2(data[2], 16) ^ SX3(data[3], 24)) << 16;
    data[0] |= (SX3(temp[0], 0) ^ S(data[1], 8) ^ S(data[2], 16) ^ SX2(data[3], 24)) << 24;

    temp[1] = data[1];
    data[1]  = (SX2(temp[1], 0) ^ SX3(data[2], 8) ^ S(data[3], 16) ^ S(temp[0], 24));
    data[1] |= (S(temp[1], 0) ^ SX2(data[2], 8) ^ SX3(data[3], 16) ^ S(temp[0], 24)) << 8;
    data[1] |= (S(temp[1], 0) ^ S(data[2], 8) ^ SX2(data[3], 16) ^ SX3(temp[0], 24)) << 16;
    data[1] |= (SX3(temp[1], 0) ^ S(data[2], 8) ^ S(data[3], 16) ^ SX2(temp[0], 24)) << 24;

    temp[2] = data[2];
    data[2]  = (SX2(temp[2], 0) ^ SX3(data[3], 8) ^ S(temp[0], 16) ^ S(temp[1], 24));
    data[2] |= (S(temp[2], 0) ^ SX2(data[3], 8) ^ SX3(temp[0], 16) ^ S(temp[1], 24)) << 8;
    data[2] |= (S(temp[2], 0) ^ S(data[3], 8) ^ SX2(temp[0], 16) ^ SX3(temp[1], 24)) << 16;
    data[2] |= (SX3(temp[2], 0) ^ S(data[3], 8) ^ S(temp[0], 16) ^ SX2(temp[1], 24)) << 24;

    temp[3] = data[3];
    data[3]  = (SX2(temp[3], 0) ^ SX3(temp[0], 8) ^ S(temp[1], 16) ^ S(temp[2], 24));
    data[3] |= (S(temp[3], 0) ^ SX2(temp[0], 8) ^ SX3(temp[1], 16) ^ S(temp[2], 24)) << 8;
    data[3] |= (S(temp[3], 0) ^ S(temp[0], 8) ^ SX2(temp[1], 16) ^ SX3(temp[2], 24)) << 16;
    data[3] |= (SX3(temp[3], 0) ^ S(temp[0], 8) ^ S(temp[1], 16) ^ SX2(temp[2], 24)) << 24;

}

#undef S
#undef SX2
#undef SX3

static void aes_encrypt_block(uint32_t *rk, uint32_t *data)
{
    int r;

    /* initial round */
    add_round_key(rk, data, 0);

    for(r = 1; r < 10; r++)
    {
        mix_shift_sub(data);
        add_round_key(rk, data, r);
    }

    /* final round */
    shift_sub(data);
    add_round_key(rk, data, r);
}

static void aes_cmac_shift_sub_key(uint32_t *k)
{
    bool carry = (k[0] & 0x80);
    uint32_t t;

    t = ((k[0] & 0x80808000) >> 15) | ((k[1] & 0x80) << 17);
    k[0] = (k[0] << 1 & 0xfefefefe) | t;

    t = ((k[1] & 0x80808000) >> 15) | ((k[2] & 0x80) << 17);
    k[1] = (k[1] << 1 & 0xfefefefe) | t;

    t = ((k[2] & 0x80808000) >> 15) | ((k[3] & 0x80) << 17);
    k[2] = (k[2] << 1 & 0xfefefefe) | t;

    t = ((k[3] & 0x80808000) >> 15);
    k[3] = (k[3] << 1 & 0xfefefefe) | t;

    if (carry)
        k[3] ^= 0x87000000;
}

__INLINE void xor_block(uint32_t *a, uint32_t *b)
{
    int i;
    for (i = 0; i < 4; i++)
    {
        a[i] ^= b[i];
    }
}

static uint64_t aes_cmac(uint32_t *key, int nb_elem, uint32_t addr[], int len[])
{
    uint32_t round_key[11 * 4];
    uint32_t input[4], vect[4], subkey[4];
    uint32_t data_hw, input_hw ; // addresses in bytes
    int i, data_len, tot_len = 0;

    for (i = 0; i < nb_elem; i++)
    {
        tot_len += len[i];
    }

    /* init aes */
    aes_expand_key(key, round_key);
    memset(vect, 0, sizeof(vect));
    memset(subkey, 0, sizeof(subkey));

    data_hw = *addr;
    data_len = *len;
    while (tot_len > 16)
    {
        if (data_len > 16)
        {
            for (i = 0; i < 4 ; i++, data_hw += 4)
            {
                input[i] = co_read32p(data_hw);
            }
            data_len -= 16;
        }
        else
        {
            input_hw = CPU2HW(input);
            for (i = 0; i < 16 ; i++)
            {
                co_write8p(input_hw, co_read8p(data_hw));
                input_hw++;
                data_hw++;
                data_len--;

                if (data_len == 0)
                {
                    addr++;
                    len++;
                    data_hw = *addr;
                    data_len = *len;
                }
            }
        }

        xor_block(vect, input);
        aes_encrypt_block(round_key, vect);
        tot_len -= 16;
    }

    /* last block */
    /* compute sub key */
    aes_encrypt_block(round_key, subkey);
    aes_cmac_shift_sub_key(subkey);

    input_hw = CPU2HW(input);
    for (i = 0; i < tot_len ; i++)
    {
        co_write8p(input_hw, co_read8p(data_hw));
        input_hw++;
        data_hw++;
        data_len--;

        if (data_len == 0)
        {
            addr++;
            len++;
            data_hw = *addr;
            data_len = *len;
        }
    }
    if (tot_len < 16)
    {
        // pad input
        co_write8p(input_hw, 0x80);
        input_hw++;
        for (i = tot_len + 1; i < 16; i++, input_hw++)
            co_write8p(input_hw, 0);

        aes_cmac_shift_sub_key(subkey);
    }
    xor_block(subkey, input);

    xor_block(vect, subkey);
    aes_encrypt_block(round_key, vect);
    return ((uint64_t)vect[1] << 32 | (uint64_t)vect[0]);
}

#endif /* NX_HSU < 2 */

#if NX_HSU
uint16_t mfp_aad[10] __SHAREDRAM;
#endif

// Assume that mic is already zeroed in the frame
uint64_t mfp_compute_bip(struct key_info_tag *key, uint32_t frame_addr,
                         int frame_len, int machdr_len, uint32_t mmic_addr)
{
    #if !NX_HSU
    uint16_t mfp_aad[10];
    #endif
    uint16_t *data = HW2CPU(frame_addr);
    uint64_t mic;
    uint32_t input_addr[3];
    int input_len[3];
    int nb_input;

    if (frame_len < MAC_SHORT_MAC_HDR_LEN)
    {
        ASSERT_WARN(0);
        return 0;
    }

    /* prepare AAD vector: FC + A1 + A2 + A3 */
    mfp_aad[0] = data[0] & ~(0x3800); /* reset some bits in frame control */
    memcpy(&mfp_aad[1], &data[2], sizeof(mfp_aad) - sizeof(uint16_t));

    input_addr[0] = CPU2HW(mfp_aad);
    input_len[0] = sizeof_b(mfp_aad);
    input_addr[1] = frame_addr + machdr_len;
    input_len[1] = frame_len - machdr_len;
    if (!mmic_addr)
    {
        input_len[1] += MAC_MGMT_MIC_LEN;
        nb_input = 2;
    }
    else
    {
        input_addr[2] = mmic_addr;
        input_len[2] = MAC_MGMT_MIC_LEN;
        nb_input = 3;
    }

    #if NX_HSU
    mic = hsu_aes_cmac(key->u.mfp.key, nb_input, input_addr, input_len);
    #endif

    #if NX_HSU < 2
    #if NX_HSU == 1
    if (mic == 0)
    #endif /* NX_HSU == 1 */
    {
        mic = aes_cmac(key->u.mfp.key, nb_input, input_addr, input_len);
    }
    #endif /* NX_HSU < 2 */

    return mic;
}

#endif /* NX_MFP */

---