crypto.c

Go to the documentation of this file.

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2012-2014  Intel Corporation. All rights reserved.
 *
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <sys/socket.h>

#include "util.h"
#include "crypto.h"

#ifndef HAVE_LINUX_IF_ALG_H
#ifndef HAVE_LINUX_TYPES_H
typedef uint8_t __u8;
typedef uint16_t __u16;
typedef uint32_t __u32;
#else
#include <linux/types.h>
#endif

struct sockaddr_alg {
    __u16   salg_family;
    __u8    salg_type[14];
    __u32   salg_feat;
    __u32   salg_mask;
    __u8    salg_name[64];
};

struct af_alg_iv {
    __u32   ivlen;
    __u8    iv[0];
};

#define ALG_SET_KEY                     1
#define ALG_SET_IV                      2
#define ALG_SET_OP                      3

#define ALG_OP_DECRYPT                  0
#define ALG_OP_ENCRYPT                  1

#define PF_ALG      38  /* Algorithm sockets.  */
#define AF_ALG      PF_ALG
#else
#include <linux/if_alg.h>
#endif

#ifndef SOL_ALG
#define SOL_ALG     279
#endif

/* Maximum message length that can be passed to aes_cmac */
#define CMAC_MSG_MAX    80

struct bt_crypto {
    int ref_count;
    int ecb_aes;
    int urandom;
    int cmac_aes;
};

static int urandom_setup(void)
{
    int fd;

    fd = open("/dev/urandom", O_RDONLY);
    if (fd < 0)
        return -1;

    return fd;
}

static int ecb_aes_setup(void)
{
    struct sockaddr_alg salg;
    int fd;

    fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
    if (fd < 0)
        return -1;

    memset(&salg, 0, sizeof(salg));
    salg.salg_family = AF_ALG;
    strcpy((char *) salg.salg_type, "skcipher");
    strcpy((char *) salg.salg_name, "ecb(aes)");

    if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
        close(fd);
        return -1;
    }

    return fd;
}

static int cmac_aes_setup(void)
{
    struct sockaddr_alg salg;
    int fd;

    fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
    if (fd < 0)
        return -1;

    memset(&salg, 0, sizeof(salg));
    salg.salg_family = AF_ALG;
    strcpy((char *) salg.salg_type, "hash");
    strcpy((char *) salg.salg_name, "cmac(aes)");

    if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
        close(fd);
        return -1;
    }

    return fd;
}

struct bt_crypto *bt_crypto_new(void)
{
    struct bt_crypto *crypto;

    crypto = new0(struct bt_crypto, 1);
    if (!crypto)
        return NULL;

    crypto->ecb_aes = ecb_aes_setup();
    if (crypto->ecb_aes < 0) {
        free(crypto);
        return NULL;
    }

    crypto->urandom = urandom_setup();
    if (crypto->urandom < 0) {
        close(crypto->ecb_aes);
        free(crypto);
        return NULL;
    }

    crypto->cmac_aes = cmac_aes_setup();
    if (crypto->cmac_aes < 0) {
        close(crypto->urandom);
        close(crypto->ecb_aes);
        free(crypto);
        return NULL;
    }

    return bt_crypto_ref(crypto);
}

struct bt_crypto *bt_crypto_ref(struct bt_crypto *crypto)
{
    if (!crypto)
        return NULL;

    __sync_fetch_and_add(&crypto->ref_count, 1);

    return crypto;
}

void bt_crypto_unref(struct bt_crypto *crypto)
{
    if (!crypto)
        return;

    if (__sync_sub_and_fetch(&crypto->ref_count, 1))
        return;

    close(crypto->urandom);
    close(crypto->ecb_aes);
    close(crypto->cmac_aes);

    free(crypto);
}

bool bt_crypto_random_bytes(struct bt_crypto *crypto,
                    uint8_t *buf, uint8_t num_bytes)
{
    ssize_t len;

    if (!crypto)
        return false;

    len = read(crypto->urandom, buf, num_bytes);
    if (len < num_bytes)
        return false;

    return true;
}

static int alg_new(int fd, const void *keyval, socklen_t keylen)
{
    if (setsockopt(fd, SOL_ALG, ALG_SET_KEY, keyval, keylen) < 0)
        return -1;

    /* FIXME: This should use accept4() with SOCK_CLOEXEC */
    return accept(fd, NULL, 0);
}

