From 8675569d73cab15e5127d1f228afd3317cbeb5bf Mon Sep 17 00:00:00 2001 From: Bongsu Jeon Date: Tue, 17 Aug 2021 06:28:11 -0700 Subject: [PATCH 1/8] nfc: virtual_ncidev: Use wait queue instead of polling In previous version, the user level virtual device application that used this driver should have the polling scheme to read a NCI frame. To remove this polling scheme, use Wait Queue. Signed-off-by: Bongsu Jeon Signed-off-by: David S. Miller --- drivers/nfc/virtual_ncidev.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/drivers/nfc/virtual_ncidev.c b/drivers/nfc/virtual_ncidev.c index 2ee0ec4bb7394..221fa3bb8705e 100644 --- a/drivers/nfc/virtual_ncidev.c +++ b/drivers/nfc/virtual_ncidev.c @@ -10,6 +10,7 @@ #include #include #include +#include #include enum virtual_ncidev_mode { @@ -27,6 +28,7 @@ enum virtual_ncidev_mode { NFC_PROTO_ISO15693_MASK) static enum virtual_ncidev_mode state; +static DECLARE_WAIT_QUEUE_HEAD(wq); static struct miscdevice miscdev; static struct sk_buff *send_buff; static struct nci_dev *ndev; @@ -61,6 +63,7 @@ static int virtual_nci_send(struct nci_dev *ndev, struct sk_buff *skb) } send_buff = skb_copy(skb, GFP_KERNEL); mutex_unlock(&nci_mutex); + wake_up_interruptible(&wq); return 0; } @@ -77,9 +80,11 @@ static ssize_t virtual_ncidev_read(struct file *file, char __user *buf, size_t actual_len; mutex_lock(&nci_mutex); - if (!send_buff) { + while (!send_buff) { mutex_unlock(&nci_mutex); - return 0; + if (wait_event_interruptible(wq, send_buff)) + return -EFAULT; + mutex_lock(&nci_mutex); } actual_len = min_t(size_t, count, send_buff->len); From 4ef956c643942ad93a420a7106413f3a86a1c84c Mon Sep 17 00:00:00 2001 From: Bongsu Jeon Date: Tue, 17 Aug 2021 06:28:12 -0700 Subject: [PATCH 2/8] selftests: nci: Remove the polling code to read a NCI frame Because the virtual NCI device uses Wait Queue, the virtual device application doesn't need to poll the NCI frame. Signed-off-by: Bongsu Jeon Reviewed-by: Krzysztof Kozlowski Signed-off-by: David S. Miller --- tools/testing/selftests/nci/nci_dev.c | 33 +++++++++------------------ 1 file changed, 11 insertions(+), 22 deletions(-) diff --git a/tools/testing/selftests/nci/nci_dev.c b/tools/testing/selftests/nci/nci_dev.c index 57b505cb15618..34e76c7fa1fee 100644 --- a/tools/testing/selftests/nci/nci_dev.c +++ b/tools/testing/selftests/nci/nci_dev.c @@ -270,8 +270,7 @@ static void *virtual_dev_open(void *data) dev_fd = *(int *)data; - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_reset_cmd)) @@ -280,8 +279,7 @@ static void *virtual_dev_open(void *data) goto error; write(dev_fd, nci_reset_rsp, sizeof(nci_reset_rsp)); - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_init_cmd)) @@ -290,8 +288,7 @@ static void *virtual_dev_open(void *data) goto error; write(dev_fd, nci_init_rsp, sizeof(nci_init_rsp)); - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_rf_disc_map_cmd)) @@ -313,8 +310,7 @@ static void *virtual_dev_open_v2(void *data) dev_fd = *(int *)data; - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_reset_cmd)) @@ -324,8 +320,7 @@ static void *virtual_dev_open_v2(void *data) write(dev_fd, nci_reset_rsp_v2, sizeof(nci_reset_rsp_v2)); write(dev_fd, nci_reset_ntf, sizeof(nci_reset_ntf)); - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_init_cmd_v2)) @@ -334,8 +329,7 @@ static void *virtual_dev_open_v2(void *data) goto error; write(dev_fd, nci_init_rsp_v2, sizeof(nci_init_rsp_v2)); - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_rf_disc_map_cmd)) @@ -402,8 +396,7 @@ static void *virtual_deinit(void *data) dev_fd = *(int *)data; - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_reset_cmd)) @@ -425,8 +418,7 @@ static void *virtual_deinit_v2(void *data) dev_fd = *(int *)data; - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_reset_cmd)) @@ -489,16 +481,14 @@ static void *virtual_poll_start(void *data) dev_fd = *(int *)data; - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_rf_discovery_cmd)) goto error; if (memcmp(nci_rf_discovery_cmd, buf, len)) goto error; - write(dev_fd, nci_rf_disc_rsp, sizeof(nci_rf_disc_rsp)) - ; + write(dev_fd, nci_rf_disc_rsp, sizeof(nci_rf_disc_rsp)); return (void *)0; error: @@ -513,8 +503,7 @@ static void *virtual_poll_stop(void *data) dev_fd = *(int *)data; - while ((len = read(dev_fd, buf, 258)) == 0) - ; + len = read(dev_fd, buf, 258); if (len <= 0) goto error; if (len != sizeof(nci_rf_deact_cmd)) From 366f6edf5dea88451720b0b1f3ea477ee6b3aa80 Mon Sep 17 00:00:00 2001 From: Bongsu Jeon Date: Tue, 17 Aug 2021 06:28:13 -0700 Subject: [PATCH 3/8] selftests: nci: Fix the typo Fix typo: rep_len -> resp_len Signed-off-by: Bongsu Jeon Signed-off-by: David S. Miller --- tools/testing/selftests/nci/nci_dev.c | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/tools/testing/selftests/nci/nci_dev.c b/tools/testing/selftests/nci/nci_dev.c index 34e76c7fa1fee..b4d85eeb5fd1d 100644 --- a/tools/testing/selftests/nci/nci_dev.c +++ b/tools/testing/selftests/nci/nci_dev.c @@ -158,7 +158,7 @@ static int get_family_id(int sd, __u32 pid) char buf[512]; } ans; struct nlattr *na; - int rep_len; + int resp_len; __u16 id; int rc; @@ -167,10 +167,10 @@ static int get_family_id(int sd, __u32 pid) if (rc < 0) return 0; - rep_len = recv(sd, &ans, sizeof(ans), 0); + resp_len = recv(sd, &ans, sizeof(ans), 0); - if (ans.n.nlmsg_type == NLMSG_ERROR || rep_len < 0 || - !NLMSG_OK(&ans.n, rep_len)) + if (ans.n.nlmsg_type == NLMSG_ERROR || resp_len < 0 || + !NLMSG_OK(&ans.n, resp_len)) return 0; na = (struct nlattr *)GENLMSG_DATA(&ans); @@ -194,7 +194,7 @@ static int send_cmd_with_idx(int sd, __u16 nlmsg_type, __u32 nlmsg_pid, static int get_nci_devid(int sd, __u16 fid, __u32 pid, int dev_id, struct msgtemplate *msg) { - int rc, rep_len; + int rc, resp_len; rc = send_cmd_with_idx(sd, fid, pid, NFC_CMD_GET_DEVICE, dev_id); if (rc < 0) { @@ -202,14 +202,14 @@ static int get_nci_devid(int sd, __u16 fid, __u32 pid, int dev_id, struct msgtem goto error; } - rep_len = recv(sd, msg, sizeof(*msg), 0); - if (rep_len < 0) { + resp_len = recv(sd, msg, sizeof(*msg), 0); + if (resp_len < 0) { rc = -2; goto error; } if (msg->n.nlmsg_type == NLMSG_ERROR || - !NLMSG_OK(&msg->n, rep_len)) { + !NLMSG_OK(&msg->n, resp_len)) { rc = -3; goto error; } @@ -222,21 +222,21 @@ static int get_nci_devid(int sd, __u16 fid, __u32 pid, int dev_id, struct msgtem static __u8 get_dev_enable_state(struct msgtemplate *msg) { struct nlattr *na; - int rep_len; + int resp_len; int len; - rep_len = GENLMSG_PAYLOAD(&msg->n); + resp_len = GENLMSG_PAYLOAD(&msg->n); na = (struct nlattr *)GENLMSG_DATA(msg); len = 0; - while (len < rep_len) { + while (len < resp_len) { len += NLA_ALIGN(na->nla_len); if (na->nla_type == NFC_ATTR_DEVICE_POWERED) return *(char *)NLA_DATA(na); na = (struct nlattr *)(GENLMSG_DATA(msg) + len); } - return rep_len; + return resp_len; } FIXTURE(NCI) { From 78a7b2a8a0fa31f63ac16ac13601db6ed8259dfc Mon Sep 17 00:00:00 2001 From: Bongsu Jeon Date: Tue, 17 Aug 2021 06:28:14 -0700 Subject: [PATCH 4/8] selftests: nci: Fix the code for next nlattr offset nlattr could have a padding for 4 bytes alignment. So next nla's offset should be calculated with a padding. Signed-off-by: Bongsu Jeon Signed-off-by: David S. Miller --- tools/testing/selftests/nci/nci_dev.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/testing/selftests/nci/nci_dev.c b/tools/testing/selftests/nci/nci_dev.c index b4d85eeb5fd1d..cf47505a6b357 100644 --- a/tools/testing/selftests/nci/nci_dev.c +++ b/tools/testing/selftests/nci/nci_dev.c @@ -113,8 +113,8 @@ static int send_cmd_mt_nla(int sd, __u16 nlmsg_type, __u32 nlmsg_pid, if (nla_len > 0) memcpy(NLA_DATA(na), nla_data[cnt], nla_len[cnt]); - msg.n.nlmsg_len += NLMSG_ALIGN(na->nla_len); - prv_len = na->nla_len; + prv_len = NLA_ALIGN(nla_len[cnt]) + NLA_HDRLEN; + msg.n.nlmsg_len += prv_len; } buf = (char *)&msg; From 1d5b8d01db98abb8c176838fad73287366874582 Mon Sep 17 00:00:00 2001 From: Bongsu Jeon Date: Tue, 17 Aug 2021 06:28:15 -0700 Subject: [PATCH 5/8] selftests: nci: Fix the wrong condition memcpy should be executed only in case nla_len's value is greater than 0. Signed-off-by: Bongsu Jeon Signed-off-by: David S. Miller --- tools/testing/selftests/nci/nci_dev.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/testing/selftests/nci/nci_dev.c b/tools/testing/selftests/nci/nci_dev.c index cf47505a6b357..a1786cef73bca 100644 --- a/tools/testing/selftests/nci/nci_dev.c +++ b/tools/testing/selftests/nci/nci_dev.c @@ -110,7 +110,7 @@ static int send_cmd_mt_nla(int sd, __u16 nlmsg_type, __u32 nlmsg_pid, na->nla_type = nla_type[cnt]; na->nla_len = nla_len[cnt] + NLA_HDRLEN; - if (nla_len > 0) + if (nla_len[cnt] > 0) memcpy(NLA_DATA(na), nla_data[cnt], nla_len[cnt]); prv_len = NLA_ALIGN(nla_len[cnt]) + NLA_HDRLEN; From 6ebbc9680a33e5e073b04d8a1f486bd9e1809515 Mon Sep 17 00:00:00 2001 From: Bongsu Jeon Date: Tue, 17 Aug 2021 06:28:16 -0700 Subject: [PATCH 6/8] selftests: nci: Add the flags parameter for the send_cmd_mt_nla To reuse the send_cmd_mt_nla for NLM_F_REQUEST and NLM_F_DUMP flag, add the flags parameter to the function. Signed-off-by: Bongsu Jeon Signed-off-by: David S. Miller --- tools/testing/selftests/nci/nci_dev.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/tools/testing/selftests/nci/nci_dev.c b/tools/testing/selftests/nci/nci_dev.c index a1786cef73bca..2b90379523c66 100644 --- a/tools/testing/selftests/nci/nci_dev.c +++ b/tools/testing/selftests/nci/nci_dev.c @@ -87,7 +87,7 @@ static int create_nl_socket(void) static int send_cmd_mt_nla(int sd, __u16 nlmsg_type, __u32 nlmsg_pid, __u8 genl_cmd, int nla_num, __u16 nla_type[], - void *nla_data[], int nla_len[]) + void *nla_data[], int nla_len[], __u16 flags) { struct sockaddr_nl nladdr; struct msgtemplate msg; @@ -98,7 +98,7 @@ static int send_cmd_mt_nla(int sd, __u16 nlmsg_type, __u32 nlmsg_pid, msg.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN); msg.n.nlmsg_type = nlmsg_type; - msg.n.nlmsg_flags = NLM_F_REQUEST; + msg.n.nlmsg_flags = flags; msg.n.nlmsg_seq = 0; msg.n.nlmsg_pid = nlmsg_pid; msg.g.cmd = genl_cmd; @@ -146,8 +146,8 @@ static int send_get_nfc_family(int sd, __u32 pid) nla_get_family_data = family_name; return send_cmd_mt_nla(sd, GENL_ID_CTRL, pid, CTRL_CMD_GETFAMILY, - 1, &nla_get_family_type, - &nla_get_family_data, &nla_get_family_len); + 1, &nla_get_family_type, &nla_get_family_data, + &nla_get_family_len, NLM_F_REQUEST); } static int get_family_id(int sd, __u32 pid) @@ -189,7 +189,7 @@ static int send_cmd_with_idx(int sd, __u16 nlmsg_type, __u32 nlmsg_pid, int nla_len = 4; return send_cmd_mt_nla(sd, nlmsg_type, nlmsg_pid, genl_cmd, 1, - &nla_type, &nla_data, &nla_len); + &nla_type, &nla_data, &nla_len, NLM_F_REQUEST); } static int get_nci_devid(int sd, __u16 fid, __u32 pid, int dev_id, struct msgtemplate *msg) @@ -531,9 +531,9 @@ TEST_F(NCI, start_poll) (void *)&self->virtual_nci_fd); ASSERT_GT(rc, -1); - rc = send_cmd_mt_nla(self->sd, self->fid, self->pid, - NFC_CMD_START_POLL, 2, nla_start_poll_type, - nla_start_poll_data, nla_start_poll_len); + rc = send_cmd_mt_nla(self->sd, self->fid, self->pid, NFC_CMD_START_POLL, 2, + nla_start_poll_type, nla_start_poll_data, + nla_start_poll_len, NLM_F_REQUEST); EXPECT_EQ(rc, 0); pthread_join(thread_t, (void **)&status); From 72696bd8a09d6dc46196b23cf42562ee7e169ebc Mon Sep 17 00:00:00 2001 From: Bongsu Jeon Date: Tue, 17 Aug 2021 06:28:17 -0700 Subject: [PATCH 7/8] selftests: nci: Extract the start/stop discovery function To reuse the start/stop discovery code in other testcase, extract the code. Signed-off-by: Bongsu Jeon Signed-off-by: David S. Miller --- tools/testing/selftests/nci/nci_dev.c | 53 +++++++++++++++++++-------- 1 file changed, 38 insertions(+), 15 deletions(-) diff --git a/tools/testing/selftests/nci/nci_dev.c b/tools/testing/selftests/nci/nci_dev.c index 2b90379523c66..a68b14642c205 100644 --- a/tools/testing/selftests/nci/nci_dev.c +++ b/tools/testing/selftests/nci/nci_dev.c @@ -517,38 +517,61 @@ static void *virtual_poll_stop(void *data) return (void *)-1; } -TEST_F(NCI, start_poll) +int start_polling(int dev_idx, int proto, int virtual_fd, int sd, int fid, int pid) { __u16 nla_start_poll_type[2] = {NFC_ATTR_DEVICE_INDEX, NFC_ATTR_PROTOCOLS}; - void *nla_start_poll_data[2] = {&self->dev_idex, &self->proto}; + void *nla_start_poll_data[2] = {&dev_idx, &proto}; int nla_start_poll_len[2] = {4, 4}; pthread_t thread_t; int status; int rc; rc = pthread_create(&thread_t, NULL, virtual_poll_start, - (void *)&self->virtual_nci_fd); - ASSERT_GT(rc, -1); + (void *)&virtual_fd); + if (rc < 0) + return rc; - rc = send_cmd_mt_nla(self->sd, self->fid, self->pid, NFC_CMD_START_POLL, 2, - nla_start_poll_type, nla_start_poll_data, - nla_start_poll_len, NLM_F_REQUEST); - EXPECT_EQ(rc, 0); + rc = send_cmd_mt_nla(sd, fid, pid, NFC_CMD_START_POLL, 2, nla_start_poll_type, + nla_start_poll_data, nla_start_poll_len, NLM_F_REQUEST); + if (rc != 0) + return rc; pthread_join(thread_t, (void **)&status); - ASSERT_EQ(status, 0); + return status; +} + +int stop_polling(int dev_idx, int virtual_fd, int sd, int fid, int pid) +{ + pthread_t thread_t; + int status; + int rc; rc = pthread_create(&thread_t, NULL, virtual_poll_stop, - (void *)&self->virtual_nci_fd); - ASSERT_GT(rc, -1); + (void *)&virtual_fd); + if (rc < 0) + return rc; - rc = send_cmd_with_idx(self->sd, self->fid, self->pid, - NFC_CMD_STOP_POLL, self->dev_idex); - EXPECT_EQ(rc, 0); + rc = send_cmd_with_idx(sd, fid, pid, + NFC_CMD_STOP_POLL, dev_idx); + if (rc != 0) + return rc; pthread_join(thread_t, (void **)&status); - ASSERT_EQ(status, 0); + return status; +} + +TEST_F(NCI, start_poll) +{ + int status; + + status = start_polling(self->dev_idex, self->proto, self->virtual_nci_fd, + self->sd, self->fid, self->pid); + EXPECT_EQ(status, 0); + + status = stop_polling(self->dev_idex, self->virtual_nci_fd, self->sd, + self->fid, self->pid); + EXPECT_EQ(status, 0); } TEST_F(NCI, deinit) From 61612511e55c886c96c0586abe3ac428402777df Mon Sep 17 00:00:00 2001 From: Bongsu Jeon Date: Tue, 17 Aug 2021 06:28:18 -0700 Subject: [PATCH 8/8] selftests: nci: Add the NCI testcase reading T4T Tag Add the NCI testcase reading T4T Tag that has NFC TEST in plain text. the virtual device application acts as T4T Tag in this testcase. Signed-off-by: Bongsu Jeon Signed-off-by: David S. Miller --- tools/testing/selftests/nci/nci_dev.c | 292 +++++++++++++++++++++++++- 1 file changed, 287 insertions(+), 5 deletions(-) diff --git a/tools/testing/selftests/nci/nci_dev.c b/tools/testing/selftests/nci/nci_dev.c index a68b14642c205..e1bf55dabdf65 100644 --- a/tools/testing/selftests/nci/nci_dev.c +++ b/tools/testing/selftests/nci/nci_dev.c @@ -57,6 +57,29 @@ const __u8 nci_init_rsp_v2[] = {0x40, 0x01, 0x1c, 0x00, 0x1a, 0x7e, 0x06, const __u8 nci_rf_disc_map_rsp[] = {0x41, 0x00, 0x01, 0x00}; const __u8 nci_rf_disc_rsp[] = {0x41, 0x03, 0x01, 0x00}; const __u8 nci_rf_deact_rsp[] = {0x41, 0x06, 0x01, 0x00}; +const __u8 nci_rf_deact_ntf[] = {0x61, 0x06, 0x02, 0x00, 0x00}; +const __u8 nci_rf_activate_ntf[] = {0x61, 0x05, 0x1D, 0x01, 0x02, 0x04, 0x00, + 0xFF, 0xFF, 0x0C, 0x44, 0x03, 0x07, 0x04, + 0x62, 0x26, 0x11, 0x80, 0x1D, 0x80, 0x01, + 0x20, 0x00, 0x00, 0x00, 0x06, 0x05, 0x75, + 0x77, 0x81, 0x02, 0x80}; +const __u8 nci_t4t_select_cmd[] = {0x00, 0x00, 0x0C, 0x00, 0xA4, 0x04, 0x00, + 0x07, 0xD2, 0x76, 0x00, 0x00, 0x85, 0x01, 0x01}; +const __u8 nci_t4t_select_cmd2[] = {0x00, 0x00, 0x07, 0x00, 0xA4, 0x00, 0x0C, 0x02, + 0xE1, 0x03}; +const __u8 nci_t4t_select_cmd3[] = {0x00, 0x00, 0x07, 0x00, 0xA4, 0x00, 0x0C, 0x02, + 0xE1, 0x04}; +const __u8 nci_t4t_read_cmd[] = {0x00, 0x00, 0x05, 0x00, 0xB0, 0x00, 0x00, 0x0F}; +const __u8 nci_t4t_read_rsp[] = {0x00, 0x00, 0x11, 0x00, 0x0F, 0x20, 0x00, 0x3B, + 0x00, 0x34, 0x04, 0x06, 0xE1, 0x04, 0x08, 0x00, + 0x00, 0x00, 0x90, 0x00}; +const __u8 nci_t4t_read_cmd2[] = {0x00, 0x00, 0x05, 0x00, 0xB0, 0x00, 0x00, 0x02}; +const __u8 nci_t4t_read_rsp2[] = {0x00, 0x00, 0x04, 0x00, 0x0F, 0x90, 0x00}; +const __u8 nci_t4t_read_cmd3[] = {0x00, 0x00, 0x05, 0x00, 0xB0, 0x00, 0x02, 0x0F}; +const __u8 nci_t4t_read_rsp3[] = {0x00, 0x00, 0x11, 0xD1, 0x01, 0x0B, 0x54, 0x02, + 0x65, 0x6E, 0x4E, 0x46, 0x43, 0x20, 0x54, 0x45, + 0x53, 0x54, 0x90, 0x00}; +const __u8 nci_t4t_rsp_ok[] = {0x00, 0x00, 0x02, 0x90, 0x00}; struct msgtemplate { struct nlmsghdr n; @@ -150,7 +173,7 @@ static int send_get_nfc_family(int sd, __u32 pid) &nla_get_family_len, NLM_F_REQUEST); } -static int get_family_id(int sd, __u32 pid) +static int get_family_id(int sd, __u32 pid, __u32 *event_group) { struct { struct nlmsghdr n; @@ -160,6 +183,7 @@ static int get_family_id(int sd, __u32 pid) struct nlattr *na; int resp_len; __u16 id; + int len; int rc; rc = send_get_nfc_family(sd, pid); @@ -173,11 +197,43 @@ static int get_family_id(int sd, __u32 pid) !NLMSG_OK(&ans.n, resp_len)) return 0; + len = 0; + resp_len = GENLMSG_PAYLOAD(&ans.n); na = (struct nlattr *)GENLMSG_DATA(&ans); - na = (struct nlattr *)((char *)na + NLA_ALIGN(na->nla_len)); - if (na->nla_type == CTRL_ATTR_FAMILY_ID) - id = *(__u16 *)NLA_DATA(na); + while (len < resp_len) { + len += NLA_ALIGN(na->nla_len); + if (na->nla_type == CTRL_ATTR_FAMILY_ID) { + id = *(__u16 *)NLA_DATA(na); + } else if (na->nla_type == CTRL_ATTR_MCAST_GROUPS) { + struct nlattr *nested_na; + struct nlattr *group_na; + int group_attr_len; + int group_attr; + + nested_na = (struct nlattr *)((char *)na + NLA_HDRLEN); + group_na = (struct nlattr *)((char *)nested_na + NLA_HDRLEN); + group_attr_len = 0; + + for (group_attr = CTRL_ATTR_MCAST_GRP_UNSPEC; + group_attr < CTRL_ATTR_MCAST_GRP_MAX; group_attr++) { + if (group_na->nla_type == CTRL_ATTR_MCAST_GRP_ID) { + *event_group = *(__u32 *)((char *)group_na + + NLA_HDRLEN); + break; + } + + group_attr_len += NLA_ALIGN(group_na->nla_len) + + NLA_HDRLEN; + if (group_attr_len >= nested_na->nla_len) + break; + + group_na = (struct nlattr *)((char *)group_na + + NLA_ALIGN(group_na->nla_len)); + } + } + na = (struct nlattr *)(GENLMSG_DATA(&ans) + len); + } return id; } @@ -347,6 +403,7 @@ FIXTURE_SETUP(NCI) { struct msgtemplate msg; pthread_t thread_t; + __u32 event_group; int status; int rc; @@ -358,12 +415,16 @@ FIXTURE_SETUP(NCI) ASSERT_NE(self->sd, -1); self->pid = getpid(); - self->fid = get_family_id(self->sd, self->pid); + self->fid = get_family_id(self->sd, self->pid, &event_group); ASSERT_NE(self->fid, -1); self->virtual_nci_fd = open("/dev/virtual_nci", O_RDWR); ASSERT_GT(self->virtual_nci_fd, -1); + rc = setsockopt(self->sd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &event_group, + sizeof(event_group)); + ASSERT_NE(rc, -1); + rc = ioctl(self->virtual_nci_fd, IOCTL_GET_NCIDEV_IDX, &self->dev_idex); ASSERT_EQ(rc, 0); @@ -574,6 +635,227 @@ TEST_F(NCI, start_poll) EXPECT_EQ(status, 0); } +int get_taginfo(int dev_idx, int sd, int fid, int pid) +{ + struct { + struct nlmsghdr n; + struct genlmsghdr g; + char buf[512]; + } ans; + + struct nlattr *na; + __u32 protocol; + int targetidx; + __u8 sel_res; + int resp_len; + int len; + + __u16 tagid_type; + void *tagid_type_data; + int tagid_len; + + tagid_type = NFC_ATTR_DEVICE_INDEX; + tagid_type_data = &dev_idx; + tagid_len = 4; + + send_cmd_mt_nla(sd, fid, pid, NFC_CMD_GET_TARGET, 1, &tagid_type, + &tagid_type_data, &tagid_len, NLM_F_REQUEST | NLM_F_DUMP); + resp_len = recv(sd, &ans, sizeof(ans), 0); + if (ans.n.nlmsg_type == NLMSG_ERROR || resp_len < 0 || + !NLMSG_OK(&ans.n, resp_len)) + return -1; + + resp_len = GENLMSG_PAYLOAD(&ans.n); + na = (struct nlattr *)GENLMSG_DATA(&ans); + + len = 0; + targetidx = -1; + protocol = -1; + sel_res = -1; + + while (len < resp_len) { + len += NLA_ALIGN(na->nla_len); + + if (na->nla_type == NFC_ATTR_TARGET_INDEX) + targetidx = *(int *)((char *)na + NLA_HDRLEN); + else if (na->nla_type == NFC_ATTR_TARGET_SEL_RES) + sel_res = *(__u8 *)((char *)na + NLA_HDRLEN); + else if (na->nla_type == NFC_ATTR_PROTOCOLS) + protocol = *(__u32 *)((char *)na + NLA_HDRLEN); + + na = (struct nlattr *)(GENLMSG_DATA(&ans) + len); + } + + if (targetidx == -1 || sel_res != 0x20 || protocol != NFC_PROTO_ISO14443_MASK) + return -1; + + return targetidx; +} + +int connect_socket(int dev_idx, int target_idx) +{ + struct sockaddr_nfc addr; + int sock; + int err = 0; + + sock = socket(AF_NFC, SOCK_SEQPACKET, NFC_SOCKPROTO_RAW); + if (sock == -1) + return -1; + + addr.sa_family = AF_NFC; + addr.dev_idx = dev_idx; + addr.target_idx = target_idx; + addr.nfc_protocol = NFC_PROTO_ISO14443; + + err = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); + if (err) { + close(sock); + return -1; + } + + return sock; +} + +int connect_tag(int dev_idx, int virtual_fd, int sd, int fid, int pid) +{ + struct genlmsghdr *genlhdr; + struct nlattr *na; + char evt_data[255]; + int target_idx; + int resp_len; + int evt_dev; + + write(virtual_fd, nci_rf_activate_ntf, sizeof(nci_rf_activate_ntf)); + resp_len = recv(sd, evt_data, sizeof(evt_data), 0); + if (resp_len < 0) + return -1; + + genlhdr = (struct genlmsghdr *)((struct nlmsghdr *)evt_data + 1); + na = (struct nlattr *)(genlhdr + 1); + evt_dev = *(int *)((char *)na + NLA_HDRLEN); + if (dev_idx != evt_dev) + return -1; + + target_idx = get_taginfo(dev_idx, sd, fid, pid); + if (target_idx == -1) + return -1; + return connect_socket(dev_idx, target_idx); +} + +int read_write_nci_cmd(int nfc_sock, int virtual_fd, const __u8 *cmd, __u32 cmd_len, + const __u8 *rsp, __u32 rsp_len) +{ + char buf[256]; + unsigned int len; + + send(nfc_sock, &cmd[3], cmd_len - 3, 0); + len = read(virtual_fd, buf, cmd_len); + if (len < 0 || memcmp(buf, cmd, cmd_len)) + return -1; + + write(virtual_fd, rsp, rsp_len); + len = recv(nfc_sock, buf, rsp_len - 2, 0); + if (len < 0 || memcmp(&buf[1], &rsp[3], rsp_len - 3)) + return -1; + + return 0; +} + +int read_tag(int nfc_sock, int virtual_fd) +{ + if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_select_cmd, + sizeof(nci_t4t_select_cmd), nci_t4t_rsp_ok, + sizeof(nci_t4t_rsp_ok))) + return -1; + + if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_select_cmd2, + sizeof(nci_t4t_select_cmd2), nci_t4t_rsp_ok, + sizeof(nci_t4t_rsp_ok))) + return -1; + + if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_read_cmd, + sizeof(nci_t4t_read_cmd), nci_t4t_read_rsp, + sizeof(nci_t4t_read_rsp))) + return -1; + + if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_select_cmd3, + sizeof(nci_t4t_select_cmd3), nci_t4t_rsp_ok, + sizeof(nci_t4t_rsp_ok))) + return -1; + + if (read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_read_cmd2, + sizeof(nci_t4t_read_cmd2), nci_t4t_read_rsp2, + sizeof(nci_t4t_read_rsp2))) + return -1; + + return read_write_nci_cmd(nfc_sock, virtual_fd, nci_t4t_read_cmd3, + sizeof(nci_t4t_read_cmd3), nci_t4t_read_rsp3, + sizeof(nci_t4t_read_rsp3)); +} + +static void *virtual_deactivate_proc(void *data) +{ + int virtual_fd; + char buf[256]; + int deactcmd_len; + int len; + + virtual_fd = *(int *)data; + deactcmd_len = sizeof(nci_rf_deact_cmd); + len = read(virtual_fd, buf, deactcmd_len); + if (len != deactcmd_len || memcmp(buf, nci_rf_deact_cmd, deactcmd_len)) + return (void *)-1; + + write(virtual_fd, nci_rf_deact_rsp, sizeof(nci_rf_deact_rsp)); + write(virtual_fd, nci_rf_deact_ntf, sizeof(nci_rf_deact_ntf)); + + return (void *)0; +} + +int disconnect_tag(int nfc_sock, int virtual_fd) +{ + pthread_t thread_t; + char buf[256]; + int status; + int len; + + send(nfc_sock, &nci_t4t_select_cmd3[3], sizeof(nci_t4t_select_cmd3) - 3, 0); + len = read(virtual_fd, buf, sizeof(nci_t4t_select_cmd3)); + if (len < 0 || memcmp(buf, nci_t4t_select_cmd3, sizeof(nci_t4t_select_cmd3))) + return -1; + + len = recv(nfc_sock, buf, sizeof(nci_t4t_rsp_ok), 0); + if (len != -1) + return -1; + + status = pthread_create(&thread_t, NULL, virtual_deactivate_proc, + (void *)&virtual_fd); + + close(nfc_sock); + pthread_join(thread_t, (void **)&status); + return status; +} + +TEST_F(NCI, t4t_tag_read) +{ + int nfc_sock; + int status; + + status = start_polling(self->dev_idex, self->proto, self->virtual_nci_fd, + self->sd, self->fid, self->pid); + EXPECT_EQ(status, 0); + + nfc_sock = connect_tag(self->dev_idex, self->virtual_nci_fd, self->sd, + self->fid, self->pid); + ASSERT_GT(nfc_sock, -1); + + status = read_tag(nfc_sock, self->virtual_nci_fd); + ASSERT_EQ(status, 0); + + status = disconnect_tag(nfc_sock, self->virtual_nci_fd); + EXPECT_EQ(status, 0); +} + TEST_F(NCI, deinit) { struct msgtemplate msg;