tools/smp-tester: Implement initial smp_c1 function

diff --git a/tools/smp-tester.c b/tools/smp-tester.c
index 28bc1a0..ad66536 100644 (file)
--- a/tools/smp-tester.c
+++ b/tools/smp-tester.c
@@ -89,6 +89,11 @@ struct test_data {
        uint16_t handle;
        size_t counter;
        int alg_sk;
+       uint8_t smp_tk[16];
+       uint8_t smp_prnd[16];
+       uint8_t smp_rrnd[16];
+       uint8_t smp_preq[7];
+       uint8_t smp_prsp[7];
 };
 
 struct smp_req_rsp {
@@ -133,6 +138,141 @@ static int alg_setup(void)
        return sk;
 }
 
+static int alg_new(int alg_sk, const uint8_t *key)
+{
+       int sk;
+
+       if (setsockopt(alg_sk, SOL_ALG, ALG_SET_KEY, key, 16) < 0) {
+               tester_warn("setsockopt(ALG_SET_KEY): %s", strerror(errno));
+               return -1;
+       }
+
+       sk = accept4(alg_sk, NULL, 0, SOCK_CLOEXEC);
+       if (sk < 0) {
+               tester_warn("accept4(AF_ALG): %s", strerror(errno));
+               return -1;
+       }
+
+       return sk;
+}
+
+static int alg_encrypt(int sk, uint8_t in[16], uint8_t out[16])
+{
+       __u32 alg_op = ALG_OP_ENCRYPT;
+       char cbuf[CMSG_SPACE(sizeof(alg_op))];
+       struct cmsghdr *cmsg;
+       struct msghdr msg;
+       struct iovec iov;
+       int ret;
+
+       memset(cbuf, 0, sizeof(cbuf));
+       memset(&msg, 0, sizeof(msg));
+
+       msg.msg_control = cbuf;
+       msg.msg_controllen = sizeof(cbuf);
+
+       cmsg = CMSG_FIRSTHDR(&msg);
+       cmsg->cmsg_level = SOL_ALG;
+       cmsg->cmsg_type = ALG_SET_OP;
+       cmsg->cmsg_len = CMSG_LEN(sizeof(alg_op));
+       memcpy(CMSG_DATA(cmsg), &alg_op, sizeof(alg_op));
+
+       iov.iov_base = in;
+       iov.iov_len = 16;
+
+       msg.msg_iov = &iov;
+       msg.msg_iovlen = 1;
+
+       ret = sendmsg(sk, &msg, 0);
+       if (ret < 0) {
+               tester_warn("sendmsg(AF_ALG): %s", strerror(errno));
+               return ret;
+       }
+
+       ret = read(sk, out, 16);
+       if (ret < 0)
+               tester_warn("read(AF_ALG): %s", strerror(errno));
+
+       return 0;
+
+}
+
+static int smp_e(uint8_t key[16], uint8_t in[16], uint8_t out[16])
+{
+       struct test_data *data = tester_get_data();
+       int sk, err;
+
+       sk = alg_new(data->alg_sk, key);
+       if (sk < 0)
+               return sk;
+
+       err = alg_encrypt(sk, in, out);
+
+       close(sk);
+
+       return err;
+}
+
+static inline void swap128(const uint8_t src[16], uint8_t dst[16])
+{
+        int i;
+        for (i = 0; i < 16; i++)
+                dst[15 - i] = src[i];
+}
+
+static inline void swap56(const uint8_t src[7], uint8_t dst[7])
+{
+        int i;
+        for (i = 0; i < 7; i++)
+                dst[6 - i] = src[i];
+}
+
+typedef struct {
+        uint64_t a, b;
+} u128;
+
+static inline void u128_xor(u128 *r, const u128 *p, const u128 *q)
+{
+        r->a = p->a ^ q->a;
+        r->b = p->b ^ q->b;
+}
+
+static int smp_c1(uint8_t k[16], uint8_t r[16], uint8_t preq[7],
+                       uint8_t pres[7], uint8_t _iat, bdaddr_t *ia,
+                       uint8_t _rat, bdaddr_t *ra, uint8_t res[16])
+{
+        uint8_t p1[16], p2[16];
+        int err;
+
+        memset(p1, 0, 16);
+
+        /* p1 = pres || preq || _rat || _iat */
+        swap56(pres, p1);
+        swap56(preq, p1 + 7);
+        p1[14] = _rat;
+        p1[15] = _iat;
+
+        memset(p2, 0, 16);
+
+        /* p2 = padding || ia || ra */
+        baswap((bdaddr_t *) (p2 + 4), ia);
+        baswap((bdaddr_t *) (p2 + 10), ra);
+
+        /* res = r XOR p1 */
+        u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
+
+        /* res = e(k, res) */
+        err = smp_e(k, res, res);
+        if (err)
+                return err;
+
+        /* res = res XOR p2 */
+        u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
+
+        /* res = e(k, res) */
+        return smp_e(k, res, res);
+}
+
 static void mgmt_debug(const char *str, void *user_data)
 {
        const char *prefix = user_data;
@@ -440,15 +580,23 @@ static void pair_device_complete(uint8_t status, uint16_t length,
 
 static const void *get_pdu(const uint8_t *data)
 {
+       struct test_data *test_data = tester_get_data();
        uint8_t opcode = data[0];
-       static uint16_t buf[17];
+       static uint8_t buf[17];
+       static bdaddr_t bda;
 
        switch (opcode) {
        case 0x03: /* Pairing Confirm */
-               memcpy(buf, data, sizeof(buf));
+               buf[0] = data[0];
+               smp_c1(test_data->smp_tk, test_data->smp_prnd,
+                       test_data->smp_preq, test_data->smp_prsp, 0, &bda,
+                                                       0, &bda, &buf[1]);
                return buf;
        case 0x04: /* Pairing Random */
-               memcpy(buf, data, sizeof(buf));
+               buf[0] = data[0];
+               smp_c1(test_data->smp_tk, test_data->smp_rrnd,
+                       test_data->smp_preq, test_data->smp_prsp, 0, &bda,
+                                                       0, &bda, &buf[1]);
                return buf;
        default:
                return data;