static bool alg_encrypt(int fd, const void *inbuf, size_t inlen,
                        void *outbuf, size_t outlen)
{
    __u32 alg_op = ALG_OP_ENCRYPT;
    char cbuf[CMSG_SPACE(sizeof(alg_op))];
    struct cmsghdr *cmsg;
    struct msghdr msg;
    struct iovec iov;
    ssize_t len;

    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 = (void *) inbuf;
    iov.iov_len = inlen;

    msg.msg_iov = &iov;
    msg.msg_iovlen = 1;

    len = sendmsg(fd, &msg, 0);
    if (len < 0)
        return false;

    len = read(fd, outbuf, outlen);
    if (len < 0)
        return false;

    return true;
}

static inline void swap_buf(const uint8_t *src, uint8_t *dst, uint16_t len)
{
    int i;

    for (i = 0; i < len; i++)
        dst[len - 1 - i] = src[i];
}

bool bt_crypto_sign_att(struct bt_crypto *crypto, const uint8_t key[16],
                const uint8_t *m, uint16_t m_len,
                uint32_t sign_cnt, uint8_t signature[12])
{
    int fd;
    int len;
    uint8_t tmp[16], out[16];
    uint16_t msg_len = m_len + sizeof(uint32_t);
    uint8_t msg[msg_len];
    uint8_t msg_s[msg_len];

    if (!crypto)
        return false;

    memset(msg, 0, msg_len);
    memcpy(msg, m, m_len);

    /* Add sign_counter to the message */
    put_le32(sign_cnt, msg + m_len);

    /* The most significant octet of key corresponds to key[0] */
    swap_buf(key, tmp, 16);

    fd = alg_new(crypto->cmac_aes, tmp, 16);
    if (fd < 0)
        return false;

    /* Swap msg before signing */
    swap_buf(msg, msg_s, msg_len);

    len = send(fd, msg_s, msg_len, 0);
    if (len < 0) {
        close(fd);
        return false;
    }

    len = read(fd, out, 16);
    if (len < 0) {
        close(fd);
        return false;
    }

    close(fd);

    /*
     * As to BT spec. 4.1 Vol[3], Part C, chapter 10.4.1 sign counter should
     * be placed in the signature
     */
    put_be32(sign_cnt, out + 8);

    /*
     * The most significant octet of hash corresponds to out[0]  - swap it.
     * Then truncate in most significant bit first order to a length of
     * 12 octets
     */
    swap_buf(out, tmp, 16);
    memcpy(signature, tmp + 4, 12);

    return true;
}
bool bt_crypto_e(struct bt_crypto *crypto, const uint8_t key[16],
            const uint8_t plaintext[16], uint8_t encrypted[16])
{
    uint8_t tmp[16], in[16], out[16];
    int fd;

    if (!crypto)
        return false;

    /* The most significant octet of key corresponds to key[0] */
    swap_buf(key, tmp, 16);

    fd = alg_new(crypto->ecb_aes, tmp, 16);
    if (fd < 0)
        return false;


    /* Most significant octet of plaintextData corresponds to in[0] */
    swap_buf(plaintext, in, 16);

    if (!alg_encrypt(fd, in, 16, out, 16)) {
        close(fd);
        return false;
    }

    /* Most significant octet of encryptedData corresponds to out[0] */
    swap_buf(out, encrypted, 16);

    close(fd);

    return true;
}

bool bt_crypto_ah(struct bt_crypto *crypto, const uint8_t k[16],
                    const uint8_t r[3], uint8_t hash[3])
{
    uint8_t rp[16];
    uint8_t encrypted[16];

    if (!crypto)
        return false;

    /* r' = padding || r */
    memcpy(rp, r, 3);
    memset(rp + 3, 0, 13);

    /* e(k, r') */
    if (!bt_crypto_e(crypto, k, rp, encrypted))
        return false;

    /* ah(k, r) = e(k, r') mod 2^24 */
    memcpy(hash, encrypted, 3);

    return true;
}

typedef struct {
    uint64_t a, b;
} u128;

static inline void u128_xor(const uint8_t p[16], const uint8_t q[16],
                                uint8_t r[16])
{
    u128 pp, qq, rr;

    memcpy(&pp, p, 16);
    memcpy(&qq, q, 16);

    rr.a = pp.a ^ qq.a;
    rr.b = pp.b ^ qq.b;

    memcpy(r, &rr, 16);
}

