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;
}
Exemple #2
0
/* 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;
}
Exemple #6
0
/**
 * 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;
  }
}
Exemple #7
0
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();
}
Exemple #8
0
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);
}
Exemple #9
0
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;
        }
    }
}