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