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
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    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*******************************************************************************/
135static 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*******************************************************************************/
185static 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*******************************************************************************/
216static 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*******************************************************************************/
263static 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*******************************************************************************/
294BOOLEAN 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 */
342void 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
348void 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