tools/testing/selftests/nci/nci_dev.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (C) 2021 Samsung Electrnoics
   4  * Bongsu Jeon <bongsu.jeon@samsung.com>
   5  *
   6  * Test code for nci
   7  */
   8
   9 #include <stdlib.h>
  10 #include <errno.h>
  11 #include <string.h>
  12 #include <sys/ioctl.h>
  13 #include <fcntl.h>
  14 #include <pthread.h>
  15 #include <linux/genetlink.h>
  16 #include <sys/socket.h>
  17 #include <linux/nfc.h>
  18
  19 #include "../kselftest_harness.h"
  20
  21 #define GENLMSG_DATA(glh)       ((void *)(NLMSG_DATA(glh) + GENL_HDRLEN))
  22 #define GENLMSG_PAYLOAD(glh)    (NLMSG_PAYLOAD(glh, 0) - GENL_HDRLEN)
  23 #define NLA_DATA(na)            ((void *)((char *)(na) + NLA_HDRLEN))
  24 #define NLA_PAYLOAD(len)        ((len) - NLA_HDRLEN)
  25
  26 #define MAX_MSG_SIZE    1024
  27
  28 #define IOCTL_GET_NCIDEV_IDX    0
  29 #define VIRTUAL_NFC_PROTOCOLS   (NFC_PROTO_JEWEL_MASK | \
  30                                  NFC_PROTO_MIFARE_MASK | \
  31                                  NFC_PROTO_FELICA_MASK | \
  32                                  NFC_PROTO_ISO14443_MASK | \
  33                                  NFC_PROTO_ISO14443_B_MASK | \
  34                                  NFC_PROTO_ISO15693_MASK)
  35
  36 const __u8 nci_reset_cmd[] = {0x20, 0x00, 0x01, 0x01};
  37 const __u8 nci_init_cmd[] = {0x20, 0x01, 0x00};
  38 const __u8 nci_rf_discovery_cmd[] = {0x21, 0x03, 0x09, 0x04, 0x00, 0x01,
  39                                       0x01, 0x01, 0x02, 0x01, 0x06, 0x01};
  40 const __u8 nci_init_cmd_v2[] = {0x20, 0x01, 0x02, 0x00, 0x00};
  41 const __u8 nci_rf_disc_map_cmd[] = {0x21, 0x00, 0x07, 0x02, 0x04, 0x03,
  42                                      0x02, 0x05, 0x03, 0x03};
  43 const __u8 nci_rf_deact_cmd[] = {0x21, 0x06, 0x01, 0x00};
  44 const __u8 nci_reset_rsp[] = {0x40, 0x00, 0x03, 0x00, 0x10, 0x01};
  45 const __u8 nci_reset_rsp_v2[] = {0x40, 0x00, 0x01, 0x00};
  46 const __u8 nci_reset_ntf[] = {0x60, 0x00, 0x09, 0x02, 0x01, 0x20, 0x0e,
  47                                0x04, 0x61, 0x00, 0x04, 0x02};
  48 const __u8 nci_init_rsp[] = {0x40, 0x01, 0x14, 0x00, 0x02, 0x0e, 0x02,
  49                               0x00, 0x03, 0x01, 0x02, 0x03, 0x02, 0xc8,
  50                               0x00, 0xff, 0x10, 0x00, 0x0e, 0x12, 0x00,
  51                               0x00, 0x04};
  52 const __u8 nci_init_rsp_v2[] = {0x40, 0x01, 0x1c, 0x00, 0x1a, 0x7e, 0x06,
  53                                  0x00, 0x02, 0x92, 0x04, 0xff, 0xff, 0x01,
  54                                  0x00, 0x40, 0x06, 0x00, 0x00, 0x01, 0x01,
  55                                  0x00, 0x02, 0x00, 0x03, 0x01, 0x01, 0x06,
  56                                  0x00, 0x80, 0x00};
  57 const __u8 nci_rf_disc_map_rsp[] = {0x41, 0x00, 0x01, 0x00};
  58 const __u8 nci_rf_disc_rsp[] = {0x41, 0x03, 0x01, 0x00};
  59 const __u8 nci_rf_deact_rsp[] = {0x41, 0x06, 0x01, 0x00};
  60 const __u8 nci_rf_deact_ntf[] = {0x61, 0x06, 0x02, 0x00, 0x00};
  61 const __u8 nci_rf_activate_ntf[] = {0x61, 0x05, 0x1D, 0x01, 0x02, 0x04, 0x00,
  62                                      0xFF, 0xFF, 0x0C, 0x44, 0x03, 0x07, 0x04,
  63                                      0x62, 0x26, 0x11, 0x80, 0x1D, 0x80, 0x01,
  64                                      0x20, 0x00, 0x00, 0x00, 0x06, 0x05, 0x75,
  65                                      0x77, 0x81, 0x02, 0x80};
  66 const __u8 nci_t4t_select_cmd[] = {0x00, 0x00, 0x0C, 0x00, 0xA4, 0x04, 0x00,
  67                                     0x07, 0xD2, 0x76, 0x00, 0x00, 0x85, 0x01, 0x01};
  68 const __u8 nci_t4t_select_cmd2[] = {0x00, 0x00, 0x07, 0x00, 0xA4, 0x00, 0x0C, 0x02,
  69                                      0xE1, 0x03};
  70 const __u8 nci_t4t_select_cmd3[] = {0x00, 0x00, 0x07, 0x00, 0xA4, 0x00, 0x0C, 0x02,
  71                                      0xE1, 0x04};
  72 const __u8 nci_t4t_read_cmd[] = {0x00, 0x00, 0x05, 0x00, 0xB0, 0x00, 0x00, 0x0F};
  73 const __u8 nci_t4t_read_rsp[] = {0x00, 0x00, 0x11, 0x00, 0x0F, 0x20, 0x00, 0x3B,
  74                                   0x00, 0x34, 0x04, 0x06, 0xE1, 0x04, 0x08, 0x00,
  75                                   0x00, 0x00, 0x90, 0x00};
  76 const __u8 nci_t4t_read_cmd2[] = {0x00, 0x00, 0x05, 0x00, 0xB0, 0x00, 0x00, 0x02};
  77 const __u8 nci_t4t_read_rsp2[] = {0x00, 0x00, 0x04, 0x00, 0x0F, 0x90, 0x00};
  78 const __u8 nci_t4t_read_cmd3[] = {0x00, 0x00, 0x05, 0x00, 0xB0, 0x00, 0x02, 0x0F};
  79 const __u8 nci_t4t_read_rsp3[] = {0x00, 0x00, 0x11, 0xD1, 0x01, 0x0B, 0x54, 0x02,
  80                                    0x65, 0x6E, 0x4E, 0x46, 0x43, 0x20, 0x54, 0x45,
  81                                    0x53, 0x54, 0x90, 0x00};
  82 const __u8 nci_t4t_rsp_ok[] = {0x00, 0x00, 0x02, 0x90, 0x00};
  83
  84 struct msgtemplate {
  85         struct nlmsghdr n;
  86         struct genlmsghdr g;
  87         char buf[MAX_MSG_SIZE];
  88 };
  89
  90 static int create_nl_socket(void)
  91 {
  92         int fd;
  93         struct sockaddr_nl local;
  94
  95         fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_GENERIC);
  96         if (fd < 0)
  97                 return -1;
  98
  99         memset(&local, 0, sizeof(local));
 100         local.nl_family = AF_NETLINK;
 101
 102         if (bind(fd, (struct sockaddr *)&local, sizeof(local)) < 0)
 103                 goto error;
 104
 105         return fd;
 106 error:
 107         close(fd);
 108         return -1;
 109 }
 110
 111 static int send_cmd_mt_nla(int sd, __u16 nlmsg_type, __u32 nlmsg_pid,
 112                            __u8 genl_cmd, int nla_num, __u16 nla_type[],
 113                            void *nla_data[], int nla_len[], __u16 flags)
 114 {
 115         struct sockaddr_nl nladdr;
 116         struct msgtemplate msg;
 117         struct nlattr *na;
 118         int cnt, prv_len;
 119         int r, buflen;
 120         char *buf;
 121
 122         msg.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
 123         msg.n.nlmsg_type = nlmsg_type;
 124         msg.n.nlmsg_flags = flags;
 125         msg.n.nlmsg_seq = 0;
 126         msg.n.nlmsg_pid = nlmsg_pid;
 127         msg.g.cmd = genl_cmd;
 128         msg.g.version = 0x1;
 129
 130         prv_len = 0;
 131         for (cnt = 0; cnt < nla_num; cnt++) {
 132                 na = (struct nlattr *)(GENLMSG_DATA(&msg) + prv_len);
 133                 na->nla_type = nla_type[cnt];
 134                 na->nla_len = nla_len[cnt] + NLA_HDRLEN;
 135
 136                 if (nla_len[cnt] > 0)
 137                         memcpy(NLA_DATA(na), nla_data[cnt], nla_len[cnt]);
 138
 139                 prv_len = NLA_ALIGN(nla_len[cnt]) + NLA_HDRLEN;
 140                 msg.n.nlmsg_len += prv_len;
 141         }
 142
 143         buf = (char *)&msg;
 144         buflen = msg.n.nlmsg_len;
 145         memset(&nladdr, 0, sizeof(nladdr));
 146         nladdr.nl_family = AF_NETLINK;
 147
 148         while ((r = sendto(sd, buf, buflen, 0, (struct sockaddr *)&nladdr,
 149                            sizeof(nladdr))) < buflen) {
 150                 if (r > 0) {
 151                         buf += r;
 152                         buflen -= r;
 153                 } else if (errno != EAGAIN) {
 154                         return -1;
 155                 }
 156         }
 157         return 0;
 158 }
 159
 160 static int send_get_nfc_family(int sd, __u32 pid)
 161 {
 162         __u16 nla_get_family_type = CTRL_ATTR_FAMILY_NAME;
 163         void *nla_get_family_data;
 164         int nla_get_family_len;
 165         char family_name[100];
 166
 167         nla_get_family_len = strlen(NFC_GENL_NAME) + 1;
 168         strcpy(family_name, NFC_GENL_NAME);
 169         nla_get_family_data = family_name;
 170
 171         return send_cmd_mt_nla(sd, GENL_ID_CTRL, pid, CTRL_CMD_GETFAMILY,
 172                                 1, &nla_get_family_type, &nla_get_family_data,
 173                                 &nla_get_family_len, NLM_F_REQUEST);
 174 }
 175
 176 static int get_family_id(int sd, __u32 pid, __u32 *event_group)
 177 {
 178         struct {
 179                 struct nlmsghdr n;
 180                 struct genlmsghdr g;
 181                 char buf[512];
 182         } ans;
 183         struct nlattr *na;
 184         int resp_len;
 185         __u16 id;
 186         int len;
 187         int rc;
 188
 189         rc = send_get_nfc_family(sd, pid);
 190
 191         if (rc < 0)
 192                 return 0;
 193
 194         resp_len = recv(sd, &ans, sizeof(ans), 0);
 195
 196         if (ans.n.nlmsg_type == NLMSG_ERROR || resp_len < 0 ||
 197             !NLMSG_OK(&ans.n, resp_len))
 198                 return 0;
 199
 200         len = 0;
 201         resp_len = GENLMSG_PAYLOAD(&ans.n);
 202         na = (struct nlattr *)GENLMSG_DATA(&ans);
 203
 204         while (len < resp_len) {
 205                 len += NLA_ALIGN(na->nla_len);
 206                 if (na->nla_type == CTRL_ATTR_FAMILY_ID) {
 207                         id = *(__u16 *)NLA_DATA(na);
 208                 } else if (na->nla_type == CTRL_ATTR_MCAST_GROUPS) {
 209                         struct nlattr *nested_na;
 210                         struct nlattr *group_na;
 211                         int group_attr_len;
 212                         int group_attr;
 213
 214                         nested_na = (struct nlattr *)((char *)na + NLA_HDRLEN);
 215                         group_na = (struct nlattr *)((char *)nested_na + NLA_HDRLEN);
 216                         group_attr_len = 0;
 217
 218                         for (group_attr = CTRL_ATTR_MCAST_GRP_UNSPEC;
 219                                 group_attr < CTRL_ATTR_MCAST_GRP_MAX; group_attr++) {
 220                                 if (group_na->nla_type == CTRL_ATTR_MCAST_GRP_ID) {
 221                                         *event_group = *(__u32 *)((char *)group_na +
 222                                                                   NLA_HDRLEN);
 223                                         break;
 224                                 }
 225
 226                                 group_attr_len += NLA_ALIGN(group_na->nla_len) +
 227                                                   NLA_HDRLEN;
 228                                 if (group_attr_len >= nested_na->nla_len)
 229                                         break;
 230
 231                                 group_na = (struct nlattr *)((char *)group_na +
 232                                                              NLA_ALIGN(group_na->nla_len));
 233                         }
 234                 }
 235                 na = (struct nlattr *)(GENLMSG_DATA(&ans) + len);
 236         }
 237         return id;
 238 }
 239
 240 static int send_cmd_with_idx(int sd, __u16 nlmsg_type, __u32 nlmsg_pid,
 241                              __u8 genl_cmd, int dev_id)
 242 {
 243         __u16 nla_type = NFC_ATTR_DEVICE_INDEX;
 244         void *nla_data = &dev_id;
 245         int nla_len = 4;
 246
 247         return send_cmd_mt_nla(sd, nlmsg_type, nlmsg_pid, genl_cmd, 1,
 248                                 &nla_type, &nla_data, &nla_len, NLM_F_REQUEST);
 249 }
 250
 251 static int get_nci_devid(int sd, __u16 fid, __u32 pid, int dev_id, struct msgtemplate *msg)
 252 {
 253         int rc, resp_len;
 254
 255         rc = send_cmd_with_idx(sd, fid, pid, NFC_CMD_GET_DEVICE, dev_id);
 256         if (rc < 0) {
 257                 rc = -1;
 258                 goto error;
 259         }
 260
 261         resp_len = recv(sd, msg, sizeof(*msg), 0);
 262         if (resp_len < 0) {
 263                 rc = -2;
 264                 goto error;
 265         }
 266
 267         if (msg->n.nlmsg_type == NLMSG_ERROR ||
 268             !NLMSG_OK(&msg->n, resp_len)) {
 269                 rc = -3;
 270                 goto error;
 271         }
 272
 273         return 0;
 274 error:
 275         return rc;
 276 }
 277
 278 static __u8 get_dev_enable_state(struct msgtemplate *msg)
 279 {
 280         struct nlattr *na;
 281         int resp_len;
 282         int len;
 283
 284         resp_len = GENLMSG_PAYLOAD(&msg->n);
 285         na = (struct nlattr *)GENLMSG_DATA(msg);
 286         len = 0;
 287
 288         while (len < resp_len) {
 289                 len += NLA_ALIGN(na->nla_len);
 290                 if (na->nla_type == NFC_ATTR_DEVICE_POWERED)
 291                         return *(char *)NLA_DATA(na);
 292                 na = (struct nlattr *)(GENLMSG_DATA(msg) + len);
 293         }
 294
 295         return resp_len;
 296 }
 297
 298 FIXTURE(NCI) {
 299         int virtual_nci_fd;
 300         bool open_state;
 301         int dev_idex;
 302         bool isNCI2;
 303         int proto;
 304         __u32 pid;
 305         __u16 fid;
 306         int sd;
 307 };
 308
 309 FIXTURE_VARIANT(NCI) {
 310         bool isNCI2;
 311 };
 312
 313 FIXTURE_VARIANT_ADD(NCI, NCI1_0) {
 314         .isNCI2 = false,
 315 };
 316
 317 FIXTURE_VARIANT_ADD(NCI, NCI2_0) {
 318         .isNCI2 = true,
 319 };
 320
 321 static void *virtual_dev_open(void *data)
 322 {
 323         char buf[258];
 324         int dev_fd;
 325         int len;
 326
 327         dev_fd = *(int *)data;
 328
 329         len = read(dev_fd, buf, 258);
 330         if (len <= 0)
 331                 goto error;
 332         if (len != sizeof(nci_reset_cmd))
 333                 goto error;
 334         if (memcmp(nci_reset_cmd, buf, len))
 335                 goto error;
 336         write(dev_fd, nci_reset_rsp, sizeof(nci_reset_rsp));
 337
 338         len = read(dev_fd, buf, 258);
 339         if (len <= 0)
 340                 goto error;
 341         if (len != sizeof(nci_init_cmd))
 342                 goto error;
 343         if (memcmp(nci_init_cmd, buf, len))
 344                 goto error;
 345         write(dev_fd, nci_init_rsp, sizeof(nci_init_rsp));
 346
 347         len = read(dev_fd, buf, 258);
 348         if (len <= 0)
 349                 goto error;
 350         if (len != sizeof(nci_rf_disc_map_cmd))
 351                 goto error;
 352         if (memcmp(nci_rf_disc_map_cmd, buf, len))
 353                 goto error;
 354         write(dev_fd, nci_rf_disc_map_rsp, sizeof(nci_rf_disc_map_rsp));
 355
 356         return (void *)0;
 357 error:
 358         return (void *)-1;
 359 }
 360
 361 static void *virtual_dev_open_v2(void *data)
 362 {
 363         char buf[258];
 364         int dev_fd;
 365         int len;
 366
 367         dev_fd = *(int *)data;
 368
 369         len = read(dev_fd, buf, 258);
 370         if (len <= 0)
 371                 goto error;
 372         if (len != sizeof(nci_reset_cmd))
 373                 goto error;
 374         if (memcmp(nci_reset_cmd, buf, len))
 375                 goto error;
 376         write(dev_fd, nci_reset_rsp_v2, sizeof(nci_reset_rsp_v2));
 377         write(dev_fd, nci_reset_ntf, sizeof(nci_reset_ntf));
 378
 379         len = read(dev_fd, buf, 258);
 380         if (len <= 0)
 381                 goto error;
 382         if (len != sizeof(nci_init_cmd_v2))
 383                 goto error;
 384         if (memcmp(nci_init_cmd_v2, buf, len))
 385                 goto error;
 386         write(dev_fd, nci_init_rsp_v2, sizeof(nci_init_rsp_v2));
 387
 388         len = read(dev_fd, buf, 258);
 389         if (len <= 0)
 390                 goto error;
 391         if (len != sizeof(nci_rf_disc_map_cmd))
 392                 goto error;
 393         if (memcmp(nci_rf_disc_map_cmd, buf, len))
 394                 goto error;
 395         write(dev_fd, nci_rf_disc_map_rsp, sizeof(nci_rf_disc_map_rsp));
 396
 397         return (void *)0;
 398 error:
 399         return (void *)-1;
 400 }
 401
 402 FIXTURE_SETUP(NCI)
 403 {
 404         struct msgtemplate msg;
 405         pthread_t thread_t;
 406         __u32 event_group;
 407         int status;
 408         int rc;
 409
 410         self->open_state = false;
 411         self->proto = VIRTUAL_NFC_PROTOCOLS;
 412         self->isNCI2 = variant->isNCI2;
 413
 414         self->sd = create_nl_socket();
 415         ASSERT_NE(self->sd, -1);
 416
 417         self->pid = getpid();
 418         self->fid = get_family_id(self->sd, self->pid, &event_group);
 419         ASSERT_NE(self->fid, -1);
 420
 421         self->virtual_nci_fd = open("/dev/virtual_nci", O_RDWR);
 422         ASSERT_GT(self->virtual_nci_fd, -1);
 423
 424         rc = setsockopt(self->sd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &event_group,
 425                         sizeof(event_group));
 426         ASSERT_NE(rc, -1);
 427
 428         rc = ioctl(self->virtual_nci_fd, IOCTL_GET_NCIDEV_IDX, &self->dev_idex);
 429         ASSERT_EQ(rc, 0);
 430
 431         rc = get_nci_devid(self->sd, self->fid, self->pid, self->dev_idex, &msg);
 432         ASSERT_EQ(rc, 0);
 433         EXPECT_EQ(get_dev_enable_state(&msg), 0);
 434
 435         if (self->isNCI2)
 436                 rc = pthread_create(&thread_t, NULL, virtual_dev_open_v2,
 437                                     (void *)&self->virtual_nci_fd);
 438         else
 439                 rc = pthread_create(&thread_t, NULL, virtual_dev_open,
 440                                     (void *)&self->virtual_nci_fd);
 441         ASSERT_GT(rc, -1);
 442
 443         rc = send_cmd_with_idx(self->sd, self->fid, self->pid,
 444                                NFC_CMD_DEV_UP, self->dev_idex);
 445         EXPECT_EQ(rc, 0);
 446
 447         pthread_join(thread_t, (void **)&status);
 448         ASSERT_EQ(status, 0);
 449         self->open_state = true;
 450 }
 451
 452 static void *virtual_deinit(void *data)
 453 {
 454         char buf[258];
 455         int dev_fd;
 456         int len;
 457
 458         dev_fd = *(int *)data;
 459
 460         len = read(dev_fd, buf, 258);
 461         if (len <= 0)
 462                 goto error;
 463         if (len != sizeof(nci_reset_cmd))
 464                 goto error;
 465         if (memcmp(nci_reset_cmd, buf, len))
 466                 goto error;
 467         write(dev_fd, nci_reset_rsp, sizeof(nci_reset_rsp));
 468
 469         return (void *)0;
 470 error:
 471         return (void *)-1;
 472 }
 473
 474 static void *virtual_deinit_v2(void *data)
 475 {
 476         char buf[258];
 477         int dev_fd;
 478         int len;
 479
 480         dev_fd = *(int *)data;
 481
 482         len = read(dev_fd, buf, 258);
 483         if (len <= 0)
 484                 goto error;
 485         if (len != sizeof(nci_reset_cmd))
 486                 goto error;
 487         if (memcmp(nci_reset_cmd, buf, len))
 488                 goto error;
 489         write(dev_fd, nci_reset_rsp_v2, sizeof(nci_reset_rsp_v2));
 490         write(dev_fd, nci_reset_ntf, sizeof(nci_reset_ntf));
 491
 492         return (void *)0;
 493 error:
 494         return (void *)-1;
 495 }
 496
 497 FIXTURE_TEARDOWN(NCI)
 498 {
 499         pthread_t thread_t;
 500         int status;
 501         int rc;
 502
 503         if (self->open_state) {
 504                 if (self->isNCI2)
 505                         rc = pthread_create(&thread_t, NULL,
 506                                             virtual_deinit_v2,
 507                                             (void *)&self->virtual_nci_fd);
 508                 else
 509                         rc = pthread_create(&thread_t, NULL, virtual_deinit,
 510                                             (void *)&self->virtual_nci_fd);
 511
 512                 ASSERT_GT(rc, -1);
 513                 rc = send_cmd_with_idx(self->sd, self->fid, self->pid,
 514                                        NFC_CMD_DEV_DOWN, self->dev_idex);
 515                 EXPECT_EQ(rc, 0);
 516
 517                 pthread_join(thread_t, (void **)&status);
 518                 ASSERT_EQ(status, 0);
 519         }
 520
 521         close(self->sd);
 522         close(self->virtual_nci_fd);
 523         self->open_state = false;
 524 }
 525
 526 TEST_F(NCI, init)
 527 {
 528         struct msgtemplate msg;
 529         int rc;
 530
 531         rc = get_nci_devid(self->sd, self->fid, self->pid, self->dev_idex,
 532                            &msg);
 533         ASSERT_EQ(rc, 0);
 534         EXPECT_EQ(get_dev_enable_state(&msg), 1);
 535 }
 536
 537 static void *virtual_poll_start(void *data)
 538 {
 539         char buf[258];
 540         int dev_fd;
 541         int len;
 542
 543         dev_fd = *(int *)data;
 544
 545         len = read(dev_fd, buf, 258);
 546         if (len <= 0)
 547                 goto error;
 548         if (len != sizeof(nci_rf_discovery_cmd))
 549                 goto error;
 550         if (memcmp(nci_rf_discovery_cmd, buf, len))
 551                 goto error;
 552         write(dev_fd, nci_rf_disc_rsp, sizeof(nci_rf_disc_rsp));
 553
 554         return (void *)0;
 555 error:
 556         return (void *)-1;
 557 }
 558
 559 static void *virtual_poll_stop(void *data)
 560 {
 561         char buf[258];
 562         int dev_fd;
 563         int len;
 564
 565         dev_fd = *(int *)data;
 566
 567         len = read(dev_fd, buf, 258);
 568         if (len <= 0)
 569                 goto error;
 570         if (len != sizeof(nci_rf_deact_cmd))
 571                 goto error;
 572         if (memcmp(nci_rf_deact_cmd, buf, len))
 573                 goto error;
 574         write(dev_fd, nci_rf_deact_rsp, sizeof(nci_rf_deact_rsp));
 575
 576         return (void *)0;
 577 error:
 578         return (void *)-1;
 579 }
 580
 581 int start_polling(int dev_idx, int proto, int virtual_fd, int sd, int fid, int pid)
 582 {
 583         __u16 nla_start_poll_type[2] = {NFC_ATTR_DEVICE_INDEX,
 584                                          NFC_ATTR_PROTOCOLS};
 585         void *nla_start_poll_data[2] = {&dev_idx, &proto};
 586         int nla_start_poll_len[2] = {4, 4};
 587         pthread_t thread_t;
 588         int status;
 589         int rc;
 590
 591         rc = pthread_create(&thread_t, NULL, virtual_poll_start,
 592                             (void *)&virtual_fd);
 593         if (rc < 0)
 594                 return rc;
 595
 596         rc = send_cmd_mt_nla(sd, fid, pid, NFC_CMD_START_POLL, 2, nla_start_poll_type,
 597                              nla_start_poll_data, nla_start_poll_len, NLM_F_REQUEST);
 598         if (rc != 0)
 599                 return rc;
 600
 601         pthread_join(thread_t, (void **)&status);
 602         return status;
 603 }
 604
 605 int stop_polling(int dev_idx, int virtual_fd, int sd, int fid, int pid)
 606 {
 607         pthread_t thread_t;
 608         int status;
 609         int rc;
 610
 611         rc = pthread_create(&thread_t, NULL, virtual_poll_stop,
 612                             (void *)&virtual_fd);
 613         if (rc < 0)
 614                 return rc;
 615
 616         rc = send_cmd_with_idx(sd, fid, pid,
 617                                NFC_CMD_STOP_POLL, dev_idx);
 618         if (rc != 0)
 619                 return rc;
 620
 621         pthread_join(thread_t, (void **)&status);
 622         return status;
 623 }
 624
 625 TEST_F(NCI, start_poll)
 626 {
 627         int status;
 628
 629         status = start_polling(self->dev_idex, self->proto, self->virtual_nci_fd,
 630                                self->sd, self->fid, self->pid);
 631         EXPECT_EQ(status, 0);
 632
 633         status = stop_polling(self->dev_idex, self->virtual_nci_fd, self->sd,
 634                               self->fid, self->pid);
 635         EXPECT_EQ(status, 0);
 636 }
 637
 638 int get_taginfo(int dev_idx, int sd, int fid, int pid)
 639 {
 640         struct {
 641                 struct nlmsghdr n;
 642                 struct genlmsghdr g;
 643                 char buf[512];
 644         } ans;
 645
 646         struct nlattr *na;
 647         __u32 protocol;
 648         int targetidx;
 649         __u8 sel_res;
 650         int resp_len;
 651         int len;
 652
 653         __u16 tagid_type;
 654         void *tagid_type_data;
 655         int tagid_len;
 656
 657         tagid_type = NFC_ATTR_DEVICE_INDEX;
 658         tagid_type_data = &dev_idx;
 659         tagid_len = 4;
 660
 661         send_cmd_mt_nla(sd, fid, pid, NFC_CMD_GET_TARGET, 1, &tagid_type,
 662                         &tagid_type_data, &tagid_len, NLM_F_REQUEST | NLM_F_DUMP);
 663         resp_len = recv(sd, &ans, sizeof(ans), 0);
 664         if (ans.n.nlmsg_type == NLMSG_ERROR || resp_len < 0 ||
 665             !NLMSG_OK(&ans.n, resp_len))
 666                 return -1;
 667
 668         resp_len = GENLMSG_PAYLOAD(&ans.n);
 669         na = (struct nlattr *)GENLMSG_DATA(&ans);
 670
 671         len = 0;
 672         targetidx = -1;
 673         protocol = -1;
 674         sel_res = -1;
 675
 676         while (len < resp_len) {
 677                 len += NLA_ALIGN(na->nla_len);
 678
 679                 if (na->nla_type == NFC_ATTR_TARGET_INDEX)
 680                         targetidx = *(int *)((char *)na + NLA_HDRLEN);
 681                 else if (na->nla_type == NFC_ATTR_TARGET_SEL_RES)
 682                         sel_res = *(__u8 *)((char *)na + NLA_HDRLEN);
 683                 else if (na->nla_type == NFC_ATTR_PROTOCOLS)
 684                         protocol = *(__u32 *)((char *)na + NLA_HDRLEN);
 685
 686                 na = (struct nlattr *)(GENLMSG_DATA(&ans) + len);
 687         }
 688
 689         if (targetidx == -1 || sel_res != 0x20 || protocol != NFC_PROTO_ISO14443_MASK)
 690                 return -1;
 691
 692         return targetidx;
 693 }
 694
 695 int connect_socket(int dev_idx, int target_idx)
 696 {
 697         struct sockaddr_nfc addr;
 698         int sock;
 699         int err = 0;
 700
 701         sock = socket(AF_NFC, SOCK_SEQPACKET, NFC_SOCKPROTO_RAW);
 702         if (sock == -1)
 703                 return -1;
 704
 705         addr.sa_family = AF_NFC;
 706         addr.dev_idx = dev_idx;
 707         addr.target_idx = target_idx;
 708         addr.nfc_protocol = NFC_PROTO_ISO14443;
 709
 710         err = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
 711         if (err) {
 712                 close(sock);
 713                 return -1;
 714         }
 715
 716         return sock;
 717 }
 718
 719 int connect_tag(int dev_idx, int virtual_fd, int sd, int fid, int pid)
 720 {
 721         struct genlmsghdr *genlhdr;
 722         struct nlattr *na;
 723         char evt_data[255];
 724         int target_idx;
 725         int resp_len;
 726         int evt_dev;
 727
 728         write(virtual_fd, nci_rf_activate_ntf, sizeof(nci_rf_activate_ntf));
 729         resp_len = recv(sd, evt_data, sizeof(evt_data), 0);
 730         if (resp_len < 0)
 731                 return -1;
 732
 733         genlhdr = (struct genlmsghdr *)((struct nlmsghdr *)evt_data + 1);
 734         na = (struct nlattr *)(genlhdr + 1);
 735         evt_dev = *(int *)((char *)na + NLA_HDRLEN);
 736         if (dev_idx != evt_dev)
 737                 return -1;
 738
 739         target_idx = get_taginfo(dev_idx, sd, fid, pid);
 740         if (target_idx == -1)
 741                 return -1;
 742         return connect_socket(dev_idx, target_idx);
 743 }
 744
 745 int read_write_nci_cmd(int nfc_sock, int virtual_fd, const __u8 *cmd, __u32 cmd_len,
 746                        const __u8 *rsp, __u32 rsp_len)
 747 {
 748         char buf[256];
 749         unsigned int len;
 750
 751         send(nfc_sock, &cmd[3], cmd_len - 3, 0);
 752         len = read(virtual_fd, buf, cmd_len);
 753         if (len < 0 || memcmp(buf, cmd, cmd_len))
 754                 return -1;
 755
 756         write(virtual_fd, rsp, rsp_len);
 757         len = recv(nfc_sock, buf, rsp_len - 2, 0);
 758         if (len < 0 || memcmp(&buf[1], &rsp[3], rsp_len - 3))
 759                 return -1;
 760
 761         return 0;
 762 }
 763
 764 int read_tag(int nfc_sock, int virtual_fd)
 765 {
 766         if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_select_cmd,
 767                                sizeof(nci_t4t_select_cmd), nci_t4t_rsp_ok,
 768                                sizeof(nci_t4t_rsp_ok)))
 769                 return -1;
 770
 771         if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_select_cmd2,
 772                                sizeof(nci_t4t_select_cmd2), nci_t4t_rsp_ok,
 773                                sizeof(nci_t4t_rsp_ok)))
 774                 return -1;
 775
 776         if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_read_cmd,
 777                                sizeof(nci_t4t_read_cmd), nci_t4t_read_rsp,
 778                                sizeof(nci_t4t_read_rsp)))
 779                 return -1;
 780
 781         if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_select_cmd3,
 782                                sizeof(nci_t4t_select_cmd3), nci_t4t_rsp_ok,
 783                                sizeof(nci_t4t_rsp_ok)))
 784                 return -1;
 785
 786         if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_read_cmd2,
 787                                sizeof(nci_t4t_read_cmd2), nci_t4t_read_rsp2,
 788                                sizeof(nci_t4t_read_rsp2)))
 789                 return -1;
 790
 791         return read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_read_cmd3,
 792                                   sizeof(nci_t4t_read_cmd3), nci_t4t_read_rsp3,
 793                                   sizeof(nci_t4t_read_rsp3));
 794 }
 795
 796 static void *virtual_deactivate_proc(void *data)
 797 {
 798         int virtual_fd;
 799         char buf[256];
 800         int deactcmd_len;
 801         int len;
 802
 803         virtual_fd = *(int *)data;
 804         deactcmd_len = sizeof(nci_rf_deact_cmd);
 805         len = read(virtual_fd, buf, deactcmd_len);
 806         if (len != deactcmd_len || memcmp(buf, nci_rf_deact_cmd, deactcmd_len))
 807                 return (void *)-1;
 808
 809         write(virtual_fd, nci_rf_deact_rsp, sizeof(nci_rf_deact_rsp));
 810         write(virtual_fd, nci_rf_deact_ntf, sizeof(nci_rf_deact_ntf));
 811
 812         return (void *)0;
 813 }
 814
 815 int disconnect_tag(int nfc_sock, int virtual_fd)
 816 {
 817         pthread_t thread_t;
 818         char buf[256];
 819         int status;
 820         int len;
 821
 822         send(nfc_sock, &nci_t4t_select_cmd3[3], sizeof(nci_t4t_select_cmd3) - 3, 0);
 823         len = read(virtual_fd, buf, sizeof(nci_t4t_select_cmd3));
 824         if (len < 0 || memcmp(buf, nci_t4t_select_cmd3, sizeof(nci_t4t_select_cmd3)))
 825                 return -1;
 826
 827         len = recv(nfc_sock, buf, sizeof(nci_t4t_rsp_ok), 0);
 828         if (len != -1)
 829                 return -1;
 830
 831         status = pthread_create(&thread_t, NULL, virtual_deactivate_proc,
 832                                 (void *)&virtual_fd);
 833
 834         close(nfc_sock);
 835         pthread_join(thread_t, (void **)&status);
 836         return status;
 837 }
 838
 839 TEST_F(NCI, t4t_tag_read)
 840 {
 841         int nfc_sock;
 842         int status;
 843
 844         status = start_polling(self->dev_idex, self->proto, self->virtual_nci_fd,
 845                                self->sd, self->fid, self->pid);
 846         EXPECT_EQ(status, 0);
 847
 848         nfc_sock = connect_tag(self->dev_idex, self->virtual_nci_fd, self->sd,
 849                                self->fid, self->pid);
 850         ASSERT_GT(nfc_sock, -1);
 851
 852         status = read_tag(nfc_sock, self->virtual_nci_fd);
 853         ASSERT_EQ(status, 0);
 854
 855         status = disconnect_tag(nfc_sock, self->virtual_nci_fd);
 856         EXPECT_EQ(status, 0);
 857 }
 858
 859 TEST_F(NCI, deinit)
 860 {
 861         struct msgtemplate msg;
 862         pthread_t thread_t;
 863         int status;
 864         int rc;
 865
 866         rc = get_nci_devid(self->sd, self->fid, self->pid, self->dev_idex,
 867                            &msg);
 868         ASSERT_EQ(rc, 0);
 869         EXPECT_EQ(get_dev_enable_state(&msg), 1);
 870
 871         if (self->isNCI2)
 872                 rc = pthread_create(&thread_t, NULL, virtual_deinit_v2,
 873                                     (void *)&self->virtual_nci_fd);
 874         else
 875                 rc = pthread_create(&thread_t, NULL, virtual_deinit,
 876                                     (void *)&self->virtual_nci_fd);
 877         ASSERT_GT(rc, -1);
 878
 879         rc = send_cmd_with_idx(self->sd, self->fid, self->pid,
 880                                NFC_CMD_DEV_DOWN, self->dev_idex);
 881         EXPECT_EQ(rc, 0);
 882
 883         pthread_join(thread_t, (void **)&status);
 884         self->open_state = 0;
 885         ASSERT_EQ(status, 0);
 886
 887         rc = get_nci_devid(self->sd, self->fid, self->pid, self->dev_idex,
 888                            &msg);
 889         ASSERT_EQ(rc, 0);
 890         EXPECT_EQ(get_dev_enable_state(&msg), 0);
 891 }
 892
 893 TEST_HARNESS_MAIN