/*******************************************************************************
**
** 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_EVENT ("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 smp_calculate_comfirm
**
** Description This function is called to calculate Confirm value.
**
** Returns void
**
*******************************************************************************/
void smp_calculate_comfirm (tSMP_CB *p_cb, BT_OCTET16 rand, BD_ADDR bda)
{
BT_OCTET16 p1;
tSMP_ENC output;
tSMP_STATUS status = SMP_PAIR_FAIL_UNKNOWN;
SMP_TRACE_DEBUG0 ("smp_calculate_comfirm ");
/* generate p1 = pres || preq || rat' || iat' */
smp_gen_p1_4_confirm(p_cb, p1);
/* p1 = rand XOR p1 */
smp_xor_128(p1, rand);
smp_debug_print_nbyte_little_endian ((UINT8 *)p1, (const UINT8 *)"P1' = r XOR p1", 16);
/* calculate e(k, r XOR p1), where k = TK */
if (!SMP_Encrypt(p_cb->tk, BT_OCTET16_LEN, p1, BT_OCTET16_LEN, &output))
{
SMP_TRACE_ERROR0("smp_generate_csrk failed");
smp_sm_event(p_cb, SMP_AUTH_CMPL_EVT, &status);
}
else
{
smp_calculate_comfirm_cont(p_cb, &output);
}
}
/*******************************************************************************
**
** 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_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)(cmac_cb.len % 16));
smp_xor_128(&cmac_cb.text[0], k2);
}
}
/*******************************************************************************
**
** 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_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 smp_calculate_comfirm_cont
**
** Description This function is called when SConfirm/MConfirm is generated
** proceed to send the Confirm request/response to peer device.
**
** Returns void
**
*******************************************************************************/
static void smp_calculate_comfirm_cont(tSMP_CB *p_cb, tSMP_ENC *p)
{
BT_OCTET16 p2;
tSMP_ENC output;
tSMP_STATUS status = SMP_PAIR_FAIL_UNKNOWN;
SMP_TRACE_DEBUG0 ("smp_calculate_comfirm_cont ");
#if SMP_DEBUG == TRUE
SMP_TRACE_DEBUG0("Confirm step 1 p1' = e(k, r XOR p1) Generated");
smp_debug_print_nbyte_little_endian (p->param_buf, (const UINT8 *)"C1", 16);
#endif
smp_gen_p2_4_confirm(p_cb, p2);
/* calculate p2 = (p1' XOR p2) */
smp_xor_128(p2, p->param_buf);
smp_debug_print_nbyte_little_endian ((UINT8 *)p2, (const UINT8 *)"p2' = C1 xor p2", 16);
/* calculate: Confirm = E(k, p1' XOR p2) */
if (!SMP_Encrypt(p_cb->tk, BT_OCTET16_LEN, p2, BT_OCTET16_LEN, &output))
{
SMP_TRACE_ERROR0("smp_calculate_comfirm_cont failed");
smp_sm_event(p_cb, SMP_AUTH_CMPL_EVT, &status);
}
else
{
switch (p_cb->rand_enc_proc)
{
case SMP_GEN_CONFIRM:
smp_process_confirm(p_cb, &output);
break;
case SMP_GEN_COMPARE:
smp_process_compare(p_cb, &output);
break;
}
}
}