smp-tester: Add basic public key handling

1 files changed, 63 insertions, 10 deletions

diff --git a/tools/smp-tester.c b/tools/smp-tester.c
index 45c4d8bed..1b00705e8 100644
--- a/tools/smp-tester.c
+++ b/tools/smp-tester.c
@@ -42,6 +42,7 @@
 #include "emulator/hciemu.h"
 
 #include "src/shared/crypto.h"
+#include "src/shared/ecc.h"
 #include "src/shared/tester.h"
 #include "src/shared/mgmt.h"
 
@@ -69,6 +70,10 @@ struct test_data {
 	uint8_t preq[7];
 	uint8_t prsp[7];
 	uint8_t ltk[16];
+	uint8_t remote_pk[64];
+	uint8_t local_pk[64];
+	uint8_t local_sk[32];
+	uint8_t dhkey[32];
 	int unmet_conditions;
 };
 
@@ -448,9 +453,17 @@ static const uint8_t smp_sc_rsp_1[] = {	0x02,	/* Pairing Response */
 					0x0d,	/* Rsp. key dist. */
 };
 
+static const uint8_t smp_sc_pk[65] = { 0x0c };
+
+static const uint8_t smp_sc_failed_rsp_1[] = { 0x05, 0x08 };
+
 static const struct smp_req_rsp cli_sc_req_2[] = {
 	{ NULL, 0, smp_sc_req_1, sizeof(smp_sc_req_1) },
-	{ smp_sc_rsp_1, sizeof(smp_sc_rsp_1), NULL, 0 },
+	{ smp_sc_rsp_1, sizeof(smp_sc_rsp_1), smp_sc_pk, sizeof(smp_sc_pk) },
+	{ smp_sc_pk, sizeof(smp_sc_pk), NULL, 0 },
+	{ smp_confirm_req_1, sizeof(smp_confirm_req_1),
+				smp_random_req_1, sizeof(smp_random_req_1) },
+	{ smp_random_req_1, sizeof(smp_random_req_1), NULL, 0 },
 };
 
 static const struct smp_data smp_client_sc_req_2_test = {
@@ -492,6 +505,15 @@ static void setup_powered_client_callback(uint8_t status, uint16_t length,
 	bthost_set_adv_enable(bthost, 0x01, 0x00);
 }
 
+static void make_pk(struct test_data *data)
+{
+	if (!ecc_make_key(data->local_pk, data->local_sk)) {
+		tester_print("Failed to general local ECDH keypair");
+		tester_setup_failed();
+		return;
+	}
+}
+
 static void setup_powered_client(const void *test_data)
 {
 	struct test_data *data = tester_get_data();
@@ -504,10 +526,13 @@ static void setup_powered_client(const void *test_data)
 				sizeof(param), param, NULL, NULL, NULL);
 	mgmt_send(data->mgmt, MGMT_OP_SET_BONDABLE, data->mgmt_index,
 				sizeof(param), param, NULL, NULL, NULL);
-	if (smp->sc)
+	if (smp->sc) {
 		mgmt_send(data->mgmt, MGMT_OP_SET_SECURE_CONN,
 				data->mgmt_index, sizeof(param), param, NULL,
 				NULL, NULL);
+		make_pk(data);
+	}
+
 	mgmt_send(data->mgmt, MGMT_OP_SET_POWERED, data->mgmt_index,
 			sizeof(param), param, setup_powered_client_callback,
 			NULL, NULL);
@@ -527,8 +552,9 @@ static void pair_device_complete(uint8_t status, uint16_t length,
 static const void *get_pdu(const uint8_t *pdu)
 {
 	struct test_data *data = tester_get_data();
+	const struct smp_data *smp = data->test_data;
 	uint8_t opcode = pdu[0];
-	static uint8_t buf[17];
+	static uint8_t buf[65];
 
 	switch (opcode) {
 	case 0x01: /* Pairing Request */
@@ -539,14 +565,24 @@ static const void *get_pdu(const uint8_t *pdu)
 		break;
 	case 0x03: /* Pairing Confirm */
 		buf[0] = pdu[0];
-		bt_crypto_c1(data->crypto, data->tk, data->prnd, data->prsp,
-					data->preq, data->ia_type, data->ia,
-					data->ra_type, data->ra, &buf[1]);
+		if (smp->sc)
+			bt_crypto_f4(data->crypto, data->local_pk,
+					data->remote_pk, data->prnd, 0,
+					&buf[1]);
+		else
+			bt_crypto_c1(data->crypto, data->tk, data->prnd,
+					data->prsp, data->preq, data->ia_type,
+					data->ia, data->ra_type, data->ra,
+					&buf[1]);
 		return buf;
 	case 0x04: /* Pairing Random */
 		buf[0] = pdu[0];
 		memcpy(&buf[1], data->prnd, 16);
 		return buf;
+	case 0x0c: /* Public Key */
+		buf[0] = pdu[0];
+		memcpy(&buf[1], data->local_pk, 64);
+		return buf;
 	default:
 		break;
 	}
@@ -582,14 +618,19 @@ static bool verify_random(const uint8_t rnd[16])
 	return true;
 }
 
+static bool sc_random(struct test_data *test_data)
+{
+	return true;
+}
+
 static void smp_server(const void *data, uint16_t len, void *user_data)
 {
 	struct test_data *test_data = user_data;
 	struct bthost *bthost = hciemu_client_get_host(test_data->hciemu);
 	const struct smp_data *smp = test_data->test_data;
 	const struct smp_req_rsp *req;
-	const void *pdu;
 	uint8_t opcode;
+	const void *pdu;
 
 	if (len < 1) {
 		tester_warn("Received too small SMP PDU");
@@ -627,8 +668,18 @@ static void smp_server(const void *data, uint16_t len, void *user_data)
 		goto next;
 	case 0x04: /* Pairing Random */
 		memcpy(test_data->rrnd, data + 1, 16);
-		if (!verify_random(data + 1))
-			goto failed;
+		if (smp->sc) {
+			if (!sc_random(test_data))
+				goto failed;
+		} else {
+			if (!verify_random(data + 1))
+				goto failed;
+		}
+		goto next;
+	case 0x0c: /* Public Key */
+		memcpy(test_data->remote_pk, data + 1, 64);
+		ecdh_shared_secret(test_data->remote_pk, test_data->local_sk,
+							test_data->dhkey);
 		goto next;
 	default:
 		break;
@@ -821,10 +872,12 @@ static void setup_powered_server(const void *test_data)
 				sizeof(param), param, NULL, NULL, NULL);
 	mgmt_send(data->mgmt, MGMT_OP_SET_ADVERTISING, data->mgmt_index,
 				sizeof(param), param, NULL, NULL, NULL);
-	if (smp->sc)
+	if (smp->sc) {
 		mgmt_send(data->mgmt, MGMT_OP_SET_SECURE_CONN,
 				data->mgmt_index, sizeof(param), param, NULL,
 				NULL, NULL);
+		make_pk(data);
+	}
 
 	mgmt_send(data->mgmt, MGMT_OP_SET_POWERED, data->mgmt_index,
 			sizeof(param), param, setup_powered_server_callback,