int bletest_hci_le_encrypt(uint8_t *key, uint8_t *pt) { int rc; uint8_t *dst; uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_LE_ENCRYPT_LEN]; uint8_t rspbuf[16]; uint8_t rsplen; dst = buf; ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_ENCRYPT, BLE_HCI_LE_ENCRYPT_LEN, dst); dst += BLE_HCI_CMD_HDR_LEN; swap_buf(dst, key, BLE_ENC_BLOCK_SIZE); swap_buf(dst + BLE_ENC_BLOCK_SIZE, pt, BLE_ENC_BLOCK_SIZE); rc = ble_hs_hci_cmd_tx(buf, rspbuf, 16, &rsplen); if (rc != 0) { return rc; } if (rsplen != 16) { return BLE_HS_ECONTROLLER; } return rc; }
/* Data are taken from RFC 4493 Test Vectors */ static void test_le_encrypt(const void *test_data) { struct user_data *user = tester_get_data(); struct bt_hci_cmd_le_encrypt cmd; uint8_t key[16] = { 0x2b, 0x7e, 0x15, 0x16, 0x28, 0xae, 0xd2, 0xa6, 0xab, 0xf7, 0x15, 0x88, 0x09, 0xcf, 0x4f, 0x3c }; uint8_t plaintext[16] = { 0 }; /* Swap bytes since our interface has LE interface, opposed to * common crypto interface */ swap_buf(key, cmd.key, 16); swap_buf(plaintext, cmd.plaintext, 16); util_hexdump('<', cmd.key, 16, test_debug, NULL); util_hexdump('<', cmd.plaintext, 16, test_debug, NULL); if (!bt_hci_send(user->hci_ut, BT_HCI_CMD_LE_ENCRYPT, &cmd, sizeof(cmd), test_le_encrypt_complete, NULL, NULL)) { tester_warn("Failed to send HCI LE Encrypt command"); tester_test_failed(); return; } }
int bletest_send_ltk_req_reply(uint16_t handle) { struct hci_lt_key_req_reply hkr; uint16_t ack_conn_handle; uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_LT_KEY_REQ_REPLY_LEN]; uint8_t ack_params_len; int rc; hkr.conn_handle = handle; swap_buf(hkr.long_term_key, (uint8_t *)g_bletest_LTK, 16); ble_hs_hci_cmd_build_le_lt_key_req_reply(&hkr, buf, sizeof buf); rc = ble_hs_hci_cmd_tx(buf, &ack_conn_handle, sizeof ack_conn_handle, &ack_params_len); if (rc != 0) { return rc; } if (ack_params_len != BLE_HCI_LT_KEY_REQ_REPLY_ACK_PARAM_LEN - 1) { return -1; } if (le16toh(ack_conn_handle) != handle) { return -1; } return 0; }
int bletest_hci_le_add_resolv_list(uint8_t *local_irk, uint8_t *peer_irk, uint8_t *peer_ident_addr, uint8_t addr_type) { int rc; struct hci_add_dev_to_resolving_list padd; uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_ADD_TO_RESOLV_LIST_LEN]; padd.addr_type = addr_type; memcpy(padd.addr, peer_ident_addr, BLE_DEV_ADDR_LEN); swap_buf(padd.local_irk, local_irk, 16); swap_buf(padd.peer_irk, peer_irk, 16); rc = ble_hs_hci_cmd_build_add_to_resolv_list(&padd, buf, sizeof buf); if (!rc) { rc = ble_hs_hci_cmd_tx_empty_ack(buf); } return rc; }
/** * LE encrypt command * * @param cmdbuf * @param rspbuf * @param rsplen * * @return int */ static int ble_ll_hci_le_encrypt(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen) { int rc; struct ble_encryption_block ecb; /* Call the link layer to encrypt the data */ swap_buf(ecb.key, cmdbuf, BLE_ENC_BLOCK_SIZE); swap_buf(ecb.plain_text, cmdbuf + BLE_ENC_BLOCK_SIZE, BLE_ENC_BLOCK_SIZE); rc = ble_hw_encrypt_block(&ecb); if (!rc) { swap_buf(rspbuf, ecb.cipher_text, BLE_ENC_BLOCK_SIZE); *rsplen = BLE_ENC_BLOCK_SIZE; rc = BLE_ERR_SUCCESS; } else { *rsplen = 0; rc = BLE_ERR_CTLR_BUSY; } return rc; }
/** * Fit data to double buffer. * return pointer to full buffer or NULL if current buffer is not full */ uint8_t* bufferize(uint8_t *payload, uint32_t count){ uint8_t *ret; if (FREE > count){ memcpy(currbuf + offset, payload, count); offset += count; return NULL; } else{ /* put in current buffer as much as possible */ memcpy(currbuf + offset, payload, FREE); /* this pointer will be returned as a result of work */ ret = currbuf; /* switch to free buffer */ swap_buf(); /* rest of data put in free buffer */ memcpy(currbuf, payload + FREE, count - FREE); offset = count - FREE; return ret; } }
static void test_le_encrypt_complete(const void *data, uint8_t size, void *user_data) { const struct bt_hci_rsp_le_encrypt *rsp = data; uint8_t sample[16] = { 0x7d, 0xf7, 0x6b, 0x0c, 0x1a, 0xb8, 0x99, 0xb3, 0x3e, 0x42, 0xf0, 0x47, 0xb9, 0x1b, 0x54, 0x6f }; uint8_t enc_data[16]; if (rsp->status) { tester_warn("Failed HCI LE Encrypt (0x%02x)", rsp->status); tester_test_failed(); return; } swap_buf(rsp->data, enc_data, 16); util_hexdump('>', enc_data, 16, test_debug, NULL); if (!memcmp(sample, enc_data, 16)) tester_test_passed(); else tester_test_failed(); }
static void runtest(struct alg *alg, int count, int size, u_long cmd, struct timeval *tv) { int i, fd = crget(); struct timeval start, stop; char *cleartext = NULL, *ciphertext = NULL, *originaltext = NULL; struct session2_op sop; struct crypt_op cop; char iv[MAX_IV_SIZE]; bzero(&sop, sizeof(sop)); if (!alg->ishash) { sop.keylen = (alg->minkeylen + alg->maxkeylen)/2; sop.key = (char *) malloc(sop.keylen); if (sop.key == NULL) err(1, "malloc (key)"); for (i = 0; i < sop.keylen; i++) sop.key[i] = rdigit(); sop.cipher = alg->code; if (swap_key) swap_buf(sop.key, sop.keylen); } else { sop.mackeylen = (alg->minkeylen + alg->maxkeylen)/2; sop.mackey = (char *) malloc(sop.mackeylen); if (sop.mackey == NULL) err(1, "malloc (mac)"); for (i = 0; i < sop.mackeylen; i++) sop.mackey[i] = rdigit(); sop.mac = alg->code; if (swap_key) swap_buf(sop.mackey, sop.mackeylen); } sop.crid = crid; if (verbose) printf(" crid = %x\n", crid); if (ioctl(fd, cmd, &sop) < 0) { if (cmd == CIOCGSESSION || cmd == CIOCGSESSION2) { close(fd); if (verbose) { printf("cipher %s", alg->name); if (alg->ishash) printf(" mackeylen %u\n", sop.mackeylen); else printf(" keylen %u\n", sop.keylen); perror("CIOCGSESSION"); } /* hardware doesn't support algorithm; skip it */ return; } printf("cipher %s keylen %u mackeylen %u\n", alg->name, sop.keylen, sop.mackeylen); err(1, "CIOCGSESSION"); } originaltext = (char *)malloc((alg->ishash ? 2 : 3)*size + alg->hashsize); if (originaltext == NULL) err(1, "malloc (text)"); cleartext = originaltext+size; ciphertext = cleartext+size; for (i = 0; i < size; i++) cleartext[i] = rdigit(); memcpy(originaltext, cleartext, size); for (i = 0; i < N(iv); i++) iv[i] = rdigit(); if (verbose) { printf("alg = %s\n", alg->name); printf("session = 0x%x\n", sop.ses); printf("device = %s\n", crfind(sop.crid)); printf("count = %d, size = %d\n", count, size); if (!alg->ishash) { printf("iv:"); hexdump(iv, sizeof iv); } printf("cleartext:"); hexdump(cleartext, MIN(size, CHUNK)); } gettimeofday(&start, NULL); if (!alg->ishash) { for (i = 0; i < count; i++) { cop.ses = sop.ses; cop.op = COP_ENCRYPT; cop.flags = opflags; cop.len = size; cop.src = cleartext; cop.dst = ciphertext; cop.mac = 0; if (alg->hashsize) cop.iv = NULL; else cop.iv = iv; if (swap_data) swap_buf(cleartext, size); if (swap_data) swap_buf(ciphertext, size); if (swap_iv && !alg->hashsize) swap_buf(iv, N(iv)); if (ioctl(fd, CIOCCRYPT, &cop) < 0) err(1, "line %d:ioctl(CIOCCRYPT)", __LINE__); if (swap_data) swap_buf(cleartext, size); if (swap_data) swap_buf(ciphertext, size); if (swap_iv && !alg->hashsize) swap_buf(iv, N(iv)); if (verify && bcmp(ciphertext, cleartext, size) == 0) { printf("cipher text unchanged:"); hexdump(ciphertext, size); } if (verbose) { printf("ciphertext:"); hexdump(ciphertext, MIN(size, CHUNK)); printf("cipheriv:"); hexdump(iv, MIN(size, CHUNK)); } memset(cleartext, 'x', MIN(size, CHUNK)); cop.ses = sop.ses; cop.op = COP_DECRYPT; cop.flags = opflags; cop.len = size; cop.src = ciphertext; cop.dst = cleartext; cop.mac = 0; if (alg->hashsize) cop.iv = NULL; else cop.iv = iv; if (swap_data) swap_buf(cleartext, size); if (swap_data) swap_buf(ciphertext, size); if (swap_iv && !alg->hashsize) swap_buf(iv, N(iv)); if (ioctl(fd, CIOCCRYPT, &cop) < 0) err(1, "line %d:ioctl(CIOCCRYPT)", __LINE__); if (swap_data) swap_buf(cleartext, size); if (swap_data) swap_buf(ciphertext, size); if (swap_iv && !alg->hashsize) swap_buf(iv, N(iv)); if (verify && bcmp(cleartext, originaltext, size) != 0) { printf("decrypt mismatch:\n"); printf("original:"); hexdump(originaltext, size); printf("cleartext:"); hexdump(cleartext, size); } } } else { for (i = 0; i < count; i++) { cop.ses = sop.ses; cop.op = 0; cop.flags = opflags; cop.len = size; cop.src = cleartext; cop.dst = 0; cop.mac = ciphertext; cop.iv = 0; if (ioctl(fd, CIOCCRYPT, &cop) < 0) err(1, "line %d:ioctl(CIOCCRYPT)", __LINE__); if (verbose) { printf("ciphertext:"); hexdump(ciphertext, size); } } } gettimeofday(&stop, NULL); if (ioctl(fd, CIOCFSESSION, &sop.ses) < 0) perror("ioctl(CIOCFSESSION)"); if (verbose) { printf("cleartext:"); hexdump(cleartext, MIN(size, CHUNK)); } timersub(&stop, &start, tv); if (sop.key) free(sop.key); if (sop.mackey) free(sop.mackey); free(originaltext); close(fd); }
void bletest_execute_initiator(void) { int i; int rc; int8_t rssi; uint16_t handle; uint8_t new_chan_map[5]; /* * Determine if there is an active connection for the current handle * we are trying to create. If so, start looking for the next one */ if (g_bletest_current_conns < BLETEST_CFG_CONCURRENT_CONNS) { handle = g_bletest_current_conns + 1; if (ble_ll_conn_find_active_conn(handle)) { /* Set LED to slower blink rate */ g_bletest_led_rate = OS_TICKS_PER_SEC; /* Ask for version information */ rc = bletest_hci_rd_rem_version(handle); /* Ask for remote used features */ rc = bletest_hci_le_read_rem_used_feat(handle); /* Scanning better be stopped! */ assert(ble_ll_scan_enabled() == 0); /* Add to current connections */ if (!rc) { ++g_bletest_current_conns; /* Move to next connection */ if (g_bletest_current_conns < BLETEST_CFG_CONCURRENT_CONNS) { /* restart initiating */ g_bletest_cur_peer_addr[5] += 1; g_dev_addr[5] += 1; bletest_init_initiator(); } } } else { if (ble_ll_scan_enabled() == 0) { bletest_hci_le_create_connection(&g_cc); } } } else { if ((int32_t)(os_time_get() - g_next_os_time) >= 0) { if ((g_bletest_state == 1) || (g_bletest_state == 3)) { for (i = 0; i < g_bletest_current_conns; ++i) { if (ble_ll_conn_find_active_conn(i + 1)) { bletest_hci_le_rd_chanmap(i+1); } } } else if (g_bletest_state == 2) { new_chan_map[0] = 0; new_chan_map[1] = 0x3; new_chan_map[2] = 0; new_chan_map[3] = 0x1F; new_chan_map[4] = 0; bletest_hci_le_set_host_chan_class(new_chan_map); } else if (g_bletest_state == 4) { #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1) struct hci_start_encrypt hsle; for (i = 0; i < g_bletest_current_conns; ++i) { if (ble_ll_conn_find_active_conn(i + 1)) { hsle.connection_handle = i + 1; hsle.encrypted_diversifier = g_bletest_EDIV; hsle.random_number = g_bletest_RAND; swap_buf(hsle.long_term_key, (uint8_t *)g_bletest_LTK, 16); bletest_hci_le_start_encrypt(&hsle); } } #endif } else if (g_bletest_state == 8) { #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1) struct hci_start_encrypt hsle; for (i = 0; i < g_bletest_current_conns; ++i) { if (ble_ll_conn_find_active_conn(i + 1)) { hsle.connection_handle = i + 1; hsle.encrypted_diversifier = g_bletest_EDIV; hsle.random_number = ~g_bletest_RAND; swap_buf(hsle.long_term_key, (uint8_t *)g_bletest_LTK, 16); bletest_hci_le_start_encrypt(&hsle); } } #endif } else { for (i = 0; i < g_bletest_current_conns; ++i) { if (ble_ll_conn_find_active_conn(i + 1)) { ble_hs_hci_util_read_rssi(i+1, &rssi); } } } ++g_bletest_state; if (g_bletest_state > 9) { g_bletest_state = 9; } g_next_os_time = os_time_get() + OS_TICKS_PER_SEC * 3; } } }