smp_cmac.c revision 5fe6f0cf6b223e3ed6be4912d55b3ed5b41ce0cd 
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
35typedef struct
36{
37    UINT8               *text;
38    UINT16              len;
39    UINT16              round;
40}tCMAC_CB;
41
42tCMAC_CB    cmac_cb;
43
44/* Rb for AES-128 as block cipher, LSB as [0] */
45BT_OCTET16 const_Rb = {
46    0x87, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
47    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
48};
49
50void print128(BT_OCTET16 x, const UINT8 *key_name)
51{
52#if SMP_DEBUG == TRUE && SMP_DEBUG_VERBOSE == 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*******************************************************************************/
79static 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*******************************************************************************/
95static 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*******************************************************************************/
117static void cmac_aes_cleanup(void)
118{
119    osi_free(cmac_cb.text);
120    memset(&cmac_cb, 0, sizeof(tCMAC_CB));
121}
122
123/*******************************************************************************
124**
125** Function         cmac_aes_k_calculate
126**
127** Description      This function is the calculation of block cipher using AES-128.
128**
129** Returns          void
130**
131*******************************************************************************/
132static BOOLEAN cmac_aes_k_calculate(BT_OCTET16 key, UINT8 *p_signature, UINT16 tlen)
133{
134    tSMP_ENC output;
135    UINT8    i = 1, err = 0;
136    UINT8    x[16] = {0};
137    UINT8   *p_mac;
138
139    SMP_TRACE_EVENT ("cmac_aes_k_calculate ");
140
141    while (i <= cmac_cb.round)
142    {
143        smp_xor_128(&cmac_cb.text[(cmac_cb.round - i)*BT_OCTET16_LEN], x); /* Mi' := Mi (+) X  */
144
145        if (!SMP_Encrypt(key, BT_OCTET16_LEN, &cmac_cb.text[(cmac_cb.round - i)*BT_OCTET16_LEN], BT_OCTET16_LEN, &output))
146        {
147            err = 1;
148            break;
149        }
150
151        memcpy(x, output.param_buf, BT_OCTET16_LEN);
152        i ++;
153    }
154
155    if (!err)
156    {
157        p_mac = output.param_buf + (BT_OCTET16_LEN - tlen);
158        memcpy(p_signature, p_mac, tlen);
159
160        SMP_TRACE_DEBUG("tlen = %d p_mac = %d", tlen, p_mac);
161        SMP_TRACE_DEBUG("p_mac[0] = 0x%02x p_mac[1] = 0x%02x p_mac[2] = 0x%02x p_mac[3] = 0x%02x",
162                         *p_mac, *(p_mac + 1), *(p_mac + 2), *(p_mac + 3));
163        SMP_TRACE_DEBUG("p_mac[4] = 0x%02x p_mac[5] = 0x%02x p_mac[6] = 0x%02x p_mac[7] = 0x%02x",
164                         *(p_mac + 4), *(p_mac + 5), *(p_mac + 6), *(p_mac + 7));
165
166        return TRUE;
167
168    }
169    else
170        return FALSE;
171}
172/*******************************************************************************
173**
174** Function         cmac_prepare_last_block
175**
176** Description      This function proceeed to prepare the last block of message
177**                  Mn depending on the size of the message.
178**
179** Returns          void
180**
181*******************************************************************************/
182static void cmac_prepare_last_block (BT_OCTET16 k1, BT_OCTET16 k2)
183{
184//    UINT8       x[16] = {0};
185    BOOLEAN      flag;
186
187    SMP_TRACE_EVENT ("cmac_prepare_last_block ");
188    /* last block is a complete block set flag to 1 */
189    flag = ((cmac_cb.len % BT_OCTET16_LEN) == 0 && cmac_cb.len != 0)  ? TRUE : FALSE;
190
191    SMP_TRACE_WARNING("flag = %d round = %d", flag, cmac_cb.round);
192
193    if ( flag )
194    { /* last block is complete block */
195        smp_xor_128(&cmac_cb.text[0], k1);
196    }
197    else /* padding then xor with k2 */
198    {
199        padding(&cmac_cb.text[0], (UINT8)(cmac_cb.len % 16));
200
201        smp_xor_128(&cmac_cb.text[0], k2);
202    }
203}
204/*******************************************************************************
205**
206** Function         cmac_subkey_cont
207**
208** Description      This is the callback function when CIPHk(0[128]) is completed.
209**
210** Returns          void
211**
212*******************************************************************************/
213static void cmac_subkey_cont(tSMP_ENC *p)
214{
215    UINT8 k1[BT_OCTET16_LEN], k2[BT_OCTET16_LEN];
216    UINT8 *pp = p->param_buf;
217    SMP_TRACE_EVENT ("cmac_subkey_cont ");
218    print128(pp, (const UINT8 *)"K1 before shift");
219
220    /* If MSB(L) = 0, then K1 = L << 1 */
221    if ( (pp[BT_OCTET16_LEN - 1] & 0x80) != 0 )
222    {
223        /* Else K1 = ( L << 1 ) (+) Rb */
224        leftshift_onebit(pp, k1);
225        smp_xor_128(k1, const_Rb);
226    }
227    else
228    {
229        leftshift_onebit(pp, k1);
230    }
231
232    if ( (k1[BT_OCTET16_LEN - 1] & 0x80) != 0 )
233    {
234        /* K2 =  (K1 << 1) (+) Rb */
235        leftshift_onebit(k1, k2);
236        smp_xor_128(k2, const_Rb);
237    }
238    else
239    {
240        /* If MSB(K1) = 0, then K2 = K1 << 1 */
241        leftshift_onebit(k1, k2);
242    }
243
244    print128(k1, (const UINT8 *)"K1");
245    print128(k2, (const UINT8 *)"K2");
246
247    cmac_prepare_last_block (k1, k2);
248}
249/*******************************************************************************
250**
251** Function         cmac_generate_subkey
252**
253** Description      This is the function to generate the two subkeys.
254**
255** Parameters       key - CMAC key, expect SRK when used by SMP.
256**
257** Returns          void
258**
259*******************************************************************************/
260static BOOLEAN cmac_generate_subkey(BT_OCTET16 key)
261{
262    BT_OCTET16 z = {0};
263    BOOLEAN     ret = TRUE;
264    tSMP_ENC output;
265    SMP_TRACE_EVENT (" cmac_generate_subkey");
266
267    if (SMP_Encrypt(key, BT_OCTET16_LEN, z, BT_OCTET16_LEN, &output))
268    {
269        cmac_subkey_cont(&output);;
270    }
271    else
272        ret = FALSE;
273
274    return ret;
275}
276/*******************************************************************************
277**
278** Function         aes_cipher_msg_auth_code
279**
280** Description      This is the AES-CMAC Generation Function with tlen implemented.
281**
282** Parameters       key - CMAC key in little endian order, expect SRK when used by SMP.
283**                  input - text to be signed in little endian byte order.
284**                  length - length of the input in byte.
285**                  tlen - lenth of mac desired
286**                  p_signature - data pointer to where signed data to be stored, tlen long.
287**
288** Returns          FALSE if out of resources, TRUE in other cases.
289**
290*******************************************************************************/
291BOOLEAN aes_cipher_msg_auth_code(BT_OCTET16 key, UINT8 *input, UINT16 length,
292                                 UINT16 tlen, UINT8 *p_signature)
293{
294    UINT16  len, diff;
295    UINT16  n = (length + BT_OCTET16_LEN - 1) / BT_OCTET16_LEN;       /* n is number of rounds */
296    BOOLEAN ret = FALSE;
297
298    SMP_TRACE_EVENT ("%s", __func__);
299
300    if (n == 0)  n = 1;
301    len = n * BT_OCTET16_LEN;
302
303    SMP_TRACE_WARNING("AES128_CMAC started, allocate buffer size = %d", len);
304    /* allocate a memory space of multiple of 16 bytes to hold text  */
305    cmac_cb.text = (UINT8 *)osi_calloc(len);
306    cmac_cb.round = n;
307    diff = len - length;
308
309    if (input != NULL && length > 0) {
310        memcpy(&cmac_cb.text[diff] , input, (int)length);
311        cmac_cb.len = length;
312    } else {
313        cmac_cb.len = 0;
314    }
315
316    /* prepare calculation for subkey s and last block of data */
317    if (cmac_generate_subkey(key)) {
318        /* start calculation */
319        ret = cmac_aes_k_calculate(key, p_signature, tlen);
320    }
321    /* clean up */
322    cmac_aes_cleanup();
323
324    return ret;
325}
326
327    #if 0 /* testing code, sample data from spec */
328void test_cmac_cback(UINT8 *p_mac, UINT16 tlen)
329{
330    SMP_TRACE_EVENT ("test_cmac_cback ");
331    SMP_TRACE_ERROR("test_cmac_cback");
332}
333
334void test_cmac(void)
335{
336    SMP_TRACE_EVENT ("test_cmac ");
337    UINT8 M[64] = {
338        0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96,
339        0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a,
340        0xae, 0x2d, 0x8a, 0x57, 0x1e, 0x03, 0xac, 0x9c,
341        0x9e, 0xb7, 0x6f, 0xac, 0x45, 0xaf, 0x8e, 0x51,
342        0x30, 0xc8, 0x1c, 0x46, 0xa3, 0x5c, 0xe4, 0x11,
343        0xe5, 0xfb, 0xc1, 0x19, 0x1a, 0x0a, 0x52, 0xef,
344        0xf6, 0x9f, 0x24, 0x45, 0xdf, 0x4f, 0x9b, 0x17,
345        0xad, 0x2b, 0x41, 0x7b, 0xe6, 0x6c, 0x37, 0x10
346    };
347
348    UINT8 key[16] = {
349        0x3c, 0x4f, 0xcf, 0x09, 0x88, 0x15, 0xf7, 0xab,
350        0xa6, 0xd2, 0xae, 0x28, 0x16, 0x15, 0x7e, 0x2b
351    };
352    UINT8 i =0, tmp;
353    UINT16 len;
354
355    len = 64;
356
357    for (i = 0; i < len/2; i ++)
358    {
359        tmp = M[i];
360        M[i] = M[len -1 - i];
361        M[len -1 - i] = tmp;
362    }
363
364
365    memset(&cmac_cb, 0, sizeof(tCMAC_CB));
366
367    SMP_TRACE_WARNING("\n Example 1: len = %d\n", len);
368
369    aes_cipher_msg_auth_code(key, M, len, 128, test_cmac_cback, 0);
370
371}
372    #endif
373#endif
374
375