int le_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, void *cp)
{
struct mgmt_cp_user_passkey_reply *psk_reply = cp;
struct l2cap_conn *conn = hcon->smp_conn;
u8 key[16];
u8 reason = 0;
int ret = 0;
BT_DBG("");
hcon->tk_valid = TRUE;
switch (mgmt_op) {
case MGMT_OP_USER_CONFIRM_NEG_REPLY:
reason = SMP_CONFIRM_FAILED;
break;
case MGMT_OP_USER_CONFIRM_REPLY:
break;
case MGMT_OP_USER_PASSKEY_REPLY:
memset(key, 0, sizeof(key));
BT_DBG("PassKey: %d", psk_reply->passkey);
put_unaligned_le32(psk_reply->passkey, key);
swap128(key, hcon->tk);
break;
default:
reason = SMP_CONFIRM_FAILED;
ret = -EOPNOTSUPP;
break;
}
if (reason) {
BT_DBG("smp_send_cmd: SMP_CMD_PAIRING_FAIL");
smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
&reason);
del_timer(&hcon->smp_timer);
clear_bit(HCI_CONN_ENCRYPT_PEND, &hcon->pend);
#ifdef BLUETOOTH_CUSTOMIZE
clear_bit(HCI_CONN_LE_CONN_UPDATE_PEND, &hcon->pend);
#endif /* BLUETOOTH_CUSTOMIZE */
mgmt_auth_failed(hcon->hdev->id, conn->dst, reason);
hci_conn_put(hcon);
} else if (hcon->cfm_pending) {
BT_DBG("send_pairing_confirm");
ret = send_pairing_confirm(conn);
}
return ret;
}
static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
{
struct smp_cmd_pairing *req, *rsp = (void *) skb->data;
struct smp_cmd_pairing_confirm cp;
struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;
int ret;
u8 res[16], key_size;
BT_DBG("conn %p", conn);
skb_pull(skb, sizeof(*rsp));
req = (void *) &conn->preq[1];
key_size = min(req->max_key_size, rsp->max_key_size);
if (check_enc_key_size(conn, key_size))
return SMP_ENC_KEY_SIZE;
if (rsp->oob_flag)
return SMP_OOB_NOT_AVAIL;
/* Just works */
memset(conn->tk, 0, sizeof(conn->tk));
conn->prsp[0] = SMP_CMD_PAIRING_RSP;
memcpy(&conn->prsp[1], rsp, sizeof(*rsp));
ret = smp_rand(conn->prnd);
if (ret)
return SMP_UNSPECIFIED;
ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp, 0,
conn->src, conn->hcon->dst_type, conn->dst, res);
if (ret)
return SMP_UNSPECIFIED;
swap128(res, cp.confirm_val);
smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
return 0;
}
static void confirm_work(struct work_struct *work)
{
struct smp_chan *smp = container_of(work, struct smp_chan, confirm);
struct l2cap_conn *conn = smp->conn;
struct crypto_blkcipher *tfm;
struct smp_cmd_pairing_confirm cp;
int ret;
u8 res[16], reason;
BT_DBG("conn %p", conn);
tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC);
if (IS_ERR(tfm)) {
reason = SMP_UNSPECIFIED;
goto error;
}
smp->tfm = tfm;
if (conn->hcon->out)
ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0,
conn->src, conn->hcon->dst_type, conn->dst,
res);
else
ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,
conn->hcon->dst_type, conn->dst, 0, conn->src,
res);
if (ret) {
reason = SMP_UNSPECIFIED;
goto error;
}
clear_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags);
swap128(res, cp.confirm_val);
smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);
return;
error:
smp_failure(conn, reason, 1);
}
void att_dump_attributes(void){
att_iterator_t it;
att_iterator_init(&it);
uint8_t uuid128[16];
while (att_iterator_has_next(&it)){
att_iterator_fetch_next(&it);
if (it.handle == 0) {
log_info("Handle: END");
return;
}
log_info("Handle: 0x%04x, flags: 0x%04x, uuid: ", it.handle, it.flags);
if (it.flags & ATT_PROPERTY_UUID128){
swap128(it.uuid, uuid128);
log_info("%s", uuid128_to_str(uuid128));
} else {
log_info("%04x", READ_BT_16(it.uuid, 0));
}
log_info(", value_len: %u, value: ", it.value_len);
hexdump(it.value, it.value_len);
}
}
int smp_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, __le32 passkey)
{
struct l2cap_conn *conn = hcon->smp_conn;
struct smp_chan *smp;
u32 value;
u8 key[16];
BT_DBG("");
if (!conn)
return -ENOTCONN;
smp = conn->smp_chan;
switch (mgmt_op) {
case MGMT_OP_USER_PASSKEY_REPLY:
value = le32_to_cpu(passkey);
memset(key, 0, sizeof(key));
BT_DBG("PassKey: %d", value);
put_unaligned_le32(value, key);
swap128(key, smp->tk);
/* Fall Through */
case MGMT_OP_USER_CONFIRM_REPLY:
set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);
break;
case MGMT_OP_USER_PASSKEY_NEG_REPLY:
case MGMT_OP_USER_CONFIRM_NEG_REPLY:
smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1);
return 0;
default:
smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1);
return -EOPNOTSUPP;
}
/* If it is our turn to send Pairing Confirm, do so now */
if (test_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags))
queue_work(hcon->hdev->workqueue, &smp->confirm);
return 0;
}
Encoding *RegisterAllocator::movaps(OperandXMMREG r128i, OperandXMMREG r128j)
{
if(r128i == r128j) return 0;
// Register overwritten, when not used, eliminate load instruction
if(XMM[r128i.reg].loadInstruction && loadElimination)
{
XMM[r128i.reg].loadInstruction->reserve();
XMM[r128i.reg].loadInstruction = 0;
}
// Register overwritten, when not used, eliminate copy instruction
if(XMM[r128i.reg].copyInstruction && copyPropagation)
{
XMM[r128i.reg].copyInstruction->reserve();
XMM[r128i.reg].copyInstruction = 0;
}
Encoding *spillInstruction = XMM[r128i.reg].spillInstruction;
AllocationData spillAllocation = XMM[r128i.reg].spill;
Encoding *movaps = Assembler::movaps(r128i, r128j);
if(XMM[r128i.reg].reference == 0 || XMM[r128j.reg].reference == 0) // Return if not in allocation table
{
return movaps;
}
// Attempt copy propagation
if(movaps && copyPropagation)
{
swap128(r128i.reg, r128j.reg);
XMM[r128i.reg].copyInstruction = movaps;
}
XMM[r128i.reg].spillInstruction = spillInstruction;
XMM[r128i.reg].spill = spillAllocation;
return movaps;
}
static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
{
struct smp_chan *smp = conn->smp_chan;
struct hci_dev *hdev = conn->hcon->hdev;
BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));
skb_pull(skb, sizeof(smp->pcnf));
if (conn->hcon->out) {
u8 random[16];
swap128(smp->prnd, random);
smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
random);
} else {
queue_work(hdev->workqueue, &smp->confirm);
}
return 0;
}
static void random_work(struct work_struct *work)
{
struct smp_chan *smp = container_of(work, struct smp_chan, random);
struct l2cap_conn *conn = smp->conn;
struct hci_conn *hcon = conn->hcon;
struct crypto_blkcipher *tfm = smp->tfm;
u8 reason, confirm[16], res[16], key[16];
int ret;
if (IS_ERR_OR_NULL(tfm)) {
reason = SMP_UNSPECIFIED;
goto error;
}
BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
if (hcon->out)
ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, 0,
conn->src, hcon->dst_type, conn->dst, res);
else
ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,
hcon->dst_type, conn->dst, 0, conn->src, res);
if (ret) {
reason = SMP_UNSPECIFIED;
goto error;
}
swap128(res, confirm);
if (memcmp(smp->pcnf, confirm, sizeof(smp->pcnf)) != 0) {
BT_ERR("Pairing failed (confirmation values mismatch)");
reason = SMP_CONFIRM_FAILED;
goto error;
}
if (hcon->out) {
u8 stk[16], rand[8];
__le16 ediv;
memset(rand, 0, sizeof(rand));
ediv = 0;
smp_s1(tfm, smp->tk, smp->rrnd, smp->prnd, key);
swap128(key, stk);
memset(stk + smp->enc_key_size, 0,
SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);
if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags)) {
reason = SMP_UNSPECIFIED;
goto error;
}
hci_le_start_enc(hcon, ediv, rand, stk);
hcon->enc_key_size = smp->enc_key_size;
} else {
u8 stk[16], r[16], rand[8];
__le16 ediv;
memset(rand, 0, sizeof(rand));
ediv = 0;
swap128(smp->prnd, r);
smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
smp_s1(tfm, smp->tk, smp->prnd, smp->rrnd, key);
swap128(key, stk);
memset(stk + smp->enc_key_size, 0,
SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);
hci_add_ltk(hcon->hdev, conn->dst, hcon->dst_type,
HCI_SMP_STK_SLAVE, 0, 0, stk, smp->enc_key_size,
ediv, rand);
}
return;
error:
smp_failure(conn, reason, 1);
}
static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth,
u8 local_io, u8 remote_io)
{
struct hci_conn *hcon = conn->hcon;
struct smp_chan *smp = conn->smp_chan;
u8 method;
u32 passkey = 0;
int ret = 0;
/* Initialize key for JUST WORKS */
memset(smp->tk, 0, sizeof(smp->tk));
clear_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);
BT_DBG("tk_request: auth:%d lcl:%d rem:%d", auth, local_io, remote_io);
/* If neither side wants MITM, use JUST WORKS */
/* If either side has unknown io_caps, use JUST WORKS */
/* Otherwise, look up method from the table */
if (!(auth & SMP_AUTH_MITM) ||
local_io > SMP_IO_KEYBOARD_DISPLAY ||
remote_io > SMP_IO_KEYBOARD_DISPLAY)
method = JUST_WORKS;
else
method = gen_method[remote_io][local_io];
/* If not bonding, don't ask user to confirm a Zero TK */
if (!(auth & SMP_AUTH_BONDING) && method == JUST_CFM)
method = JUST_WORKS;
/* If Just Works, Continue with Zero TK */
if (method == JUST_WORKS) {
set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);
return 0;
}
/* Not Just Works/Confirm results in MITM Authentication */
if (method != JUST_CFM)
set_bit(SMP_FLAG_MITM_AUTH, &smp->smp_flags);
/* If both devices have Keyoard-Display I/O, the master
* Confirms and the slave Enters the passkey.
*/
if (method == OVERLAP) {
if (hcon->link_mode & HCI_LM_MASTER)
method = CFM_PASSKEY;
else
method = REQ_PASSKEY;
}
/* Generate random passkey. Not valid until confirmed. */
if (method == CFM_PASSKEY) {
u8 key[16];
memset(key, 0, sizeof(key));
get_random_bytes(&passkey, sizeof(passkey));
passkey %= 1000000;
put_unaligned_le32(passkey, key);
swap128(key, smp->tk);
BT_DBG("PassKey: %d", passkey);
}
hci_dev_lock(hcon->hdev);
if (method == REQ_PASSKEY)
ret = mgmt_user_passkey_request(hcon->hdev, conn->dst,
hcon->type, hcon->dst_type);
else
ret = mgmt_user_confirm_request(hcon->hdev, conn->dst,
hcon->type, hcon->dst_type,
cpu_to_le32(passkey), 0);
hci_dev_unlock(hcon->hdev);
return ret;
}
static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
{
struct hci_conn *hcon = conn->hcon;
struct crypto_blkcipher *tfm = hcon->hdev->tfm;
int ret;
u8 key[16], res[16], random[16], confirm[16];
swap128(skb->data, random);
skb_pull(skb, sizeof(random));
if (conn->hcon->out)
ret = smp_c1(tfm, hcon->tk, random, hcon->preq, hcon->prsp, 0,
conn->src, hcon->dst_type, conn->dst,
res);
else
ret = smp_c1(tfm, hcon->tk, random, hcon->preq, hcon->prsp,
hcon->dst_type, conn->dst, 0, conn->src,
res);
if (ret)
return SMP_UNSPECIFIED;
BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
swap128(res, confirm);
if (memcmp(hcon->pcnf, confirm, sizeof(hcon->pcnf)) != 0) {
BT_ERR("Pairing failed (confirmation values mismatch)");
return SMP_CONFIRM_FAILED;
}
if (conn->hcon->out) {
u8 stk[16], rand[8];
__le16 ediv;
memset(rand, 0, sizeof(rand));
ediv = 0;
smp_s1(tfm, hcon->tk, random, hcon->prnd, key);
swap128(key, stk);
memset(stk + hcon->smp_key_size, 0,
SMP_MAX_ENC_KEY_SIZE - hcon->smp_key_size);
hci_le_start_enc(hcon, ediv, rand, stk);
hcon->enc_key_size = hcon->smp_key_size;
} else {
u8 stk[16], r[16], rand[8];
__le16 ediv;
memset(rand, 0, sizeof(rand));
ediv = 0;
swap128(hcon->prnd, r);
smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);
smp_s1(tfm, hcon->tk, hcon->prnd, random, key);
swap128(key, stk);
memset(stk + hcon->smp_key_size, 0,
SMP_MAX_ENC_KEY_SIZE - hcon->smp_key_size);
hci_add_ltk(conn->hcon->hdev, 0, conn->dst, hcon->dst_type,
hcon->smp_key_size, hcon->auth, ediv, rand, stk);
}
return 0;
}
static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth,
u8 local_io, u8 remote_io)
{
struct hci_conn *hcon = conn->hcon;
u8 method;
u32 passkey = 0;
int ret = 0;
memset(hcon->tk, 0, sizeof(hcon->tk));
hcon->tk_valid = FALSE;
hcon->auth = auth;
if (remote_oob && hcon->oob) {
method = SMP_REQ_OOB;
goto agent_request;
}
BT_DBG("tk_request: auth:%d lcl:%d rem:%d", auth, local_io, remote_io);
if (!(auth & SMP_AUTH_MITM) ||
local_io > SMP_IO_KEYBOARD_DISPLAY ||
remote_io > SMP_IO_KEYBOARD_DISPLAY) {
hcon->auth &= ~SMP_AUTH_MITM;
hcon->tk_valid = TRUE;
return 0;
}
method = gen_method[local_io][remote_io];
BT_DBG("tk_method: %d", method);
if (method == SMP_JUST_WORKS || method == SMP_JUST_CFM)
hcon->auth &= ~SMP_AUTH_MITM;
if (!(auth & SMP_AUTH_BONDING) && method == SMP_JUST_CFM) {
hcon->tk_valid = TRUE;
return 0;
} else if (method == SMP_JUST_WORKS) {
hcon->tk_valid = TRUE;
return 0;
} else if (method == SMP_OVERLAP) {
if (hcon->link_mode & HCI_LM_MASTER)
method = SMP_CFM_PASSKEY;
else
method = SMP_REQ_PASSKEY;
}
BT_DBG("tk_method-2: %d", method);
if (method == SMP_CFM_PASSKEY) {
u8 key[16];
memset(key, 0, sizeof(key));
get_random_bytes(&passkey, sizeof(passkey));
passkey %= 1000000;
put_unaligned_le32(passkey, key);
swap128(key, hcon->tk);
BT_DBG("PassKey: %d", passkey);
}
agent_request:
hci_dev_lock(hcon->hdev);
switch (method) {
case SMP_REQ_PASSKEY:
ret = mgmt_user_confirm_request(hcon->hdev->id,
HCI_EV_USER_PASSKEY_REQUEST, conn->dst, 0);
break;
case SMP_CFM_PASSKEY:
default:
ret = mgmt_user_confirm_request(hcon->hdev->id,
HCI_EV_USER_CONFIRM_REQUEST, conn->dst, passkey);
break;
}
hci_dev_unlock(hcon->hdev);
return ret;
}
static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth,
u8 local_io, u8 remote_io)
{
struct hci_conn *hcon = conn->hcon;
u8 method;
u32 passkey = 0;
int ret = 0;
/* Initialize key to JUST WORKS */
memset(hcon->tk, 0, sizeof(hcon->tk));
hcon->tk_valid = FALSE;
hcon->auth = auth;
/* By definition, OOB data will be used if both sides have it available
*/
if (remote_oob && hcon->oob) {
method = SMP_REQ_OOB;
goto agent_request;
}
BT_DBG("tk_request: auth:%d lcl:%d rem:%d", auth, local_io, remote_io);
/* If neither side wants MITM, use JUST WORKS */
/* If either side has unknown io_caps, use JUST_WORKS */
if (!(auth & SMP_AUTH_MITM) ||
local_io > SMP_IO_KEYBOARD_DISPLAY ||
remote_io > SMP_IO_KEYBOARD_DISPLAY) {
hcon->auth &= ~SMP_AUTH_MITM;
hcon->tk_valid = TRUE;
return 0;
}
/* MITM is now officially requested, but not required */
/* Determine what we need (if anything) from the agent */
method = gen_method[local_io][remote_io];
BT_DBG("tk_method: %d", method);
if (method == SMP_JUST_WORKS || method == SMP_JUST_CFM)
hcon->auth &= ~SMP_AUTH_MITM;
/* Don't bother confirming unbonded JUST_WORKS */
if (!(auth & SMP_AUTH_BONDING) && method == SMP_JUST_CFM) {
hcon->tk_valid = TRUE;
return 0;
} else if (method == SMP_JUST_WORKS) {
hcon->tk_valid = TRUE;
return 0;
} else if (method == SMP_OVERLAP) {
if (hcon->link_mode & HCI_LM_MASTER)
method = SMP_CFM_PASSKEY;
else
method = SMP_REQ_PASSKEY;
}
BT_DBG("tk_method-2: %d", method);
if (method == SMP_CFM_PASSKEY) {
u8 key[16];
/* Generate a passkey for display. It is not valid until
* confirmed.
*/
memset(key, 0, sizeof(key));
get_random_bytes(&passkey, sizeof(passkey));
passkey %= 1000000;
put_unaligned_le32(passkey, key);
swap128(key, hcon->tk);
BT_DBG("PassKey: %d", passkey);
}
agent_request:
hci_dev_lock(hcon->hdev);
switch (method) {
case SMP_REQ_PASSKEY:
ret = mgmt_user_confirm_request(hcon->hdev->id,
HCI_EV_USER_PASSKEY_REQUEST, conn->dst, 0);
break;
case SMP_CFM_PASSKEY:
default:
ret = mgmt_user_confirm_request(hcon->hdev->id,
HCI_EV_USER_CONFIRM_REQUEST, conn->dst, passkey);
break;
}
hci_dev_unlock(hcon->hdev);
return ret;
}
static void sm_c1(key_t k, key_t r, uint8_t preq[7], uint8_t pres[7], uint8_t iat, uint8_t rat, bd_addr_t ia, bd_addr_t ra, key_t c1){
// p1 = pres || preq || rat’ || iat’
// "The octet of iat’ becomes the least significant octet of p1 and the most signifi-
// cant octet of pres becomes the most significant octet of p1.
// For example, if the 8-bit iat’ is 0x01, the 8-bit rat’ is 0x00, the 56-bit preq
// is 0x07071000000101 and the 56 bit pres is 0x05000800000302 then
// p1 is 0x05000800000302070710000001010001."
key_t p1_flipped;
swap56(pres, &p1_flipped[0]);
swap56(preq, &p1_flipped[7]);
p1_flipped[14] = rat;
p1_flipped[15] = iat;
printf("p1' "); hexdump(p1_flipped, 16);
// p2 = padding || ia || ra
// "The least significant octet of ra becomes the least significant octet of p2 and
// the most significant octet of padding becomes the most significant octet of p2.
// For example, if 48-bit ia is 0xA1A2A3A4A5A6 and the 48-bit ra is
// 0xB1B2B3B4B5B6 then p2 is 0x00000000A1A2A3A4A5A6B1B2B3B4B5B6.
key_t p2_flipped;
memset(p2_flipped, 0, 16);
memcpy(&p2_flipped[4], ia, 6);
memcpy(&p2_flipped[10], ra, 6);
printf("p2' "); hexdump(p2_flipped, 16);
// t1 = r xor p1
int i;
key_t t1_flipped;
for (i=0;i<16;i++){
t1_flipped[i] = r[15-i] ^ p1_flipped[i];
}
printf("t1' "); hexdump(t1_flipped, 16);
key_t tk_flipped;
swap128(sm_tk, tk_flipped);
printf("tk' "); hexdump(tk_flipped, 16);
// setup aes decryption
unsigned long rk[RKLENGTH(KEYBITS)];
int nrounds = rijndaelSetupEncrypt(rk, &tk_flipped[0], KEYBITS);
// t2 = e(k, r_xor_p1)
key_t t2_flipped;
rijndaelEncrypt(rk, nrounds, t1_flipped, t2_flipped);
printf("t2' "); hexdump(t2_flipped, 16);
key_t t3_flipped;
for (i=0;i<16;i++){
t3_flipped[i] = t2_flipped[i] ^ p2_flipped[i];
}
printf("t3' "); hexdump(t3_flipped, 16);
key_t c1_flipped;
rijndaelEncrypt(rk, nrounds, t3_flipped, c1_flipped);
printf("c1' "); hexdump(c1_flipped, 16);
swap128(c1_flipped, c1);
printf("c1: "); hexdump(c1, 16);
}
int le_smpconnect(bdaddr_t *bd,int hci)
{
struct sockaddr_l2cap l2c;
int s;
unsigned char buf[40];
ssize_t len;
int i;
int count;
int handle = 0;
ng_l2cap_smp_pairinfo preq, pres;
fd_set rfds,wfds;
uint8_t k[16];
int conok = 0;
struct sockaddr_l2cap myname;
s = socket(PF_BLUETOOTH, SOCK_SEQPACKET|SOCK_NONBLOCK,
BLUETOOTH_PROTO_L2CAP);
l2c.l2cap_len = sizeof(l2c);
l2c.l2cap_family = AF_BLUETOOTH;
l2c.l2cap_psm = 0;
l2c.l2cap_cid = NG_L2CAP_SMP_CID;
l2c.l2cap_bdaddr_type = BDADDR_LE_PUBLIC;
bcopy(bd, &l2c.l2cap_bdaddr, sizeof(*bd));
printf("CONNECT\n");
if(connect(s, (struct sockaddr *) &l2c, sizeof(l2c)) == 0){
printf("CONNECTOK\n");
}else{
perror("connect");
}
#if 1
do{
handle = le_connect_result(hci);
}while(handle==0);
#endif
printf("handle%x\n", handle);
{
int fl;
fl = fcntl(s, F_GETFL, 0);
fcntl(s, F_SETFL, fl&~O_NONBLOCK);
}
printf("HOGEHOGE\n");
{
preq.code = NG_L2CAP_SMP_PAIRREQ;
preq.iocap = 4;
preq.oobflag = 0;
preq.authreq = 1;
preq.maxkeysize = 16;
preq.ikeydist = 1;
preq.rkeydist = 1;
write(s,&preq, sizeof(preq));
printf("A\n");
do {
int len;
printf("B\n");
len = read(s, &pres, sizeof(pres));
printf("%d, pi.code %d\n",len, pres.code);
}while(pres.code != NG_L2CAP_SMP_PAIRRES);
printf("C\n");
printf("%d %d %d %d %d %d %d(%d)\n", pres.code,pres.iocap ,
pres.oobflag, pres.authreq,
pres.maxkeysize, pres.ikeydist, pres.rkeydist, sizeof(pres));
}
{
socklen_t siz = sizeof(myname);
char bdastr[40];
if(getsockname(s, (struct sockaddr *)&myname,&siz)!=0){
perror("getsockname");
}
printf("%d\n", myname.l2cap_bdaddr_type);
printf("%s\n", bt_ntoa(&myname.l2cap_bdaddr, NULL));
}
{
ng_l2cap_smp_keyinfo mrand,mconfirm,srand,sconfirm;
ng_l2cap_smp_reqres failed;
int res;
uint8_t rval[16];
int ng = 0;
bzero(k, sizeof(k));
arc4random_buf(rval, sizeof(rval));
swap128(rval, mrand.body);
mconfirm.code = NG_L2CAP_SMP_PAIRCONF;
mrand.code = NG_L2CAP_SMP_PAIRRAND;
smp_c1(k, rval, (uint8_t *)&preq, (uint8_t *)&pres,
(myname.l2cap_bdaddr_type == BDADDR_LE_RANDOM)? 1:0,
&myname.l2cap_bdaddr, 0, bd);
swap128(rval, mconfirm.body);
write(s, &mconfirm, sizeof(mconfirm));
res = read(s, &sconfirm, sizeof(sconfirm));
printf("%d\n", res);
if(sconfirm.code != NG_L2CAP_SMP_PAIRCONF){
printf("sconfirm.code %d\n", sconfirm.code);
}
write(s, &mrand, sizeof(mrand));
res = read(s, &srand, sizeof(srand));
printf("%d\n", res);
if(srand.code != NG_L2CAP_SMP_PAIRRAND){
printf("srand.code %d\n", srand.code);
ng = 1;
goto fail;
}
swap128(srand.body, rval);
smp_c1(k, rval, (uint8_t *)&preq, (uint8_t *)&pres,
(myname.l2cap_bdaddr_type == BDADDR_LE_RANDOM)? 1:0,
&myname.l2cap_bdaddr, 0, bd);
for(i =0; i< 16; i++){
printf("%x:%x,", rval[i], sconfirm.body[15-i]);
if(rval[i] != sconfirm.body[15-i]){
ng = 1;
goto fail;
}
}
{
uint8_t mr[16], sr[16],stk[16];
ng_hci_le_start_encryption_cp cp;
ng_hci_status_rp rp;
uint8_t buf[128];
ng_hci_event_pkt_t *ep;
ng_hci_encryption_change_ep *eep;
int n;
swap128(mrand.body, mr);
swap128(srand.body, sr);
smp_s1(k, sr, mr, stk);
swap128(stk, cp.long_term_key);
#if 1
cp.connection_handle = handle;
cp.random_number = 0;
cp.encrypted_diversifier = 0;
n = sizeof(cp);
hci_request(hci, NG_HCI_OPCODE(NG_HCI_OGF_LE
,NG_HCI_OCF_LE_START_ENCRYPTION),
(char *)&cp, sizeof(cp), (char *)&rp, &n);
#endif
printf("LE_ENC OK\n");
{
ng_l2cap_smp_keyinfo ki;
ng_l2cap_smp_masterinfo mi;
ng_hci_le_start_encryption_cp cp;
ng_hci_status_rp rp;
uint8_t pkt[30];
int encok=0, mok=0;
while(encok==0||mok==0){
read(s, pkt, sizeof(pkt));
printf("%d\n", pkt[0]);
switch(pkt[0]){
case NG_L2CAP_SMP_MASTERINFO:
mok=1;
bcopy(pkt, &mi,sizeof(mi));
break;
case NG_L2CAP_SMP_ENCINFO:
encok=1;
bcopy(pkt,&ki, sizeof(ki));
break;
}
printf("%d %d\n", encok, mok);
}
printf("EDIV:%x \nRAND",mi.ediv);
cp.encrypted_diversifier = mi.ediv;
cp.random_number = 0;
for(i = 0; i < 8 ; i++){
printf("%02x ", mi.rand[i]);
cp.random_number
|= (((uint64_t)mi.rand[i])<<(i*8));
}
printf("\nKEY");
for(i = 0 ; i < 16; i++){
printf("%02x ", ki.body[i]);
cp.long_term_key[i] = ki.body[i];
}
printf("\n");
arc4random_buf(ki.body, sizeof(ki.body));
ki.code = NG_L2CAP_SMP_ENCINFO;
write(s, &ki, sizeof(ki));
mi.ediv = arc4random()&0xffff;
arc4random_buf(&mi.rand, sizeof(mi.rand));
mi.code = NG_L2CAP_SMP_MASTERINFO;
write(s, &mi, sizeof(mi));
sleep(4);
cp.connection_handle = handle;
n = sizeof(cp);
hci_request(hci, NG_HCI_OPCODE(NG_HCI_OGF_LE
,NG_HCI_OCF_LE_START_ENCRYPTION),
(char *)&cp, sizeof(cp), (char *)&rp, &n);
sleep(30);
}
}
fail:
if(ng){
failed.code = NG_L2CAP_SMP_PAIRFAIL;
failed.reqres = 4;
write(s, &failed, sizeof(failed));
}
}
return 0;
}
/* LISTING_START(GAPLEAdvDataParsing): Parsing advertising data */
static void dump_advertisement_data(uint8_t * adv_data, uint8_t adv_size){
ad_context_t context;
bd_addr_t address;
uint8_t uuid_128[16];
for (ad_iterator_init(&context, adv_size, adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)){
uint8_t data_type = ad_iterator_get_data_type(&context);
uint8_t size = ad_iterator_get_data_len(&context);
uint8_t * data = ad_iterator_get_data(&context);
if (data_type > 0 && data_type < 0x1B){
printf(" %s: ", ad_types[data_type]);
}
int i;
// Assigned Numbers GAP
switch (data_type){
case 0x01: // Flags
// show only first octet, ignore rest
for (i=0; i<8;i++){
if (data[0] & (1<<i)){
printf("%s; ", flags[i]);
}
}
break;
case 0x02: // Incomplete List of 16-bit Service Class UUIDs
case 0x03: // Complete List of 16-bit Service Class UUIDs
case 0x14: // List of 16-bit Service Solicitation UUIDs
for (i=0; i<size;i+=2){
printf("%02X ", READ_BT_16(data, i));
}
break;
case 0x04: // Incomplete List of 32-bit Service Class UUIDs
case 0x05: // Complete List of 32-bit Service Class UUIDs
for (i=0; i<size;i+=4){
printf("%04X ", READ_BT_32(data, i));
}
break;
case 0x06: // Incomplete List of 128-bit Service Class UUIDs
case 0x07: // Complete List of 128-bit Service Class UUIDs
case 0x15: // List of 128-bit Service Solicitation UUIDs
swap128(data, uuid_128);
printUUID128(uuid_128);
break;
case 0x08: // Shortened Local Name
case 0x09: // Complete Local Name
for (i=0; i<size;i++){
printf("%c", (char)(data[i]));
}
break;
case 0x0A: // Tx Power Level
printf("%d dBm", *(int8_t*)data);
break;
case 0x12: // Slave Connection Interval Range
printf("Connection Interval Min = %u ms, Max = %u ms", READ_BT_16(data, 0) * 5/4, READ_BT_16(data, 2) * 5/4);
break;
case 0x16: // Service Data
printf_hexdump(data, size);
break;
case 0x17: // Public Target Address
case 0x18: // Random Target Address
bt_flip_addr(address, data);
print_bd_addr(address);
break;
case 0x19: // Appearance
// https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.gap.appearance.xml
printf("%02X", READ_BT_16(data, 0) );
break;
case 0x1A: // Advertising Interval
printf("%u ms", READ_BT_16(data, 0) * 5/8 );
break;
case 0x3D: // 3D Information Data
printf_hexdump(data, size);
break;
case 0xFF: // Manufacturer Specific Data
break;
case 0x0D: // Class of Device (3B)
case 0x0E: // Simple Pairing Hash C (16B)
case 0x0F: // Simple Pairing Randomizer R (16B)
case 0x10: // Device ID
case 0x11: // Security Manager TK Value (16B)
default:
printf("Unknown Advertising Data Type");
break;
}
printf("\n");
}
printf("\n");
}
