Home | History | Annotate | Download | only in smp
      1 /******************************************************************************
      2  *
      3  *  Copyright (C) 2008-2012 Broadcom Corporation
      4  *
      5  *  Licensed under the Apache License, Version 2.0 (the "License");
      6  *  you may not use this file except in compliance with the License.
      7  *  You may obtain a copy of the License at:
      8  *
      9  *  http://www.apache.org/licenses/LICENSE-2.0
     10  *
     11  *  Unless required by applicable law or agreed to in writing, software
     12  *  distributed under the License is distributed on an "AS IS" BASIS,
     13  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     14  *  See the License for the specific language governing permissions and
     15  *  limitations under the License.
     16  *
     17  ******************************************************************************/
     18 
     19 /******************************************************************************
     20  *
     21  *  This file contains the implementation of the AES128 CMAC algorithm.
     22  *
     23  ******************************************************************************/
     24 
     25 #include "bt_target.h"
     26 
     27 #if SMP_INCLUDED == TRUE
     28     #include <stdio.h>
     29     #include <string.h>
     30 
     31     #include "btm_ble_api.h"
     32     #include "smp_int.h"
     33     #include "hcimsgs.h"
     34 
     35 typedef struct
     36 {
     37     UINT8               *text;
     38     UINT16              len;
     39     UINT16              round;
     40 }tCMAC_CB;
     41 
     42 tCMAC_CB    cmac_cb;
     43 
     44 /* Rb for AES-128 as block cipher, LSB as [0] */
     45 BT_OCTET16 const_Rb = {
     46     0x87, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
     47     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
     48 };
     49 
     50 void print128(BT_OCTET16 x, const UINT8 *key_name)
     51 {
     52 #if SMP_DEBUG == TRUE
     53     UINT8  *p = (UINT8 *)x;
     54     UINT8  i;
     55 
     56     SMP_TRACE_WARNING("%s(MSB ~ LSB) = ", key_name);
     57 
     58     for (i = 0; i < 4; i ++)
     59     {
     60         SMP_TRACE_WARNING("%02x %02x %02x %02x",
     61                            p[BT_OCTET16_LEN - i*4 -1], p[BT_OCTET16_LEN - i*4 -2],
     62                            p[BT_OCTET16_LEN - i*4 -3], p[BT_OCTET16_LEN - i*4 -4]);
     63     }
     64 #endif
     65 }
     66 
     67 /*******************************************************************************
     68 **
     69 ** Function         padding
     70 **
     71 ** Description      utility function to padding the given text to be a 128 bits
     72 **                  data. The parameter dest is input and output parameter, it
     73 **                  must point to a BT_OCTET16_LEN memory space; where include
     74 **                  length bytes valid data.
     75 **
     76 ** Returns          void
     77 **
     78 *******************************************************************************/
     79 static void padding ( BT_OCTET16 dest, UINT8 length )
     80 {
     81     UINT8   i, *p = dest;
     82     /* original last block */
     83     for ( i = length ; i < BT_OCTET16_LEN; i++ )
     84         p[BT_OCTET16_LEN - i - 1] = ( i == length ) ? 0x80 : 0;
     85 }
     86 /*******************************************************************************
     87 **
     88 ** Function         leftshift_onebit
     89 **
     90 ** Description      utility function to left shift one bit for a 128 bits value.
     91 **
     92 ** Returns          void
     93 **
     94 *******************************************************************************/
     95 static void leftshift_onebit(UINT8 *input, UINT8 *output)
     96 {
     97     UINT8   i, overflow = 0 , next_overflow = 0;
     98     SMP_TRACE_EVENT ("leftshift_onebit ");
     99     /* input[0] is LSB */
    100     for ( i = 0; i < BT_OCTET16_LEN ; i ++ )
    101     {
    102         next_overflow = (input[i] & 0x80) ? 1:0;
    103         output[i] = (input[i] << 1) | overflow;
    104         overflow = next_overflow;
    105     }
    106     return;
    107 }
    108 /*******************************************************************************
    109 **
    110 ** Function         cmac_aes_cleanup
    111 **
    112 ** Description      clean up function for AES_CMAC algorithm.
    113 **
    114 ** Returns          void
    115 **
    116 *******************************************************************************/
    117 static void cmac_aes_cleanup(void)
    118 {
    119     if (cmac_cb.text != NULL)
    120     {
    121         GKI_freebuf(cmac_cb.text);
    122     }
    123     memset(&cmac_cb, 0, sizeof(tCMAC_CB));
    124 }
    125 
    126 /*******************************************************************************
    127 **
    128 ** Function         cmac_aes_k_calculate
    129 **
    130 ** Description      This function is the calculation of block cipher using AES-128.
    131 **
    132 ** Returns          void
    133 **
    134 *******************************************************************************/
    135 static BOOLEAN cmac_aes_k_calculate(BT_OCTET16 key, UINT8 *p_signature, UINT16 tlen)
    136 {
    137     tSMP_ENC output;
    138     UINT8    i = 1, err = 0;
    139     UINT8    x[16] = {0};
    140     UINT8   *p_mac;
    141 
    142     SMP_TRACE_EVENT ("cmac_aes_k_calculate ");
    143 
    144     while (i <= cmac_cb.round)
    145     {
    146         smp_xor_128(&cmac_cb.text[(cmac_cb.round - i)*BT_OCTET16_LEN], x); /* Mi' := Mi (+) X  */
    147 
    148         if (!SMP_Encrypt(key, BT_OCTET16_LEN, &cmac_cb.text[(cmac_cb.round - i)*BT_OCTET16_LEN], BT_OCTET16_LEN, &output))
    149         {
    150             err = 1;
    151             break;
    152         }
    153 
    154         memcpy(x, output.param_buf, BT_OCTET16_LEN);
    155         i ++;
    156     }
    157 
    158     if (!err)
    159     {
    160         p_mac = output.param_buf + (BT_OCTET16_LEN - tlen);
    161         memcpy(p_signature, p_mac, tlen);
    162 
    163         SMP_TRACE_DEBUG("tlen = %d p_mac = %d", tlen, p_mac);
    164         SMP_TRACE_DEBUG("p_mac[0] = 0x%02x p_mac[1] = 0x%02x p_mac[2] = 0x%02x p_mac[3] = 0x%02x",
    165                          *p_mac, *(p_mac + 1), *(p_mac + 2), *(p_mac + 3));
    166         SMP_TRACE_DEBUG("p_mac[4] = 0x%02x p_mac[5] = 0x%02x p_mac[6] = 0x%02x p_mac[7] = 0x%02x",
    167                          *(p_mac + 4), *(p_mac + 5), *(p_mac + 6), *(p_mac + 7));
    168 
    169         return TRUE;
    170 
    171     }
    172     else
    173         return FALSE;
    174 }
    175 /*******************************************************************************
    176 **
    177 ** Function         cmac_prepare_last_block
    178 **
    179 ** Description      This function proceeed to prepare the last block of message
    180 **                  Mn depending on the size of the message.
    181 **
    182 ** Returns          void
    183 **
    184 *******************************************************************************/
    185 static void cmac_prepare_last_block (BT_OCTET16 k1, BT_OCTET16 k2)
    186 {
    187 //    UINT8       x[16] = {0};
    188     BOOLEAN      flag;
    189 
    190     SMP_TRACE_EVENT ("cmac_prepare_last_block ");
    191     /* last block is a complete block set flag to 1 */
    192     flag = ((cmac_cb.len % BT_OCTET16_LEN) == 0 && cmac_cb.len != 0)  ? TRUE : FALSE;
    193 
    194     SMP_TRACE_WARNING("flag = %d round = %d", flag, cmac_cb.round);
    195 
    196     if ( flag )
    197     { /* last block is complete block */
    198         smp_xor_128(&cmac_cb.text[0], k1);
    199     }
    200     else /* padding then xor with k2 */
    201     {
    202         padding(&cmac_cb.text[0], (UINT8)(cmac_cb.len % 16));
    203 
    204         smp_xor_128(&cmac_cb.text[0], k2);
    205     }
    206 }
    207 /*******************************************************************************
    208 **
    209 ** Function         cmac_subkey_cont
    210 **
    211 ** Description      This is the callback function when CIPHk(0[128]) is completed.
    212 **
    213 ** Returns          void
    214 **
    215 *******************************************************************************/
    216 static void cmac_subkey_cont(tSMP_ENC *p)
    217 {
    218     UINT8 k1[BT_OCTET16_LEN], k2[BT_OCTET16_LEN];
    219     UINT8 *pp = p->param_buf;
    220     SMP_TRACE_EVENT ("cmac_subkey_cont ");
    221     print128(pp, (const UINT8 *)"K1 before shift");
    222 
    223     /* If MSB(L) = 0, then K1 = L << 1 */
    224     if ( (pp[BT_OCTET16_LEN - 1] & 0x80) != 0 )
    225     {
    226         /* Else K1 = ( L << 1 ) (+) Rb */
    227         leftshift_onebit(pp, k1);
    228         smp_xor_128(k1, const_Rb);
    229     }
    230     else
    231     {
    232         leftshift_onebit(pp, k1);
    233     }
    234 
    235     if ( (k1[BT_OCTET16_LEN - 1] & 0x80) != 0 )
    236     {
    237         /* K2 =  (K1 << 1) (+) Rb */
    238         leftshift_onebit(k1, k2);
    239         smp_xor_128(k2, const_Rb);
    240     }
    241     else
    242     {
    243         /* If MSB(K1) = 0, then K2 = K1 << 1 */
    244         leftshift_onebit(k1, k2);
    245     }
    246 
    247     print128(k1, (const UINT8 *)"K1");
    248     print128(k2, (const UINT8 *)"K2");
    249 
    250     cmac_prepare_last_block (k1, k2);
    251 }
    252 /*******************************************************************************
    253 **
    254 ** Function         cmac_generate_subkey
    255 **
    256 ** Description      This is the function to generate the two subkeys.
    257 **
    258 ** Parameters       key - CMAC key, expect SRK when used by SMP.
    259 **
    260 ** Returns          void
    261 **
    262 *******************************************************************************/
    263 static BOOLEAN cmac_generate_subkey(BT_OCTET16 key)
    264 {
    265     BT_OCTET16 z = {0};
    266     BOOLEAN     ret = TRUE;
    267     tSMP_ENC output;
    268     SMP_TRACE_EVENT (" cmac_generate_subkey");
    269 
    270     if (SMP_Encrypt(key, BT_OCTET16_LEN, z, BT_OCTET16_LEN, &output))
    271     {
    272         cmac_subkey_cont(&output);;
    273     }
    274     else
    275         ret = FALSE;
    276 
    277     return ret;
    278 }
    279 /*******************************************************************************
    280 **
    281 ** Function         AES_CMAC
    282 **
    283 ** Description      This is the AES-CMAC Generation Function with tlen implemented.
    284 **
    285 ** Parameters       key - CMAC key in little endian order, expect SRK when used by SMP.
    286 **                  input - text to be signed in little endian byte order.
    287 **                  length - length of the input in byte.
    288 **                  tlen - lenth of mac desired
    289 **                  p_signature - data pointer to where signed data to be stored, tlen long.
    290 **
    291 ** Returns          void
    292 **
    293 *******************************************************************************/
    294 BOOLEAN AES_CMAC ( BT_OCTET16 key, UINT8 *input, UINT16 length,
    295                 UINT16 tlen, UINT8 *p_signature)
    296 {
    297     UINT16  len, diff;
    298     UINT16  n = (length + BT_OCTET16_LEN - 1) / BT_OCTET16_LEN;       /* n is number of rounds */
    299     BOOLEAN ret = FALSE;
    300 
    301     SMP_TRACE_EVENT ("AES_CMAC  ");
    302 
    303     if (n == 0)  n = 1;
    304     len = n * BT_OCTET16_LEN;
    305 
    306     SMP_TRACE_WARNING("AES128_CMAC started, allocate buffer size = %d", len);
    307     /* allocate a memory space of multiple of 16 bytes to hold text  */
    308     if ((cmac_cb.text = (UINT8 *)GKI_getbuf(len)) != NULL)
    309     {
    310         cmac_cb.round = n;
    311 
    312         memset(cmac_cb.text, 0, len);
    313         diff = len - length;
    314 
    315         if (input != NULL && length > 0)
    316         {
    317             memcpy(&cmac_cb.text[diff] , input, (int)length);
    318             cmac_cb.len = length;
    319         }
    320         else
    321             cmac_cb.len = 0;
    322 
    323         /* prepare calculation for subkey s and last block of data */
    324         if (cmac_generate_subkey(key))
    325         {
    326             /* start calculation */
    327             ret = cmac_aes_k_calculate(key, p_signature, tlen);
    328         }
    329         /* clean up */
    330         cmac_aes_cleanup();
    331     }
    332     else
    333     {
    334         ret = FALSE;
    335         SMP_TRACE_ERROR("No resources");
    336     }
    337 
    338     return ret;
    339 }
    340 
    341     #if 0 /* testing code, sample data from spec */
    342 void test_cmac_cback(UINT8 *p_mac, UINT16 tlen)
    343 {
    344     SMP_TRACE_EVENT ("test_cmac_cback ");
    345     SMP_TRACE_ERROR("test_cmac_cback");
    346 }
    347 
    348 void test_cmac(void)
    349 {
    350     SMP_TRACE_EVENT ("test_cmac ");
    351     UINT8 M[64] = {
    352         0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96,
    353         0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a,
    354         0xae, 0x2d, 0x8a, 0x57, 0x1e, 0x03, 0xac, 0x9c,
    355         0x9e, 0xb7, 0x6f, 0xac, 0x45, 0xaf, 0x8e, 0x51,
    356         0x30, 0xc8, 0x1c, 0x46, 0xa3, 0x5c, 0xe4, 0x11,
    357         0xe5, 0xfb, 0xc1, 0x19, 0x1a, 0x0a, 0x52, 0xef,
    358         0xf6, 0x9f, 0x24, 0x45, 0xdf, 0x4f, 0x9b, 0x17,
    359         0xad, 0x2b, 0x41, 0x7b, 0xe6, 0x6c, 0x37, 0x10
    360     };
    361 
    362     UINT8 key[16] = {
    363         0x3c, 0x4f, 0xcf, 0x09, 0x88, 0x15, 0xf7, 0xab,
    364         0xa6, 0xd2, 0xae, 0x28, 0x16, 0x15, 0x7e, 0x2b
    365     };
    366     UINT8 i =0, tmp;
    367     UINT16 len;
    368 
    369     len = 64;
    370 
    371     for (i = 0; i < len/2; i ++)
    372     {
    373         tmp = M[i];
    374         M[i] = M[len -1 - i];
    375         M[len -1 - i] = tmp;
    376     }
    377 
    378 
    379     memset(&cmac_cb, 0, sizeof(tCMAC_CB));
    380 
    381     SMP_TRACE_WARNING("\n Example 1: len = %d\n", len);
    382 
    383     AES_CMAC(key, M, len, 128, test_cmac_cback, 0);
    384 
    385 }
    386     #endif
    387 #endif
    388 
    389