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 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 hci_dev *hdev = hcon->hdev;
struct crypto_blkcipher *tfm = hdev->tfm_aes;
u8 reason, confirm[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");
/* Prevent mutual access to hdev->tfm_aes */
hci_dev_lock(hdev);
ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,
hcon->init_addr_type, &hcon->init_addr,
hcon->resp_addr_type, &hcon->resp_addr, confirm);
hci_dev_unlock(hdev);
if (ret) {
reason = SMP_UNSPECIFIED;
goto error;
}
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];
__le64 rand = 0;
__le16 ediv = 0;
smp_s1(tfm, smp->tk, smp->rrnd, smp->prnd, 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], auth;
__le64 rand = 0;
__le16 ediv = 0;
smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd),
smp->prnd);
smp_s1(tfm, smp->tk, smp->prnd, smp->rrnd, stk);
memset(stk + smp->enc_key_size, 0,
SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);
if (hcon->pending_sec_level == BT_SECURITY_HIGH)
auth = 1;
else
auth = 0;
hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type,
HCI_SMP_STK_SLAVE, auth, stk, smp->enc_key_size,
ediv, rand);
}
return;
error:
smp_failure(conn, reason);
}
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;
}
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;
}
