summaryrefslogtreecommitdiffstats
path: root/stack/smp/smp_cmac.c
blob: d72c3b50f0c8b5a0e2a41fbecc53757da2578e01 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
/*****************************************************************************
**
**  Name:           smp_cmac.c
**
**  Description:    This file contains the implementation of the AES128 CMAC
**                  algorithm.
**
**
**  Copyright (c) 2008-2009, Broadcom Corp., All Rights Reserved.
**  Broadcom Bluetooth Core. Proprietary and confidential.
******************************************************************************/

#include "bt_target.h"

#if SMP_INCLUDED == TRUE
    #include <stdio.h>
    #include <string.h>

    #include "btm_ble_api.h"
    #include "smp_int.h"
    #include "hcimsgs.h"

typedef struct
{
    UINT8               *text;
    UINT16              len;
    UINT16              round;
}tCMAC_CB;
 
tCMAC_CB    cmac_cb;

/* Rb for AES-128 as block cipher, LSB as [0] */
BT_OCTET16 const_Rb = {
    0x87, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

void print128(BT_OCTET16 x, const UINT8 *key_name)
{
#if SMP_DEBUG == TRUE
    UINT8  *p = (UINT8 *)x;
    UINT8  i;

    SMP_TRACE_WARNING1("%s(MSB ~ LSB) = ", key_name);

    for (i = 0; i < 4; i ++)
    {
        SMP_TRACE_WARNING4("%02x %02x %02x %02x", 
                           p[BT_OCTET16_LEN - i*4 -1], p[BT_OCTET16_LEN - i*4 -2], 
                           p[BT_OCTET16_LEN - i*4 -3], p[BT_OCTET16_LEN - i*4 -4]);
    }
#endif
}

/*******************************************************************************
**
** Function         padding
**
** Description      utility function to padding the given text to be a 128 bits 
**                  data. The parameter dest is input and output parameter, it
**                  must point to a BT_OCTET16_LEN memory space; where include
**                  length bytes valid data.
**      
** Returns          void
**
*******************************************************************************/
static void padding ( BT_OCTET16 dest, UINT8 length )
{
    UINT8   i, *p = dest;
    /* original last block */
    for ( i = length ; i < BT_OCTET16_LEN; i++ )
        p[BT_OCTET16_LEN - i - 1] = ( i == length ) ? 0x80 : 0;
}
/*******************************************************************************
**
** Function         leftshift_onebit
**
** Description      utility function to left shift one bit for a 128 bits value.
**      
** Returns          void
**
*******************************************************************************/
static void leftshift_onebit(UINT8 *input, UINT8 *output)
{
    UINT8   i, overflow = 0 , next_overflow = 0;
    SMP_TRACE_EVENT0 ("leftshift_onebit ");
    /* input[0] is LSB */
    for ( i = 0; i < BT_OCTET16_LEN ; i ++ )
    {
        next_overflow = (input[i] & 0x80) ? 1:0;
        output[i] = (input[i] << 1) | overflow;
        overflow = next_overflow;
    }
    return;
}
/*******************************************************************************
**
** Function         cmac_aes_cleanup
**
** Description      clean up function for AES_CMAC algorithm.
**      
** Returns          void
**
*******************************************************************************/
static void cmac_aes_cleanup(void)
{    
    if (cmac_cb.text != NULL)
    {
        GKI_freebuf(cmac_cb.text);
    }
    memset(&cmac_cb, 0, sizeof(tCMAC_CB));
}

/*******************************************************************************
**
** Function         cmac_aes_k_calculate
**
** Description      This function is the calculation of block cipher using AES-128.
**      
** Returns          void
**
*******************************************************************************/
static BOOLEAN cmac_aes_k_calculate(BT_OCTET16 key, UINT8 *p_signature, UINT16 tlen)
{
    tSMP_ENC output;
    UINT8    i = 1, err = 0;
    UINT8    x[16] = {0};
    UINT8   *p_mac;

    SMP_TRACE_EVENT0 ("cmac_aes_k_calculate ");

    while (i <= cmac_cb.round)
    {
        smp_xor_128(&cmac_cb.text[(cmac_cb.round - i)*BT_OCTET16_LEN], x); /* Mi' := Mi (+) X  */

        if (!SMP_Encrypt(key, BT_OCTET16_LEN, &cmac_cb.text[(cmac_cb.round - i)*BT_OCTET16_LEN], BT_OCTET16_LEN, &output))
        {
            err = 1;
            break;
        }
        
        memcpy(x, output.param_buf, BT_OCTET16_LEN);
        i ++;
    }

    if (!err)
    {
        p_mac = output.param_buf + (BT_OCTET16_LEN - tlen);
        memcpy(p_signature, p_mac, tlen);

        SMP_TRACE_DEBUG2("tlen = %d p_mac = %d", tlen, p_mac);
        SMP_TRACE_DEBUG4("p_mac[0] = 0x%02x p_mac[1] = 0x%02x p_mac[2] = 0x%02x p_mac[3] = 0x%02x", 
                         *p_mac, *(p_mac + 1), *(p_mac + 2), *(p_mac + 3));
        SMP_TRACE_DEBUG4("p_mac[4] = 0x%02x p_mac[5] = 0x%02x p_mac[6] = 0x%02x p_mac[7] = 0x%02x", 
                         *(p_mac + 4), *(p_mac + 5), *(p_mac + 6), *(p_mac + 7));

        return TRUE;

    }
    else
        return FALSE;
}
/*******************************************************************************
**
** Function         cmac_prepare_last_block
**
** Description      This function proceeed to prepare the last block of message
**                  Mn depending on the size of the message.
**      
** Returns          void
**
*******************************************************************************/
static void cmac_prepare_last_block (BT_OCTET16 k1, BT_OCTET16 k2)
{
//    UINT8       x[16] = {0};
    BOOLEAN      flag;

    SMP_TRACE_EVENT0 ("cmac_prepare_last_block ");
    /* last block is a complete block set flag to 1 */
    flag = ((cmac_cb.len % BT_OCTET16_LEN) == 0 && cmac_cb.len != 0)  ? TRUE : FALSE;

    SMP_TRACE_WARNING2("flag = %d round = %d", flag, cmac_cb.round);

    if ( flag )
    { /* last block is complete block */
        smp_xor_128(&cmac_cb.text[0], k1);
    }
    else /* padding then xor with k2 */
    {
        padding(&cmac_cb.text[0], (UINT8)(cmac_cb.len % 16));

        smp_xor_128(&cmac_cb.text[0], k2);
    }
}
/*******************************************************************************
**
** Function         cmac_subkey_cont
**
** Description      This is the callback function when CIPHk(0[128]) is completed.
**      
** Returns          void
**
*******************************************************************************/
static void cmac_subkey_cont(tSMP_ENC *p)
{
    UINT8 k1[BT_OCTET16_LEN], k2[BT_OCTET16_LEN];
    UINT8 *pp = p->param_buf;
    SMP_TRACE_EVENT0 ("cmac_subkey_cont ");
    print128(pp, (const UINT8 *)"K1 before shift");

    /* If MSB(L) = 0, then K1 = L << 1 */
    if ( (pp[BT_OCTET16_LEN - 1] & 0x80) != 0 )
    {
        /* Else K1 = ( L << 1 ) (+) Rb */
        leftshift_onebit(pp, k1);
        smp_xor_128(k1, const_Rb);
    }
    else
    {
        leftshift_onebit(pp, k1);
    }

    if ( (k1[BT_OCTET16_LEN - 1] & 0x80) != 0 )
    {
        /* K2 =  (K1 << 1) (+) Rb */
        leftshift_onebit(k1, k2);
        smp_xor_128(k2, const_Rb);
    }
    else
    {
        /* If MSB(K1) = 0, then K2 = K1 << 1 */
        leftshift_onebit(k1, k2);
    }

    print128(k1, (const UINT8 *)"K1");
    print128(k2, (const UINT8 *)"K2");

    cmac_prepare_last_block (k1, k2);
}
/*******************************************************************************
**
** Function         cmac_generate_subkey
**
** Description      This is the function to generate the two subkeys.
**            
** Parameters       key - CMAC key, expect SRK when used by SMP.
**      
** Returns          void
**
*******************************************************************************/
static BOOLEAN cmac_generate_subkey(BT_OCTET16 key)
{
    BT_OCTET16 z = {0};
    BOOLEAN     ret = TRUE;
    tSMP_ENC output;
    SMP_TRACE_EVENT0 (" cmac_generate_subkey");

    if (SMP_Encrypt(key, BT_OCTET16_LEN, z, BT_OCTET16_LEN, &output))
    {
        cmac_subkey_cont(&output);;
    }
    else
        ret = FALSE;

    return ret;
}
/*******************************************************************************
**
** Function         AES_CMAC
**
** Description      This is the AES-CMAC Generation Function with tlen implemented.
**            
** Parameters       key - CMAC key in little endian order, expect SRK when used by SMP.
**                  input - text to be signed in little endian byte order.
**                  length - length of the input in byte.
**                  tlen - lenth of mac desired
**                  p_signature - data pointer to where signed data to be stored, tlen long.
**      
** Returns          void
**
*******************************************************************************/
BOOLEAN AES_CMAC ( BT_OCTET16 key, UINT8 *input, UINT16 length,
                UINT16 tlen, UINT8 *p_signature)
{
    UINT16  len, diff;
    UINT16  n = (length + BT_OCTET16_LEN - 1) / BT_OCTET16_LEN;       /* n is number of rounds */
    BOOLEAN ret = FALSE;

    SMP_TRACE_EVENT0 ("AES_CMAC  ");

    if (n == 0)  n = 1;    
    len = n * BT_OCTET16_LEN;

    SMP_TRACE_WARNING1("AES128_CMAC started, allocate buffer size = %d", len);
    /* allocate a memory space of multiple of 16 bytes to hold text  */
    if ((cmac_cb.text = (UINT8 *)GKI_getbuf(len)) != NULL)
    {
        cmac_cb.round = n;

        memset(cmac_cb.text, 0, len);
        diff = len - length;

        if (input != NULL && length > 0)
        {
            memcpy(&cmac_cb.text[diff] , input, (int)length);
            cmac_cb.len = length;
        }
        else
            cmac_cb.len = 0;

        /* prepare calculation for subkey s and last block of data */
        if (cmac_generate_subkey(key))
        {
            /* start calculation */
            ret = cmac_aes_k_calculate(key, p_signature, tlen);
        }
        /* clean up */        
        cmac_aes_cleanup();
    }
    else
    {
        ret = FALSE;
        SMP_TRACE_ERROR0("No resources");
    }
    
    return ret;
}

    #if 0 /* testing code, sample data from spec */
void test_cmac_cback(UINT8 *p_mac, UINT16 tlen)
{
    SMP_TRACE_EVENT0 ("test_cmac_cback ");
    SMP_TRACE_ERROR0("test_cmac_cback");
}

void test_cmac(void)
{
    SMP_TRACE_EVENT0 ("test_cmac ");
    UINT8 M[64] = {
        0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96,
        0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a,
        0xae, 0x2d, 0x8a, 0x57, 0x1e, 0x03, 0xac, 0x9c,
        0x9e, 0xb7, 0x6f, 0xac, 0x45, 0xaf, 0x8e, 0x51,
        0x30, 0xc8, 0x1c, 0x46, 0xa3, 0x5c, 0xe4, 0x11,
        0xe5, 0xfb, 0xc1, 0x19, 0x1a, 0x0a, 0x52, 0xef,
        0xf6, 0x9f, 0x24, 0x45, 0xdf, 0x4f, 0x9b, 0x17,
        0xad, 0x2b, 0x41, 0x7b, 0xe6, 0x6c, 0x37, 0x10
    };

    UINT8 key[16] = {
        0x3c, 0x4f, 0xcf, 0x09, 0x88, 0x15, 0xf7, 0xab,
        0xa6, 0xd2, 0xae, 0x28, 0x16, 0x15, 0x7e, 0x2b
    };
    UINT8 i =0, tmp;
    UINT16 len;

    len = 64;

    for (i = 0; i < len/2; i ++)
    {
        tmp = M[i];
        M[i] = M[len -1 - i];
        M[len -1 - i] = tmp;
    }


    memset(&cmac_cb, 0, sizeof(tCMAC_CB));

    SMP_TRACE_WARNING1("\n Example 1: len = %d\n", len);

    AES_CMAC(key, M, len, 128, test_cmac_cback, 0);

}
    #endif
#endif