bool bt_crypto_c1(struct bt_crypto *crypto, const uint8_t k[16],
            const uint8_t r[16], const uint8_t pres[7],
            const uint8_t preq[7], uint8_t iat,
            const uint8_t ia[6], uint8_t rat,
            const uint8_t ra[6], uint8_t res[16])
{
    uint8_t p1[16], p2[16];

    /* p1 = pres || preq || _rat || _iat */
    p1[0] = iat;
    p1[1] = rat;
    memcpy(p1 + 2, preq, 7);
    memcpy(p1 + 9, pres, 7);

    /* p2 = padding || ia || ra */
    memcpy(p2, ra, 6);
    memcpy(p2 + 6, ia, 6);
    memset(p2 + 12, 0, 4);

    /* res = r XOR p1 */
    u128_xor(r, p1, res);

    /* res = e(k, res) */
    if (!bt_crypto_e(crypto, k, res, res))
        return false;

    /* res = res XOR p2 */
    u128_xor(res, p2, res);

    /* res = e(k, res) */
    return bt_crypto_e(crypto, k, res, res);
}

bool bt_crypto_s1(struct bt_crypto *crypto, const uint8_t k[16],
            const uint8_t r1[16], const uint8_t r2[16],
            uint8_t res[16])
{
    memcpy(res, r2, 8);
    memcpy(res + 8, r1, 8);

    return bt_crypto_e(crypto, k, res, res);
}

static bool aes_cmac(struct bt_crypto *crypto, uint8_t key[16], uint8_t *msg,
                    size_t msg_len, uint8_t res[16])
{
    uint8_t key_msb[16], out[16], msg_msb[CMAC_MSG_MAX];
    ssize_t len;
    int fd;

    if (msg_len > CMAC_MSG_MAX)
        return false;

    swap_buf(key, key_msb, 16);
    fd = alg_new(crypto->cmac_aes, key_msb, 16);
    if (fd < 0)
        return false;

    swap_buf(msg, msg_msb, msg_len);
    len = send(fd, msg_msb, msg_len, 0);
    if (len < 0) {
        close(fd);
        return false;
    }

    len = read(fd, out, 16);
    if (len < 0) {
        close(fd);
        return false;
    }

    swap_buf(out, res, 16);

    close(fd);

    return true;
}

bool bt_crypto_f4(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
                uint8_t x[16], uint8_t z, uint8_t res[16])
{
    uint8_t m[65];

    if (!crypto)
        return false;

    m[0] = z;
    memcpy(&m[1], v, 32);
    memcpy(&m[33], u, 32);

    return aes_cmac(crypto, x, m, sizeof(m), res);
}

bool bt_crypto_f5(struct bt_crypto *crypto, uint8_t w[32], uint8_t n1[16],
                uint8_t n2[16], uint8_t a1[7], uint8_t a2[7],
                uint8_t mackey[16], uint8_t ltk[16])
{
    uint8_t btle[4] = { 0x65, 0x6c, 0x74, 0x62 };
    uint8_t salt[16] = { 0xbe, 0x83, 0x60, 0x5a, 0xdb, 0x0b, 0x37, 0x60,
                 0x38, 0xa5, 0xf5, 0xaa, 0x91, 0x83, 0x88, 0x6c };
    uint8_t length[2] = { 0x00, 0x01 };
    uint8_t m[53], t[16];

    if (!aes_cmac(crypto, salt, w, 32, t))
        return false;

    memcpy(&m[0], length, 2);
    memcpy(&m[2], a2, 7);
    memcpy(&m[9], a1, 7);
    memcpy(&m[16], n2, 16);
    memcpy(&m[32], n1, 16);
    memcpy(&m[48], btle, 4);

    m[52] = 0; /* Counter */
    if (!aes_cmac(crypto, t, m, sizeof(m), mackey))
        return false;

    m[52] = 1; /* Counter */
    return aes_cmac(crypto, t, m, sizeof(m), ltk);
}

bool bt_crypto_f6(struct bt_crypto *crypto, uint8_t w[16], uint8_t n1[16],
            uint8_t n2[16], uint8_t r[16], uint8_t io_cap[3],
            uint8_t a1[7], uint8_t a2[7], uint8_t res[16])
{
    uint8_t m[65];

    memcpy(&m[0], a2, 7);
    memcpy(&m[7], a1, 7);
    memcpy(&m[14], io_cap, 3);
    memcpy(&m[17], r, 16);
    memcpy(&m[33], n2, 16);
    memcpy(&m[49], n1, 16);

    return aes_cmac(crypto, w, m, sizeof(m), res);
}

bool bt_crypto_g2(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
                uint8_t x[16], uint8_t y[16], uint32_t *val)
{
    uint8_t m[80], tmp[16];

    memcpy(&m[0], y, 16);
    memcpy(&m[16], v, 32);
    memcpy(&m[48], u, 32);

    if (!aes_cmac(crypto, x, m, sizeof(m), tmp))
        return false;

    *val = get_le32(tmp);
    *val %= 1000000;

    return true;
}