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 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 *******************************************************************************/
cmac_aes_k_calculate(BT_OCTET16 key,UINT8 * p_signature,UINT16 tlen)132 static 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 *******************************************************************************/
cmac_prepare_last_block(BT_OCTET16 k1,BT_OCTET16 k2)182 static 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 *******************************************************************************/
cmac_subkey_cont(tSMP_ENC * p)213 static 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 *******************************************************************************/
cmac_generate_subkey(BT_OCTET16 key)260 static 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 *******************************************************************************/
aes_cipher_msg_auth_code(BT_OCTET16 key,UINT8 * input,UINT16 length,UINT16 tlen,UINT8 * p_signature)291 BOOLEAN 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 */
328 void 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
334 void 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