| /****************************************************************************** |
| * |
| * Copyright (C) 2008-2012 Broadcom Corporation |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at: |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| * |
| ******************************************************************************/ |
| |
| /****************************************************************************** |
| * |
| * This file contains the implementation of the AES128 CMAC algorithm. |
| * |
| ******************************************************************************/ |
| |
| #include "bt_target.h" |
| |
| #include <stdio.h> |
| #include <string.h> |
| |
| #include "btm_ble_api.h" |
| #include "smp_int.h" |
| #include "hcimsgs.h" |
| |
| typedef struct |
| { |
| uint8_t *text; |
| uint16_t len; |
| uint16_t 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_t *key_name) |
| { |
| #if (SMP_DEBUG == TRUE && SMP_DEBUG_VERBOSE == TRUE) |
| uint8_t *p = (uint8_t *)x; |
| uint8_t i; |
| |
| SMP_TRACE_WARNING("%s(MSB ~ LSB) = ", key_name); |
| |
| for (i = 0; i < 4; i ++) |
| { |
| SMP_TRACE_WARNING("%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_t length ) |
| { |
| uint8_t 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_t *input, uint8_t *output) |
| { |
| uint8_t i, overflow = 0 , next_overflow = 0; |
| SMP_TRACE_EVENT ("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) |
| { |
| osi_free(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 bool cmac_aes_k_calculate(BT_OCTET16 key, uint8_t *p_signature, uint16_t tlen) |
| { |
| tSMP_ENC output; |
| uint8_t i = 1, err = 0; |
| uint8_t x[16] = {0}; |
| uint8_t *p_mac; |
| |
| SMP_TRACE_EVENT ("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_DEBUG("tlen = %d p_mac = %d", tlen, p_mac); |
| SMP_TRACE_DEBUG("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_DEBUG("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_t x[16] = {0}; |
| bool flag; |
| |
| SMP_TRACE_EVENT ("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_WARNING("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_t)(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_t k1[BT_OCTET16_LEN], k2[BT_OCTET16_LEN]; |
| uint8_t *pp = p->param_buf; |
| SMP_TRACE_EVENT ("cmac_subkey_cont "); |
| print128(pp, (const uint8_t *)"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_t *)"K1"); |
| print128(k2, (const uint8_t *)"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 bool cmac_generate_subkey(BT_OCTET16 key) |
| { |
| BT_OCTET16 z = {0}; |
| bool ret = true; |
| tSMP_ENC output; |
| SMP_TRACE_EVENT (" 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_cipher_msg_auth_code |
| * |
| * 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 false if out of resources, true in other cases. |
| * |
| ******************************************************************************/ |
| bool aes_cipher_msg_auth_code(BT_OCTET16 key, uint8_t *input, uint16_t length, |
| uint16_t tlen, uint8_t *p_signature) |
| { |
| uint16_t len, diff; |
| uint16_t n = (length + BT_OCTET16_LEN - 1) / BT_OCTET16_LEN; /* n is number of rounds */ |
| bool ret = false; |
| |
| SMP_TRACE_EVENT ("%s", __func__); |
| |
| if (n == 0) n = 1; |
| len = n * BT_OCTET16_LEN; |
| |
| SMP_TRACE_WARNING("AES128_CMAC started, allocate buffer size = %d", len); |
| /* allocate a memory space of multiple of 16 bytes to hold text */ |
| cmac_cb.text = (uint8_t *)osi_calloc(len); |
| cmac_cb.round = n; |
| 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(); |
| |
| return ret; |
| } |