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 
create_nl_socket(void)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 
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[],__u16 flags)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 
send_get_nfc_family(int sd,__u32 pid)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 
get_family_id(int sd,__u32 pid,__u32 * event_group)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 
send_cmd_with_idx(int sd,__u16 nlmsg_type,__u32 nlmsg_pid,__u8 genl_cmd,int dev_id)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 
get_nci_devid(int sd,__u16 fid,__u32 pid,int dev_id,struct msgtemplate * msg)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 
get_dev_enable_state(struct msgtemplate * msg)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 
FIXTURE(NCI)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 
FIXTURE_VARIANT(NCI)309 FIXTURE_VARIANT(NCI) {
310 	bool isNCI2;
311 };
312 
FIXTURE_VARIANT_ADD(NCI,NCI1_0)313 FIXTURE_VARIANT_ADD(NCI, NCI1_0) {
314 	.isNCI2 = false,
315 };
316 
FIXTURE_VARIANT_ADD(NCI,NCI2_0)317 FIXTURE_VARIANT_ADD(NCI, NCI2_0) {
318 	.isNCI2 = true,
319 };
320 
virtual_dev_open(void * data)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 
virtual_dev_open_v2(void * data)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 
FIXTURE_SETUP(NCI)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 
virtual_deinit(void * data)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 
virtual_deinit_v2(void * data)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 
FIXTURE_TEARDOWN(NCI)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 
TEST_F(NCI,init)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 
virtual_poll_start(void * data)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 
virtual_poll_stop(void * data)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 
start_polling(int dev_idx,int proto,int virtual_fd,int sd,int fid,int pid)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 
stop_polling(int dev_idx,int virtual_fd,int sd,int fid,int pid)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 
TEST_F(NCI,start_poll)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 
get_taginfo(int dev_idx,int sd,int fid,int pid)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 
connect_socket(int dev_idx,int target_idx)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 
connect_tag(int dev_idx,int virtual_fd,int sd,int fid,int pid)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 
read_write_nci_cmd(int nfc_sock,int virtual_fd,const __u8 * cmd,__u32 cmd_len,const __u8 * rsp,__u32 rsp_len)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 	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 
read_tag(int nfc_sock,int virtual_fd)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 
virtual_deactivate_proc(void * data)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 
disconnect_tag(int nfc_sock,int virtual_fd)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 
TEST_F(NCI,t4t_tag_read)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 
TEST_F(NCI,deinit)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
894