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
print128(BT_OCTET16 x,const UINT8 * key_name)50 void 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 *******************************************************************************/
padding(BT_OCTET16 dest,UINT8 length)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 *******************************************************************************/
leftshift_onebit(UINT8 * input,UINT8 * output)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 *******************************************************************************/
cmac_aes_cleanup(void)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 *******************************************************************************/
cmac_aes_k_calculate(BT_OCTET16 key,UINT8 * p_signature,UINT16 tlen)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 *******************************************************************************/
cmac_prepare_last_block(BT_OCTET16 k1,BT_OCTET16 k2)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 *******************************************************************************/
cmac_subkey_cont(tSMP_ENC * p)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 *******************************************************************************/
cmac_generate_subkey(BT_OCTET16 key)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_cipher_msg_auth_code
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 FALSE if out of resources, TRUE in other cases.
292 **
293 *******************************************************************************/
aes_cipher_msg_auth_code(BT_OCTET16 key,UINT8 * input,UINT16 length,UINT16 tlen,UINT8 * p_signature)294 BOOLEAN aes_cipher_msg_auth_code(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 ("%s", __func__);
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_cipher_msg_auth_code(key, M, len, 128, test_cmac_cback, 0);
384
385 }
386 #endif
387 #endif
388
389