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 61 struct msgtemplate { 62 struct nlmsghdr n; 63 struct genlmsghdr g; 64 char buf[MAX_MSG_SIZE]; 65 }; 66 67 static int create_nl_socket(void) 68 { 69 int fd; 70 struct sockaddr_nl local; 71 72 fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_GENERIC); 73 if (fd < 0) 74 return -1; 75 76 memset(&local, 0, sizeof(local)); 77 local.nl_family = AF_NETLINK; 78 79 if (bind(fd, (struct sockaddr *)&local, sizeof(local)) < 0) 80 goto error; 81 82 return fd; 83 error: 84 close(fd); 85 return -1; 86 } 87 88 static int send_cmd_mt_nla(int sd, __u16 nlmsg_type, __u32 nlmsg_pid, 89 __u8 genl_cmd, int nla_num, __u16 nla_type[], 90 void *nla_data[], int nla_len[]) 91 { 92 struct sockaddr_nl nladdr; 93 struct msgtemplate msg; 94 struct nlattr *na; 95 int cnt, prv_len; 96 int r, buflen; 97 char *buf; 98 99 msg.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN); 100 msg.n.nlmsg_type = nlmsg_type; 101 msg.n.nlmsg_flags = NLM_F_REQUEST; 102 msg.n.nlmsg_seq = 0; 103 msg.n.nlmsg_pid = nlmsg_pid; 104 msg.g.cmd = genl_cmd; 105 msg.g.version = 0x1; 106 107 prv_len = 0; 108 for (cnt = 0; cnt < nla_num; cnt++) { 109 na = (struct nlattr *)(GENLMSG_DATA(&msg) + prv_len); 110 na->nla_type = nla_type[cnt]; 111 na->nla_len = nla_len[cnt] + NLA_HDRLEN; 112 113 if (nla_len > 0) 114 memcpy(NLA_DATA(na), nla_data[cnt], nla_len[cnt]); 115 116 msg.n.nlmsg_len += NLMSG_ALIGN(na->nla_len); 117 prv_len = na->nla_len; 118 } 119 120 buf = (char *)&msg; 121 buflen = msg.n.nlmsg_len; 122 memset(&nladdr, 0, sizeof(nladdr)); 123 nladdr.nl_family = AF_NETLINK; 124 125 while ((r = sendto(sd, buf, buflen, 0, (struct sockaddr *)&nladdr, 126 sizeof(nladdr))) < buflen) { 127 if (r > 0) { 128 buf += r; 129 buflen -= r; 130 } else if (errno != EAGAIN) { 131 return -1; 132 } 133 } 134 return 0; 135 } 136 137 static int send_get_nfc_family(int sd, __u32 pid) 138 { 139 __u16 nla_get_family_type = CTRL_ATTR_FAMILY_NAME; 140 void *nla_get_family_data; 141 int nla_get_family_len; 142 char family_name[100]; 143 144 nla_get_family_len = strlen(NFC_GENL_NAME) + 1; 145 strcpy(family_name, NFC_GENL_NAME); 146 nla_get_family_data = family_name; 147 148 return send_cmd_mt_nla(sd, GENL_ID_CTRL, pid, CTRL_CMD_GETFAMILY, 149 1, &nla_get_family_type, 150 &nla_get_family_data, &nla_get_family_len); 151 } 152 153 static int get_family_id(int sd, __u32 pid) 154 { 155 struct { 156 struct nlmsghdr n; 157 struct genlmsghdr g; 158 char buf[512]; 159 } ans; 160 struct nlattr *na; 161 int rep_len; 162 __u16 id; 163 int rc; 164 165 rc = send_get_nfc_family(sd, pid); 166 167 if (rc < 0) 168 return 0; 169 170 rep_len = recv(sd, &ans, sizeof(ans), 0); 171 172 if (ans.n.nlmsg_type == NLMSG_ERROR || rep_len < 0 || 173 !NLMSG_OK(&ans.n, rep_len)) 174 return 0; 175 176 na = (struct nlattr *)GENLMSG_DATA(&ans); 177 na = (struct nlattr *)((char *)na + NLA_ALIGN(na->nla_len)); 178 if (na->nla_type == CTRL_ATTR_FAMILY_ID) 179 id = *(__u16 *)NLA_DATA(na); 180 181 return id; 182 } 183 184 static int send_cmd_with_idx(int sd, __u16 nlmsg_type, __u32 nlmsg_pid, 185 __u8 genl_cmd, int dev_id) 186 { 187 __u16 nla_type = NFC_ATTR_DEVICE_INDEX; 188 void *nla_data = &dev_id; 189 int nla_len = 4; 190 191 return send_cmd_mt_nla(sd, nlmsg_type, nlmsg_pid, genl_cmd, 1, 192 &nla_type, &nla_data, &nla_len); 193 } 194 195 static int get_nci_devid(int sd, __u16 fid, __u32 pid, int dev_id, struct msgtemplate *msg) 196 { 197 int rc, rep_len; 198 199 rc = send_cmd_with_idx(sd, fid, pid, NFC_CMD_GET_DEVICE, dev_id); 200 if (rc < 0) { 201 rc = -1; 202 goto error; 203 } 204 205 rep_len = recv(sd, msg, sizeof(*msg), 0); 206 if (rep_len < 0) { 207 rc = -2; 208 goto error; 209 } 210 211 if (msg->n.nlmsg_type == NLMSG_ERROR || 212 !NLMSG_OK(&msg->n, rep_len)) { 213 rc = -3; 214 goto error; 215 } 216 217 return 0; 218 error: 219 return rc; 220 } 221 222 static __u8 get_dev_enable_state(struct msgtemplate *msg) 223 { 224 struct nlattr *na; 225 int rep_len; 226 int len; 227 228 rep_len = GENLMSG_PAYLOAD(&msg->n); 229 na = (struct nlattr *)GENLMSG_DATA(msg); 230 len = 0; 231 232 while (len < rep_len) { 233 len += NLA_ALIGN(na->nla_len); 234 if (na->nla_type == NFC_ATTR_DEVICE_POWERED) 235 return *(char *)NLA_DATA(na); 236 na = (struct nlattr *)(GENLMSG_DATA(msg) + len); 237 } 238 239 return rep_len; 240 } 241 242 FIXTURE(NCI) { 243 int virtual_nci_fd; 244 bool open_state; 245 int dev_idex; 246 bool isNCI2; 247 int proto; 248 __u32 pid; 249 __u16 fid; 250 int sd; 251 }; 252 253 FIXTURE_VARIANT(NCI) { 254 bool isNCI2; 255 }; 256 257 FIXTURE_VARIANT_ADD(NCI, NCI1_0) { 258 .isNCI2 = false, 259 }; 260 261 FIXTURE_VARIANT_ADD(NCI, NCI2_0) { 262 .isNCI2 = true, 263 }; 264 265 static void *virtual_dev_open(void *data) 266 { 267 char buf[258]; 268 int dev_fd; 269 int len; 270 271 dev_fd = *(int *)data; 272 273 while ((len = read(dev_fd, buf, 258)) == 0) 274 ; 275 if (len <= 0) 276 goto error; 277 if (len != sizeof(nci_reset_cmd)) 278 goto error; 279 if (memcmp(nci_reset_cmd, buf, len)) 280 goto error; 281 write(dev_fd, nci_reset_rsp, sizeof(nci_reset_rsp)); 282 283 while ((len = read(dev_fd, buf, 258)) == 0) 284 ; 285 if (len <= 0) 286 goto error; 287 if (len != sizeof(nci_init_cmd)) 288 goto error; 289 if (memcmp(nci_init_cmd, buf, len)) 290 goto error; 291 write(dev_fd, nci_init_rsp, sizeof(nci_init_rsp)); 292 293 while ((len = read(dev_fd, buf, 258)) == 0) 294 ; 295 if (len <= 0) 296 goto error; 297 if (len != sizeof(nci_rf_disc_map_cmd)) 298 goto error; 299 if (memcmp(nci_rf_disc_map_cmd, buf, len)) 300 goto error; 301 write(dev_fd, nci_rf_disc_map_rsp, sizeof(nci_rf_disc_map_rsp)); 302 303 return (void *)0; 304 error: 305 return (void *)-1; 306 } 307 308 static void *virtual_dev_open_v2(void *data) 309 { 310 char buf[258]; 311 int dev_fd; 312 int len; 313 314 dev_fd = *(int *)data; 315 316 while ((len = read(dev_fd, buf, 258)) == 0) 317 ; 318 if (len <= 0) 319 goto error; 320 if (len != sizeof(nci_reset_cmd)) 321 goto error; 322 if (memcmp(nci_reset_cmd, buf, len)) 323 goto error; 324 write(dev_fd, nci_reset_rsp_v2, sizeof(nci_reset_rsp_v2)); 325 write(dev_fd, nci_reset_ntf, sizeof(nci_reset_ntf)); 326 327 while ((len = read(dev_fd, buf, 258)) == 0) 328 ; 329 if (len <= 0) 330 goto error; 331 if (len != sizeof(nci_init_cmd_v2)) 332 goto error; 333 if (memcmp(nci_init_cmd_v2, buf, len)) 334 goto error; 335 write(dev_fd, nci_init_rsp_v2, sizeof(nci_init_rsp_v2)); 336 337 while ((len = read(dev_fd, buf, 258)) == 0) 338 ; 339 if (len <= 0) 340 goto error; 341 if (len != sizeof(nci_rf_disc_map_cmd)) 342 goto error; 343 if (memcmp(nci_rf_disc_map_cmd, buf, len)) 344 goto error; 345 write(dev_fd, nci_rf_disc_map_rsp, sizeof(nci_rf_disc_map_rsp)); 346 347 return (void *)0; 348 error: 349 return (void *)-1; 350 } 351 352 FIXTURE_SETUP(NCI) 353 { 354 struct msgtemplate msg; 355 pthread_t thread_t; 356 int status; 357 int rc; 358 359 self->open_state = false; 360 self->proto = VIRTUAL_NFC_PROTOCOLS; 361 self->isNCI2 = variant->isNCI2; 362 363 self->sd = create_nl_socket(); 364 ASSERT_NE(self->sd, -1); 365 366 self->pid = getpid(); 367 self->fid = get_family_id(self->sd, self->pid); 368 ASSERT_NE(self->fid, -1); 369 370 self->virtual_nci_fd = open("/dev/virtual_nci", O_RDWR); 371 ASSERT_GT(self->virtual_nci_fd, -1); 372 373 rc = ioctl(self->virtual_nci_fd, IOCTL_GET_NCIDEV_IDX, &self->dev_idex); 374 ASSERT_EQ(rc, 0); 375 376 rc = get_nci_devid(self->sd, self->fid, self->pid, self->dev_idex, &msg); 377 ASSERT_EQ(rc, 0); 378 EXPECT_EQ(get_dev_enable_state(&msg), 0); 379 380 if (self->isNCI2) 381 rc = pthread_create(&thread_t, NULL, virtual_dev_open_v2, 382 (void *)&self->virtual_nci_fd); 383 else 384 rc = pthread_create(&thread_t, NULL, virtual_dev_open, 385 (void *)&self->virtual_nci_fd); 386 ASSERT_GT(rc, -1); 387 388 rc = send_cmd_with_idx(self->sd, self->fid, self->pid, 389 NFC_CMD_DEV_UP, self->dev_idex); 390 EXPECT_EQ(rc, 0); 391 392 pthread_join(thread_t, (void **)&status); 393 ASSERT_EQ(status, 0); 394 self->open_state = true; 395 } 396 397 static void *virtual_deinit(void *data) 398 { 399 char buf[258]; 400 int dev_fd; 401 int len; 402 403 dev_fd = *(int *)data; 404 405 while ((len = read(dev_fd, buf, 258)) == 0) 406 ; 407 if (len <= 0) 408 goto error; 409 if (len != sizeof(nci_reset_cmd)) 410 goto error; 411 if (memcmp(nci_reset_cmd, buf, len)) 412 goto error; 413 write(dev_fd, nci_reset_rsp, sizeof(nci_reset_rsp)); 414 415 return (void *)0; 416 error: 417 return (void *)-1; 418 } 419 420 static void *virtual_deinit_v2(void *data) 421 { 422 char buf[258]; 423 int dev_fd; 424 int len; 425 426 dev_fd = *(int *)data; 427 428 while ((len = read(dev_fd, buf, 258)) == 0) 429 ; 430 if (len <= 0) 431 goto error; 432 if (len != sizeof(nci_reset_cmd)) 433 goto error; 434 if (memcmp(nci_reset_cmd, buf, len)) 435 goto error; 436 write(dev_fd, nci_reset_rsp_v2, sizeof(nci_reset_rsp_v2)); 437 write(dev_fd, nci_reset_ntf, sizeof(nci_reset_ntf)); 438 439 return (void *)0; 440 error: 441 return (void *)-1; 442 } 443 444 FIXTURE_TEARDOWN(NCI) 445 { 446 pthread_t thread_t; 447 int status; 448 int rc; 449 450 if (self->open_state) { 451 if (self->isNCI2) 452 rc = pthread_create(&thread_t, NULL, 453 virtual_deinit_v2, 454 (void *)&self->virtual_nci_fd); 455 else 456 rc = pthread_create(&thread_t, NULL, virtual_deinit, 457 (void *)&self->virtual_nci_fd); 458 459 ASSERT_GT(rc, -1); 460 rc = send_cmd_with_idx(self->sd, self->fid, self->pid, 461 NFC_CMD_DEV_DOWN, self->dev_idex); 462 EXPECT_EQ(rc, 0); 463 464 pthread_join(thread_t, (void **)&status); 465 ASSERT_EQ(status, 0); 466 } 467 468 close(self->sd); 469 close(self->virtual_nci_fd); 470 self->open_state = false; 471 } 472 473 TEST_F(NCI, init) 474 { 475 struct msgtemplate msg; 476 int rc; 477 478 rc = get_nci_devid(self->sd, self->fid, self->pid, self->dev_idex, 479 &msg); 480 ASSERT_EQ(rc, 0); 481 EXPECT_EQ(get_dev_enable_state(&msg), 1); 482 } 483 484 static void *virtual_poll_start(void *data) 485 { 486 char buf[258]; 487 int dev_fd; 488 int len; 489 490 dev_fd = *(int *)data; 491 492 while ((len = read(dev_fd, buf, 258)) == 0) 493 ; 494 if (len <= 0) 495 goto error; 496 if (len != sizeof(nci_rf_discovery_cmd)) 497 goto error; 498 if (memcmp(nci_rf_discovery_cmd, buf, len)) 499 goto error; 500 write(dev_fd, nci_rf_disc_rsp, sizeof(nci_rf_disc_rsp)) 501 ; 502 503 return (void *)0; 504 error: 505 return (void *)-1; 506 } 507 508 static void *virtual_poll_stop(void *data) 509 { 510 char buf[258]; 511 int dev_fd; 512 int len; 513 514 dev_fd = *(int *)data; 515 516 while ((len = read(dev_fd, buf, 258)) == 0) 517 ; 518 if (len <= 0) 519 goto error; 520 if (len != sizeof(nci_rf_deact_cmd)) 521 goto error; 522 if (memcmp(nci_rf_deact_cmd, buf, len)) 523 goto error; 524 write(dev_fd, nci_rf_deact_rsp, sizeof(nci_rf_deact_rsp)); 525 526 return (void *)0; 527 error: 528 return (void *)-1; 529 } 530 531 TEST_F(NCI, start_poll) 532 { 533 __u16 nla_start_poll_type[2] = {NFC_ATTR_DEVICE_INDEX, 534 NFC_ATTR_PROTOCOLS}; 535 void *nla_start_poll_data[2] = {&self->dev_idex, &self->proto}; 536 int nla_start_poll_len[2] = {4, 4}; 537 pthread_t thread_t; 538 int status; 539 int rc; 540 541 rc = pthread_create(&thread_t, NULL, virtual_poll_start, 542 (void *)&self->virtual_nci_fd); 543 ASSERT_GT(rc, -1); 544 545 rc = send_cmd_mt_nla(self->sd, self->fid, self->pid, 546 NFC_CMD_START_POLL, 2, nla_start_poll_type, 547 nla_start_poll_data, nla_start_poll_len); 548 EXPECT_EQ(rc, 0); 549 550 pthread_join(thread_t, (void **)&status); 551 ASSERT_EQ(status, 0); 552 553 rc = pthread_create(&thread_t, NULL, virtual_poll_stop, 554 (void *)&self->virtual_nci_fd); 555 ASSERT_GT(rc, -1); 556 557 rc = send_cmd_with_idx(self->sd, self->fid, self->pid, 558 NFC_CMD_STOP_POLL, self->dev_idex); 559 EXPECT_EQ(rc, 0); 560 561 pthread_join(thread_t, (void **)&status); 562 ASSERT_EQ(status, 0); 563 } 564 565 TEST_F(NCI, deinit) 566 { 567 struct msgtemplate msg; 568 pthread_t thread_t; 569 int status; 570 int rc; 571 572 rc = get_nci_devid(self->sd, self->fid, self->pid, self->dev_idex, 573 &msg); 574 ASSERT_EQ(rc, 0); 575 EXPECT_EQ(get_dev_enable_state(&msg), 1); 576 577 if (self->isNCI2) 578 rc = pthread_create(&thread_t, NULL, virtual_deinit_v2, 579 (void *)&self->virtual_nci_fd); 580 else 581 rc = pthread_create(&thread_t, NULL, virtual_deinit, 582 (void *)&self->virtual_nci_fd); 583 ASSERT_GT(rc, -1); 584 585 rc = send_cmd_with_idx(self->sd, self->fid, self->pid, 586 NFC_CMD_DEV_DOWN, self->dev_idex); 587 EXPECT_EQ(rc, 0); 588 589 pthread_join(thread_t, (void **)&status); 590 self->open_state = 0; 591 ASSERT_EQ(status, 0); 592 593 rc = get_nci_devid(self->sd, self->fid, self->pid, self->dev_idex, 594 &msg); 595 ASSERT_EQ(rc, 0); 596 EXPECT_EQ(get_dev_enable_state(&msg), 0); 597 } 598 599 TEST_HARNESS_MAIN 600