1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
|
/*
* NFC Digital Protocol stack
* Copyright (c) 2013, Intel Corporation.
*
* This program is free software; you can redistribute it and/or modify it
* under the terms and conditions of the GNU General Public License,
* version 2, as published by the Free Software Foundation.
*
* This program is distributed in the hope it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
*/
#include "digital.h"
#define DIGITAL_NFC_DEP_FRAME_DIR_OUT 0xD4
#define DIGITAL_NFC_DEP_FRAME_DIR_IN 0xD5
#define DIGITAL_NFC_DEP_NFCA_SOD_SB 0xF0
#define DIGITAL_CMD_ATR_REQ 0x00
#define DIGITAL_CMD_ATR_RES 0x01
#define DIGITAL_CMD_PSL_REQ 0x04
#define DIGITAL_CMD_PSL_RES 0x05
#define DIGITAL_CMD_DEP_REQ 0x06
#define DIGITAL_CMD_DEP_RES 0x07
#define DIGITAL_ATR_REQ_MIN_SIZE 16
#define DIGITAL_ATR_REQ_MAX_SIZE 64
#define DIGITAL_NFCID3_LEN ((u8)8)
#define DIGITAL_LR_BITS_PAYLOAD_SIZE_254B 0x30
#define DIGITAL_GB_BIT 0x02
#define DIGITAL_NFC_DEP_PFB_TYPE(pfb) ((pfb) & 0xE0)
#define DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT 0x10
#define DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb) \
((pfb) & DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT)
#define DIGITAL_NFC_DEP_MI_BIT_SET(pfb) ((pfb) & 0x10)
#define DIGITAL_NFC_DEP_NAD_BIT_SET(pfb) ((pfb) & 0x08)
#define DIGITAL_NFC_DEP_DID_BIT_SET(pfb) ((pfb) & 0x04)
#define DIGITAL_NFC_DEP_PFB_PNI(pfb) ((pfb) & 0x03)
#define DIGITAL_NFC_DEP_PFB_I_PDU 0x00
#define DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU 0x40
#define DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU 0x80
struct digital_atr_req {
u8 dir;
u8 cmd;
u8 nfcid3[10];
u8 did;
u8 bs;
u8 br;
u8 pp;
u8 gb[0];
} __packed;
struct digital_atr_res {
u8 dir;
u8 cmd;
u8 nfcid3[10];
u8 did;
u8 bs;
u8 br;
u8 to;
u8 pp;
u8 gb[0];
} __packed;
struct digital_psl_req {
u8 dir;
u8 cmd;
u8 did;
u8 brs;
u8 fsl;
} __packed;
struct digital_psl_res {
u8 dir;
u8 cmd;
u8 did;
} __packed;
struct digital_dep_req_res {
u8 dir;
u8 cmd;
u8 pfb;
} __packed;
static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp);
static void digital_skb_push_dep_sod(struct nfc_digital_dev *ddev,
struct sk_buff *skb)
{
skb_push(skb, sizeof(u8));
skb->data[0] = skb->len;
if (ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)
*skb_push(skb, sizeof(u8)) = DIGITAL_NFC_DEP_NFCA_SOD_SB;
}
static int digital_skb_pull_dep_sod(struct nfc_digital_dev *ddev,
struct sk_buff *skb)
{
u8 size;
if (skb->len < 2)
return -EIO;
if (ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)
skb_pull(skb, sizeof(u8));
size = skb->data[0];
if (size != skb->len)
return -EIO;
skb_pull(skb, sizeof(u8));
return 0;
}
static void digital_in_recv_atr_res(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp)
{
struct nfc_target *target = arg;
struct digital_atr_res *atr_res;
u8 gb_len;
int rc;
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
goto exit;
}
rc = ddev->skb_check_crc(resp);
if (rc) {
PROTOCOL_ERR("14.4.1.6");
goto exit;
}
rc = digital_skb_pull_dep_sod(ddev, resp);
if (rc) {
PROTOCOL_ERR("14.4.1.2");
goto exit;
}
if (resp->len < sizeof(struct digital_atr_res)) {
rc = -EIO;
goto exit;
}
gb_len = resp->len - sizeof(struct digital_atr_res);
atr_res = (struct digital_atr_res *)resp->data;
rc = nfc_set_remote_general_bytes(ddev->nfc_dev, atr_res->gb, gb_len);
if (rc)
goto exit;
rc = nfc_dep_link_is_up(ddev->nfc_dev, target->idx, NFC_COMM_ACTIVE,
NFC_RF_INITIATOR);
ddev->curr_nfc_dep_pni = 0;
exit:
dev_kfree_skb(resp);
if (rc)
ddev->curr_protocol = 0;
}
int digital_in_send_atr_req(struct nfc_digital_dev *ddev,
struct nfc_target *target, __u8 comm_mode, __u8 *gb,
size_t gb_len)
{
struct sk_buff *skb;
struct digital_atr_req *atr_req;
uint size;
size = DIGITAL_ATR_REQ_MIN_SIZE + gb_len;
if (size > DIGITAL_ATR_REQ_MAX_SIZE) {
PROTOCOL_ERR("14.6.1.1");
return -EINVAL;
}
skb = digital_skb_alloc(ddev, size);
if (!skb)
return -ENOMEM;
skb_put(skb, sizeof(struct digital_atr_req));
atr_req = (struct digital_atr_req *)skb->data;
memset(atr_req, 0, sizeof(struct digital_atr_req));
atr_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
atr_req->cmd = DIGITAL_CMD_ATR_REQ;
if (target->nfcid2_len)
memcpy(atr_req->nfcid3, target->nfcid2,
max(target->nfcid2_len, DIGITAL_NFCID3_LEN));
else
get_random_bytes(atr_req->nfcid3, DIGITAL_NFCID3_LEN);
atr_req->did = 0;
atr_req->bs = 0;
atr_req->br = 0;
atr_req->pp = DIGITAL_LR_BITS_PAYLOAD_SIZE_254B;
if (gb_len) {
atr_req->pp |= DIGITAL_GB_BIT;
memcpy(skb_put(skb, gb_len), gb, gb_len);
}
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
digital_in_send_cmd(ddev, skb, 500, digital_in_recv_atr_res, target);
return 0;
}
static int digital_in_send_rtox(struct nfc_digital_dev *ddev,
struct digital_data_exch *data_exch, u8 rtox)
{
struct digital_dep_req_res *dep_req;
struct sk_buff *skb;
int rc;
skb = digital_skb_alloc(ddev, 1);
if (!skb)
return -ENOMEM;
*skb_put(skb, 1) = rtox;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_req = (struct digital_dep_req_res *)skb->data;
dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
dep_req->cmd = DIGITAL_CMD_DEP_REQ;
dep_req->pfb = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU |
DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT;
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
data_exch);
return rc;
}
static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp)
{
struct digital_data_exch *data_exch = arg;
struct digital_dep_req_res *dep_res;
u8 pfb;
uint size;
int rc;
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
goto exit;
}
rc = ddev->skb_check_crc(resp);
if (rc) {
PROTOCOL_ERR("14.4.1.6");
goto error;
}
rc = digital_skb_pull_dep_sod(ddev, resp);
if (rc) {
PROTOCOL_ERR("14.4.1.2");
goto exit;
}
dep_res = (struct digital_dep_req_res *)resp->data;
if (resp->len < sizeof(struct digital_dep_req_res) ||
dep_res->dir != DIGITAL_NFC_DEP_FRAME_DIR_IN ||
dep_res->cmd != DIGITAL_CMD_DEP_RES) {
rc = -EIO;
goto error;
}
pfb = dep_res->pfb;
switch (DIGITAL_NFC_DEP_PFB_TYPE(pfb)) {
case DIGITAL_NFC_DEP_PFB_I_PDU:
if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) {
PROTOCOL_ERR("14.12.3.3");
rc = -EIO;
goto error;
}
ddev->curr_nfc_dep_pni =
DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
rc = 0;
break;
case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
PR_ERR("Received a ACK/NACK PDU");
rc = -EIO;
goto error;
case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
if (!DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) {
rc = -EINVAL;
goto error;
}
rc = digital_in_send_rtox(ddev, data_exch, resp->data[3]);
if (rc)
goto error;
kfree_skb(resp);
return;
}
if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb)) {
PR_ERR("MI bit set. Chained PDU not supported.");
rc = -EIO;
goto error;
}
size = sizeof(struct digital_dep_req_res);
if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb))
size++;
if (size > resp->len) {
rc = -EIO;
goto error;
}
skb_pull(resp, size);
exit:
data_exch->cb(data_exch->cb_context, resp, rc);
error:
kfree(data_exch);
if (rc)
kfree_skb(resp);
}
int digital_in_send_dep_req(struct nfc_digital_dev *ddev,
struct nfc_target *target, struct sk_buff *skb,
struct digital_data_exch *data_exch)
{
struct digital_dep_req_res *dep_req;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_req = (struct digital_dep_req_res *)skb->data;
dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
dep_req->cmd = DIGITAL_CMD_DEP_REQ;
dep_req->pfb = ddev->curr_nfc_dep_pni;
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
return digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
data_exch);
}
|
