tjh.dev
Linux kernel mirror (for testing)
git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
kernel
os
linux
NFC: SHDLC implementation
Most NFC HCI chipsets actually use a simplified HDLC link layer to
carry HCI payloads.
This implementation registers itself as an HCI device on behalf of the
NFC driver.
Signed-off-by: Eric Lapuyade <eric.lapuyade@intel.com>
Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>
authored by
Eric Lapuyade
and committed by
John W. Linville
eb738fe5
8b8d2e08
+1058
+104
include/net/nfc/shdlc.h
+104
include/net/nfc/shdlc.h
···
1
+
/*
2
+
* Copyright (C) 2012 Intel Corporation. All rights reserved.
3
+
*
4
+
* This program is free software; you can redistribute it and/or modify
5
+
* it under the terms of the GNU General Public License as published by
6
+
* the Free Software Foundation; either version 2 of the License, or
7
+
* (at your option) any later version.
8
+
*
9
+
* This program is distributed in the hope that it will be useful,
10
+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11
+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12
+
* GNU General Public License for more details.
13
+
*
14
+
* You should have received a copy of the GNU General Public License
15
+
* along with this program; if not, write to the
16
+
* Free Software Foundation, Inc.,
17
+
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
18
+
*/
19
+
20
+
#ifndef __NFC_SHDLC_H
21
+
#define __NFC_SHDLC_H
22
+
23
+
struct nfc_shdlc;
24
+
25
+
struct nfc_shdlc_ops {
26
+
int (*open) (struct nfc_shdlc *shdlc);
27
+
void (*close) (struct nfc_shdlc *shdlc);
28
+
int (*hci_ready) (struct nfc_shdlc *shdlc);
29
+
int (*xmit) (struct nfc_shdlc *shdlc, struct sk_buff *skb);
30
+
int (*start_poll) (struct nfc_shdlc *shdlc, u32 protocols);
31
+
int (*target_from_gate) (struct nfc_shdlc *shdlc, u8 gate,
32
+
struct nfc_target *target);
33
+
int (*complete_target_discovered) (struct nfc_shdlc *shdlc, u8 gate,
34
+
struct nfc_target *target);
35
+
int (*data_exchange) (struct nfc_shdlc *shdlc,
36
+
struct nfc_target *target,
37
+
struct sk_buff *skb, struct sk_buff **res_skb);
38
+
};
39
+
40
+
enum shdlc_state {
41
+
SHDLC_DISCONNECTED = 0,
42
+
SHDLC_CONNECTING = 1,
43
+
SHDLC_NEGOCIATING = 2,
44
+
SHDLC_CONNECTED = 3
45
+
};
46
+
47
+
struct nfc_shdlc {
48
+
struct mutex state_mutex;
49
+
enum shdlc_state state;
50
+
int hard_fault;
51
+
52
+
struct nfc_hci_dev *hdev;
53
+
54
+
wait_queue_head_t *connect_wq;
55
+
int connect_tries;
56
+
int connect_result;
57
+
struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */
58
+
59
+
u8 w; /* window size */
60
+
bool srej_support;
61
+
62
+
struct timer_list t1_timer; /* send ack timeout */
63
+
bool t1_active;
64
+
65
+
struct timer_list t2_timer; /* guard/retransmit timeout */
66
+
bool t2_active;
67
+
68
+
int ns; /* next seq num for send */
69
+
int nr; /* next expected seq num for receive */
70
+
int dnr; /* oldest sent unacked seq num */
71
+
72
+
struct sk_buff_head rcv_q;
73
+
74
+
struct sk_buff_head send_q;
75
+
bool rnr; /* other side is not ready to receive */
76
+
77
+
struct sk_buff_head ack_pending_q;
78
+
79
+
struct workqueue_struct *sm_wq;
80
+
struct work_struct sm_work;
81
+
82
+
struct nfc_shdlc_ops *ops;
83
+
84
+
int client_headroom;
85
+
int client_tailroom;
86
+
87
+
void *clientdata;
88
+
};
89
+
90
+
void nfc_shdlc_recv_frame(struct nfc_shdlc *shdlc, struct sk_buff *skb);
91
+
92
+
struct nfc_shdlc *nfc_shdlc_allocate(struct nfc_shdlc_ops *ops,
93
+
struct nfc_hci_init_data *init_data,
94
+
u32 protocols,
95
+
int tx_headroom, int tx_tailroom,
96
+
int max_link_payload, const char *devname);
97
+
98
+
void nfc_shdlc_free(struct nfc_shdlc *shdlc);
99
+
100
+
void nfc_shdlc_set_clientdata(struct nfc_shdlc *shdlc, void *clientdata);
101
+
void *nfc_shdlc_get_clientdata(struct nfc_shdlc *shdlc);
102
+
struct nfc_hci_dev *nfc_shdlc_get_hci_dev(struct nfc_shdlc *shdlc);
103
+
104
+
#endif /* __NFC_SHDLC_H */
+8
net/nfc/hci/Kconfig
+8
net/nfc/hci/Kconfig
···
6
6
Say Y here if you want to build support for a kernel NFC HCI
7
7
implementation. This is mostly needed for devices that only process
8
8
HCI frames, like for example the NXP pn544.
9
+
10
+
config NFC_SHDLC
11
+
depends on NFC_HCI
12
+
bool "SHDLC link layer for HCI based NFC drivers"
13
+
default n
14
+
---help---
15
+
Say yes if you use an NFC HCI driver that requires SHDLC link layer.
16
+
If unsure, say N here.
+1
net/nfc/hci/Makefile
+1
net/nfc/hci/Makefile
+945
net/nfc/hci/shdlc.c
+945
net/nfc/hci/shdlc.c
···
1
+
/*
2
+
* Copyright (C) 2012 Intel Corporation. All rights reserved.
3
+
*
4
+
* This program is free software; you can redistribute it and/or modify
5
+
* it under the terms of the GNU General Public License as published by
6
+
* the Free Software Foundation; either version 2 of the License, or
7
+
* (at your option) any later version.
8
+
*
9
+
* This program is distributed in the hope that it will be useful,
10
+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11
+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12
+
* GNU General Public License for more details.
13
+
*
14
+
* You should have received a copy of the GNU General Public License
15
+
* along with this program; if not, write to the
16
+
* Free Software Foundation, Inc.,
17
+
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
18
+
*/
19
+
20
+
#define pr_fmt(fmt) "shdlc: %s: " fmt, __func__
21
+
22
+
#include <linux/sched.h>
23
+
#include <linux/export.h>
24
+
#include <linux/wait.h>
25
+
#include <linux/crc-ccitt.h>
26
+
#include <linux/slab.h>
27
+
#include <linux/skbuff.h>
28
+
29
+
#include <net/nfc/hci.h>
30
+
#include <net/nfc/shdlc.h>
31
+
32
+
#define SHDLC_LLC_HEAD_ROOM 2
33
+
#define SHDLC_LLC_TAIL_ROOM 2
34
+
35
+
#define SHDLC_MAX_WINDOW 4
36
+
#define SHDLC_SREJ_SUPPORT false
37
+
38
+
#define SHDLC_CONTROL_HEAD_MASK 0xe0
39
+
#define SHDLC_CONTROL_HEAD_I 0x80
40
+
#define SHDLC_CONTROL_HEAD_I2 0xa0
41
+
#define SHDLC_CONTROL_HEAD_S 0xc0
42
+
#define SHDLC_CONTROL_HEAD_U 0xe0
43
+
44
+
#define SHDLC_CONTROL_NS_MASK 0x38
45
+
#define SHDLC_CONTROL_NR_MASK 0x07
46
+
#define SHDLC_CONTROL_TYPE_MASK 0x18
47
+
48
+
#define SHDLC_CONTROL_M_MASK 0x1f
49
+
50
+
enum sframe_type {
51
+
S_FRAME_RR = 0x00,
52
+
S_FRAME_REJ = 0x01,
53
+
S_FRAME_RNR = 0x02,
54
+
S_FRAME_SREJ = 0x03
55
+
};
56
+
57
+
enum uframe_modifier {
58
+
U_FRAME_UA = 0x06,
59
+
U_FRAME_RSET = 0x19
60
+
};
61
+
62
+
#define SHDLC_CONNECT_VALUE_MS 5
63
+
#define SHDLC_T1_VALUE_MS(w) ((5 * w) / 4)
64
+
#define SHDLC_T2_VALUE_MS 300
65
+
66
+
#define SHDLC_DUMP_SKB(info, skb) \
67
+
do { \
68
+
pr_debug("%s:\n", info); \
69
+
print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \
70
+
16, 1, skb->data, skb->len, 0); \
71
+
} while (0)
72
+
73
+
/* checks x < y <= z modulo 8 */
74
+
static bool nfc_shdlc_x_lt_y_lteq_z(int x, int y, int z)
75
+
{
76
+
if (x < z)
77
+
return ((x < y) && (y <= z)) ? true : false;
78
+
else
79
+
return ((y > x) || (y <= z)) ? true : false;
80
+
}
81
+
82
+
/* checks x <= y < z modulo 8 */
83
+
static bool nfc_shdlc_x_lteq_y_lt_z(int x, int y, int z)
84
+
{
85
+
if (x <= z)
86
+
return ((x <= y) && (y < z)) ? true : false;
87
+
else /* x > z -> z+8 > x */
88
+
return ((y >= x) || (y < z)) ? true : false;
89
+
}
90
+
91
+
static struct sk_buff *nfc_shdlc_alloc_skb(struct nfc_shdlc *shdlc,
92
+
int payload_len)
93
+
{
94
+
struct sk_buff *skb;
95
+
96
+
skb = alloc_skb(shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM +
97
+
shdlc->client_tailroom + SHDLC_LLC_TAIL_ROOM +
98
+
payload_len, GFP_KERNEL);
99
+
if (skb)
100
+
skb_reserve(skb, shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM);
101
+
102
+
return skb;
103
+
}
104
+
105
+
static void nfc_shdlc_add_len_crc(struct sk_buff *skb)
106
+
{
107
+
u16 crc;
108
+
int len;
109
+
110
+
len = skb->len + 2;
111
+
*skb_push(skb, 1) = len;
112
+
113
+
crc = crc_ccitt(0xffff, skb->data, skb->len);
114
+
crc = ~crc;
115
+
*skb_put(skb, 1) = crc & 0xff;
116
+
*skb_put(skb, 1) = crc >> 8;
117
+
}
118
+
119
+
/* immediately sends an S frame. */
120
+
static int nfc_shdlc_send_s_frame(struct nfc_shdlc *shdlc,
121
+
enum sframe_type sframe_type, int nr)
122
+
{
123
+
int r;
124
+
struct sk_buff *skb;
125
+
126
+
pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr);
127
+
128
+
skb = nfc_shdlc_alloc_skb(shdlc, 0);
129
+
if (skb == NULL)
130
+
return -ENOMEM;
131
+
132
+
*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr;
133
+
134
+
nfc_shdlc_add_len_crc(skb);
135
+
136
+
r = shdlc->ops->xmit(shdlc, skb);
137
+
138
+
kfree_skb(skb);
139
+
140
+
return r;
141
+
}
142
+
143
+
/* immediately sends an U frame. skb may contain optional payload */
144
+
static int nfc_shdlc_send_u_frame(struct nfc_shdlc *shdlc,
145
+
struct sk_buff *skb,
146
+
enum uframe_modifier uframe_modifier)
147
+
{
148
+
int r;
149
+
150
+
pr_debug("uframe_modifier=%d\n", uframe_modifier);
151
+
152
+
*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier;
153
+
154
+
nfc_shdlc_add_len_crc(skb);
155
+
156
+
r = shdlc->ops->xmit(shdlc, skb);
157
+
158
+
kfree_skb(skb);
159
+
160
+
return r;
161
+
}
162
+
163
+
/*
164
+
* Free ack_pending frames until y_nr - 1, and reset t2 according to
165
+
* the remaining oldest ack_pending frame sent time
166
+
*/
167
+
static void nfc_shdlc_reset_t2(struct nfc_shdlc *shdlc, int y_nr)
168
+
{
169
+
struct sk_buff *skb;
170
+
int dnr = shdlc->dnr; /* MUST initially be < y_nr */
171
+
172
+
pr_debug("release ack pending up to frame %d excluded\n", y_nr);
173
+
174
+
while (dnr != y_nr) {
175
+
pr_debug("release ack pending frame %d\n", dnr);
176
+
177
+
skb = skb_dequeue(&shdlc->ack_pending_q);
178
+
kfree_skb(skb);
179
+
180
+
dnr = (dnr + 1) % 8;
181
+
}
182
+
183
+
if (skb_queue_empty(&shdlc->ack_pending_q)) {
184
+
if (shdlc->t2_active) {
185
+
del_timer_sync(&shdlc->t2_timer);
186
+
shdlc->t2_active = false;
187
+
188
+
pr_debug
189
+
("All sent frames acked. Stopped T2(retransmit)\n");
190
+
}
191
+
} else {
192
+
skb = skb_peek(&shdlc->ack_pending_q);
193
+
194
+
mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
195
+
msecs_to_jiffies(SHDLC_T2_VALUE_MS));
196
+
shdlc->t2_active = true;
197
+
198
+
pr_debug
199
+
("Start T2(retransmit) for remaining unacked sent frames\n");
200
+
}
201
+
}
202
+
203
+
/*
204
+
* Receive validated frames from lower layer. skb contains HCI payload only.
205
+
* Handle according to algorithm at spec:10.8.2
206
+
*/
207
+
static void nfc_shdlc_rcv_i_frame(struct nfc_shdlc *shdlc,
208
+
struct sk_buff *skb, int ns, int nr)
209
+
{
210
+
int x_ns = ns;
211
+
int y_nr = nr;
212
+
213
+
pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
214
+
215
+
if (shdlc->state != SHDLC_CONNECTED)
216
+
goto exit;
217
+
218
+
if (x_ns != shdlc->nr) {
219
+
nfc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
220
+
goto exit;
221
+
}
222
+
223
+
if (shdlc->t1_active == false) {
224
+
shdlc->t1_active = true;
225
+
mod_timer(&shdlc->t1_timer,
226
+
msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));
227
+
pr_debug("(re)Start T1(send ack)\n");
228
+
}
229
+
230
+
if (skb->len) {
231
+
nfc_hci_recv_frame(shdlc->hdev, skb);
232
+
skb = NULL;
233
+
}
234
+
235
+
shdlc->nr = (shdlc->nr + 1) % 8;
236
+
237
+
if (nfc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
238
+
nfc_shdlc_reset_t2(shdlc, y_nr);
239
+
240
+
shdlc->dnr = y_nr;
241
+
}
242
+
243
+
exit:
244
+
if (skb)
245
+
kfree_skb(skb);
246
+
}
247
+
248
+
static void nfc_shdlc_rcv_ack(struct nfc_shdlc *shdlc, int y_nr)
249
+
{
250
+
pr_debug("remote acked up to frame %d excluded\n", y_nr);
251
+
252
+
if (nfc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
253
+
nfc_shdlc_reset_t2(shdlc, y_nr);
254
+
shdlc->dnr = y_nr;
255
+
}
256
+
}
257
+
258
+
static void nfc_shdlc_requeue_ack_pending(struct nfc_shdlc *shdlc)
259
+
{
260
+
struct sk_buff *skb;
261
+
262
+
pr_debug("ns reset to %d\n", shdlc->dnr);
263
+
264
+
while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {
265
+
skb_pull(skb, 2); /* remove len+control */
266
+
skb_trim(skb, skb->len - 2); /* remove crc */
267
+
skb_queue_head(&shdlc->send_q, skb);
268
+
}
269
+
shdlc->ns = shdlc->dnr;
270
+
}
271
+
272
+
static void nfc_shdlc_rcv_rej(struct nfc_shdlc *shdlc, int y_nr)
273
+
{
274
+
struct sk_buff *skb;
275
+
276
+
pr_debug("remote asks retransmition from frame %d\n", y_nr);
277
+
278
+
if (nfc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {
279
+
if (shdlc->t2_active) {
280
+
del_timer_sync(&shdlc->t2_timer);
281
+
shdlc->t2_active = false;
282
+
pr_debug("Stopped T2(retransmit)\n");
283
+
}
284
+
285
+
if (shdlc->dnr != y_nr) {
286
+
while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
287
+
skb = skb_dequeue(&shdlc->ack_pending_q);
288
+
kfree_skb(skb);
289
+
}
290
+
}
291
+
292
+
nfc_shdlc_requeue_ack_pending(shdlc);
293
+
}
294
+
}
295
+
296
+
/* See spec RR:10.8.3 REJ:10.8.4 */
297
+
static void nfc_shdlc_rcv_s_frame(struct nfc_shdlc *shdlc,
298
+
enum sframe_type s_frame_type, int nr)
299
+
{
300
+
struct sk_buff *skb;
301
+
302
+
if (shdlc->state != SHDLC_CONNECTED)
303
+
return;
304
+
305
+
switch (s_frame_type) {
306
+
case S_FRAME_RR:
307
+
nfc_shdlc_rcv_ack(shdlc, nr);
308
+
if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */
309
+
shdlc->rnr = false;
310
+
if (shdlc->send_q.qlen == 0) {
311
+
skb = nfc_shdlc_alloc_skb(shdlc, 0);
312
+
if (skb)
313
+
skb_queue_tail(&shdlc->send_q, skb);
314
+
}
315
+
}
316
+
break;
317
+
case S_FRAME_REJ:
318
+
nfc_shdlc_rcv_rej(shdlc, nr);
319
+
break;
320
+
case S_FRAME_RNR:
321
+
nfc_shdlc_rcv_ack(shdlc, nr);
322
+
shdlc->rnr = true;
323
+
break;
324
+
default:
325
+
break;
326
+
}
327
+
}
328
+
329
+
static void nfc_shdlc_connect_complete(struct nfc_shdlc *shdlc, int r)
330
+
{
331
+
pr_debug("result=%d\n", r);
332
+
333
+
del_timer_sync(&shdlc->connect_timer);
334
+
335
+
if (r == 0) {
336
+
shdlc->ns = 0;
337
+
shdlc->nr = 0;
338
+
shdlc->dnr = 0;
339
+
340
+
shdlc->state = SHDLC_CONNECTED;
341
+
} else {
342
+
shdlc->state = SHDLC_DISCONNECTED;
343
+
344
+
/*
345
+
* TODO: Could it be possible that there are pending
346
+
* executing commands that are waiting for connect to complete
347
+
* before they can be carried? As connect is a blocking
348
+
* operation, it would require that the userspace process can
349
+
* send commands on the same device from a second thread before
350
+
* the device is up. I don't think that is possible, is it?
351
+
*/
352
+
}
353
+
354
+
shdlc->connect_result = r;
355
+
356
+
wake_up(shdlc->connect_wq);
357
+
}
358
+
359
+
static int nfc_shdlc_connect_initiate(struct nfc_shdlc *shdlc)
360
+
{
361
+
struct sk_buff *skb;
362
+
363
+
pr_debug("\n");
364
+
365
+
skb = nfc_shdlc_alloc_skb(shdlc, 2);
366
+
if (skb == NULL)
367
+
return -ENOMEM;
368
+
369
+
*skb_put(skb, 1) = SHDLC_MAX_WINDOW;
370
+
*skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0;
371
+
372
+
return nfc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
373
+
}
374
+
375
+
static int nfc_shdlc_connect_send_ua(struct nfc_shdlc *shdlc)
376
+
{
377
+
struct sk_buff *skb;
378
+
379
+
pr_debug("\n");
380
+
381
+
skb = nfc_shdlc_alloc_skb(shdlc, 0);
382
+
if (skb == NULL)
383
+
return -ENOMEM;
384
+
385
+
return nfc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
386
+
}
387
+
388
+
static void nfc_shdlc_rcv_u_frame(struct nfc_shdlc *shdlc,
389
+
struct sk_buff *skb,
390
+
enum uframe_modifier u_frame_modifier)
391
+
{
392
+
u8 w = SHDLC_MAX_WINDOW;
393
+
bool srej_support = SHDLC_SREJ_SUPPORT;
394
+
int r;
395
+
396
+
pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
397
+
398
+
switch (u_frame_modifier) {
399
+
case U_FRAME_RSET:
400
+
if (shdlc->state == SHDLC_NEGOCIATING) {
401
+
/* we sent RSET, but chip wants to negociate */
402
+
if (skb->len > 0)
403
+
w = skb->data[0];
404
+
405
+
if (skb->len > 1)
406
+
srej_support = skb->data[1] & 0x01 ? true :
407
+
false;
408
+
409
+
if ((w <= SHDLC_MAX_WINDOW) &&
410
+
(SHDLC_SREJ_SUPPORT || (srej_support == false))) {
411
+
shdlc->w = w;
412
+
shdlc->srej_support = srej_support;
413
+
r = nfc_shdlc_connect_send_ua(shdlc);
414
+
nfc_shdlc_connect_complete(shdlc, r);
415
+
}
416
+
} else if (shdlc->state > SHDLC_NEGOCIATING) {
417
+
/*
418
+
* TODO: Chip wants to reset link
419
+
* send ua, empty skb lists, reset counters
420
+
* propagate info to HCI layer
421
+
*/
422
+
}
423
+
break;
424
+
case U_FRAME_UA:
425
+
if ((shdlc->state == SHDLC_CONNECTING &&
426
+
shdlc->connect_tries > 0) ||
427
+
(shdlc->state == SHDLC_NEGOCIATING))
428
+
nfc_shdlc_connect_complete(shdlc, 0);
429
+
break;
430
+
default:
431
+
break;
432
+
}
433
+
434
+
kfree_skb(skb);
435
+
}
436
+
437
+
static void nfc_shdlc_handle_rcv_queue(struct nfc_shdlc *shdlc)
438
+
{
439
+
struct sk_buff *skb;
440
+
u8 control;
441
+
int nr;
442
+
int ns;
443
+
enum sframe_type s_frame_type;
444
+
enum uframe_modifier u_frame_modifier;
445
+
446
+
if (shdlc->rcv_q.qlen)
447
+
pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
448
+
449
+
while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
450
+
control = skb->data[0];
451
+
skb_pull(skb, 1);
452
+
switch (control & SHDLC_CONTROL_HEAD_MASK) {
453
+
case SHDLC_CONTROL_HEAD_I:
454
+
case SHDLC_CONTROL_HEAD_I2:
455
+
ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
456
+
nr = control & SHDLC_CONTROL_NR_MASK;
457
+
nfc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
458
+
break;
459
+
case SHDLC_CONTROL_HEAD_S:
460
+
s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
461
+
nr = control & SHDLC_CONTROL_NR_MASK;
462
+
nfc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
463
+
kfree_skb(skb);
464
+
break;
465
+
case SHDLC_CONTROL_HEAD_U:
466
+
u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
467
+
nfc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
468
+
break;
469
+
default:
470
+
pr_err("UNKNOWN Control=%d\n", control);
471
+
kfree_skb(skb);
472
+
break;
473
+
}
474
+
}
475
+
}
476
+
477
+
static int nfc_shdlc_w_used(int ns, int dnr)
478
+
{
479
+
int unack_count;
480
+
481
+
if (dnr <= ns)
482
+
unack_count = ns - dnr;
483
+
else
484
+
unack_count = 8 - dnr + ns;
485
+
486
+
return unack_count;
487
+
}
488
+
489
+
/* Send frames according to algorithm at spec:10.8.1 */
490
+
static void nfc_shdlc_handle_send_queue(struct nfc_shdlc *shdlc)
491
+
{
492
+
struct sk_buff *skb;
493
+
int r;
494
+
unsigned long time_sent;
495
+
496
+
if (shdlc->send_q.qlen)
497
+
pr_debug
498
+
("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
499
+
shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
500
+
shdlc->rnr == false ? "false" : "true",
501
+
shdlc->w - nfc_shdlc_w_used(shdlc->ns, shdlc->dnr),
502
+
shdlc->ack_pending_q.qlen);
503
+
504
+
while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
505
+
(shdlc->rnr == false)) {
506
+
507
+
if (shdlc->t1_active) {
508
+
del_timer_sync(&shdlc->t1_timer);
509
+
shdlc->t1_active = false;
510
+
pr_debug("Stopped T1(send ack)\n");
511
+
}
512
+
513
+
skb = skb_dequeue(&shdlc->send_q);
514
+
515
+
*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
516
+
shdlc->nr;
517
+
518
+
pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
519
+
shdlc->nr);
520
+
/* SHDLC_DUMP_SKB("shdlc frame written", skb); */
521
+
522
+
nfc_shdlc_add_len_crc(skb);
523
+
524
+
r = shdlc->ops->xmit(shdlc, skb);
525
+
if (r < 0) {
526
+
/*
527
+
* TODO: Cannot send, shdlc machine is dead, we
528
+
* must propagate the information up to HCI.
529
+
*/
530
+
shdlc->hard_fault = r;
531
+
break;
532
+
}
533
+
534
+
shdlc->ns = (shdlc->ns + 1) % 8;
535
+
536
+
time_sent = jiffies;
537
+
*(unsigned long *)skb->cb = time_sent;
538
+
539
+
skb_queue_tail(&shdlc->ack_pending_q, skb);
540
+
541
+
if (shdlc->t2_active == false) {
542
+
shdlc->t2_active = true;
543
+
mod_timer(&shdlc->t2_timer, time_sent +
544
+
msecs_to_jiffies(SHDLC_T2_VALUE_MS));
545
+
pr_debug("Started T2 (retransmit)\n");
546
+
}
547
+
}
548
+
}
549
+
550
+
static void nfc_shdlc_connect_timeout(unsigned long data)
551
+
{
552
+
struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data;
553
+
554
+
pr_debug("\n");
555
+
556
+
queue_work(shdlc->sm_wq, &shdlc->sm_work);
557
+
}
558
+
559
+
static void nfc_shdlc_t1_timeout(unsigned long data)
560
+
{
561
+
struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data;
562
+
563
+
pr_debug("SoftIRQ: need to send ack\n");
564
+
565
+
queue_work(shdlc->sm_wq, &shdlc->sm_work);
566
+
}
567
+
568
+
static void nfc_shdlc_t2_timeout(unsigned long data)
569
+
{
570
+
struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data;
571
+
572
+
pr_debug("SoftIRQ: need to retransmit\n");
573
+
574
+
queue_work(shdlc->sm_wq, &shdlc->sm_work);
575
+
}
576
+
577
+
static void nfc_shdlc_sm_work(struct work_struct *work)
578
+
{
579
+
struct nfc_shdlc *shdlc = container_of(work, struct nfc_shdlc, sm_work);
580
+
int r;
581
+
582
+
pr_debug("\n");
583
+
584
+
mutex_lock(&shdlc->state_mutex);
585
+
586
+
switch (shdlc->state) {
587
+
case SHDLC_DISCONNECTED:
588
+
skb_queue_purge(&shdlc->rcv_q);
589
+
skb_queue_purge(&shdlc->send_q);
590
+
skb_queue_purge(&shdlc->ack_pending_q);
591
+
break;
592
+
case SHDLC_CONNECTING:
593
+
if (shdlc->connect_tries++ < 5)
594
+
r = nfc_shdlc_connect_initiate(shdlc);
595
+
else
596
+
r = -ETIME;
597
+
if (r < 0)
598
+
nfc_shdlc_connect_complete(shdlc, r);
599
+
else {
600
+
mod_timer(&shdlc->connect_timer, jiffies +
601
+
msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
602
+
603
+
shdlc->state = SHDLC_NEGOCIATING;
604
+
}
605
+
break;
606
+
case SHDLC_NEGOCIATING:
607
+
if (timer_pending(&shdlc->connect_timer) == 0) {
608
+
shdlc->state = SHDLC_CONNECTING;
609
+
queue_work(shdlc->sm_wq, &shdlc->sm_work);
610
+
}
611
+
612
+
nfc_shdlc_handle_rcv_queue(shdlc);
613
+
break;
614
+
case SHDLC_CONNECTED:
615
+
nfc_shdlc_handle_rcv_queue(shdlc);
616
+
nfc_shdlc_handle_send_queue(shdlc);
617
+
618
+
if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
619
+
pr_debug
620
+
("Handle T1(send ack) elapsed (T1 now inactive)\n");
621
+
622
+
shdlc->t1_active = false;
623
+
r = nfc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
624
+
shdlc->nr);
625
+
if (r < 0)
626
+
shdlc->hard_fault = r;
627
+
}
628
+
629
+
if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
630
+
pr_debug
631
+
("Handle T2(retransmit) elapsed (T2 inactive)\n");
632
+
633
+
shdlc->t2_active = false;
634
+
635
+
nfc_shdlc_requeue_ack_pending(shdlc);
636
+
nfc_shdlc_handle_send_queue(shdlc);
637
+
}
638
+
639
+
if (shdlc->hard_fault) {
640
+
/*
641
+
* TODO: Handle hard_fault that occured during
642
+
* this invocation of the shdlc worker
643
+
*/
644
+
}
645
+
break;
646
+
default:
647
+
break;
648
+
}
649
+
mutex_unlock(&shdlc->state_mutex);
650
+
}
651
+
652
+
/*
653
+
* Called from syscall context to establish shdlc link. Sleeps until
654
+
* link is ready or failure.
655
+
*/
656
+
static int nfc_shdlc_connect(struct nfc_shdlc *shdlc)
657
+
{
658
+
DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
659
+
660
+
pr_debug("\n");
661
+
662
+
mutex_lock(&shdlc->state_mutex);
663
+
664
+
shdlc->state = SHDLC_CONNECTING;
665
+
shdlc->connect_wq = &connect_wq;
666
+
shdlc->connect_tries = 0;
667
+
shdlc->connect_result = 1;
668
+
669
+
mutex_unlock(&shdlc->state_mutex);
670
+
671
+
queue_work(shdlc->sm_wq, &shdlc->sm_work);
672
+
673
+
wait_event(connect_wq, shdlc->connect_result != 1);
674
+
675
+
return shdlc->connect_result;
676
+
}
677
+
678
+
static void nfc_shdlc_disconnect(struct nfc_shdlc *shdlc)
679
+
{
680
+
pr_debug("\n");
681
+
682
+
mutex_lock(&shdlc->state_mutex);
683
+
684
+
shdlc->state = SHDLC_DISCONNECTED;
685
+
686
+
mutex_unlock(&shdlc->state_mutex);
687
+
688
+
queue_work(shdlc->sm_wq, &shdlc->sm_work);
689
+
}
690
+
691
+
/*
692
+
* Receive an incoming shdlc frame. Frame has already been crc-validated.
693
+
* skb contains only LLC header and payload.
694
+
* If skb == NULL, it is a notification that the link below is dead.
695
+
*/
696
+
void nfc_shdlc_recv_frame(struct nfc_shdlc *shdlc, struct sk_buff *skb)
697
+
{
698
+
if (skb == NULL) {
699
+
pr_err("NULL Frame -> link is dead\n");
700
+
shdlc->hard_fault = -EREMOTEIO;
701
+
} else {
702
+
SHDLC_DUMP_SKB("incoming frame", skb);
703
+
skb_queue_tail(&shdlc->rcv_q, skb);
704
+
}
705
+
706
+
queue_work(shdlc->sm_wq, &shdlc->sm_work);
707
+
}
708
+
EXPORT_SYMBOL(nfc_shdlc_recv_frame);
709
+
710
+
static int nfc_shdlc_open(struct nfc_hci_dev *hdev)
711
+
{
712
+
struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
713
+
int r;
714
+
715
+
pr_debug("\n");
716
+
717
+
if (shdlc->ops->open) {
718
+
r = shdlc->ops->open(shdlc);
719
+
if (r < 0)
720
+
return r;
721
+
}
722
+
723
+
r = nfc_shdlc_connect(shdlc);
724
+
if (r < 0 && shdlc->ops->close)
725
+
shdlc->ops->close(shdlc);
726
+
727
+
return r;
728
+
}
729
+
730
+
static void nfc_shdlc_close(struct nfc_hci_dev *hdev)
731
+
{
732
+
struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
733
+
734
+
pr_debug("\n");
735
+
736
+
nfc_shdlc_disconnect(shdlc);
737
+
738
+
if (shdlc->ops->close)
739
+
shdlc->ops->close(shdlc);
740
+
}
741
+
742
+
static int nfc_shdlc_hci_ready(struct nfc_hci_dev *hdev)
743
+
{
744
+
struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
745
+
int r = 0;
746
+
747
+
pr_debug("\n");
748
+
749
+
if (shdlc->ops->hci_ready)
750
+
r = shdlc->ops->hci_ready(shdlc);
751
+
752
+
return r;
753
+
}
754
+
755
+
static int nfc_shdlc_xmit(struct nfc_hci_dev *hdev, struct sk_buff *skb)
756
+
{
757
+
struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
758
+
759
+
SHDLC_DUMP_SKB("queuing HCP packet to shdlc", skb);
760
+
761
+
skb_queue_tail(&shdlc->send_q, skb);
762
+
763
+
queue_work(shdlc->sm_wq, &shdlc->sm_work);
764
+
765
+
return 0;
766
+
}
767
+
768
+
static int nfc_shdlc_start_poll(struct nfc_hci_dev *hdev, u32 protocols)
769
+
{
770
+
struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
771
+
772
+
pr_debug("\n");
773
+
774
+
if (shdlc->ops->start_poll)
775
+
return shdlc->ops->start_poll(shdlc, protocols);
776
+
777
+
return 0;
778
+
}
779
+
780
+
static int nfc_shdlc_target_from_gate(struct nfc_hci_dev *hdev, u8 gate,
781
+
struct nfc_target *target)
782
+
{
783
+
struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
784
+
785
+
if (shdlc->ops->target_from_gate)
786
+
return shdlc->ops->target_from_gate(shdlc, gate, target);
787
+
788
+
return -EPERM;
789
+
}
790
+
791
+
static int nfc_shdlc_complete_target_discovered(struct nfc_hci_dev *hdev,
792
+
u8 gate,
793
+
struct nfc_target *target)
794
+
{
795
+
struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
796
+
797
+
pr_debug("\n");
798
+
799
+
if (shdlc->ops->complete_target_discovered)
800
+
return shdlc->ops->complete_target_discovered(shdlc, gate,
801
+
target);
802
+
803
+
return 0;
804
+
}
805
+
806
+
static int nfc_shdlc_data_exchange(struct nfc_hci_dev *hdev,
807
+
struct nfc_target *target,
808
+
struct sk_buff *skb,
809
+
struct sk_buff **res_skb)
810
+
{
811
+
struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
812
+
813
+
if (shdlc->ops->data_exchange)
814
+
return shdlc->ops->data_exchange(shdlc, target, skb, res_skb);
815
+
816
+
return -EPERM;
817
+
}
818
+
819
+
static struct nfc_hci_ops shdlc_ops = {
820
+
.open = nfc_shdlc_open,
821
+
.close = nfc_shdlc_close,
822
+
.hci_ready = nfc_shdlc_hci_ready,
823
+
.xmit = nfc_shdlc_xmit,
824
+
.start_poll = nfc_shdlc_start_poll,
825
+
.target_from_gate = nfc_shdlc_target_from_gate,
826
+
.complete_target_discovered = nfc_shdlc_complete_target_discovered,
827
+
.data_exchange = nfc_shdlc_data_exchange,
828
+
};
829
+
830
+
struct nfc_shdlc *nfc_shdlc_allocate(struct nfc_shdlc_ops *ops,
831
+
struct nfc_hci_init_data *init_data,
832
+
u32 protocols,
833
+
int tx_headroom, int tx_tailroom,
834
+
int max_link_payload, const char *devname)
835
+
{
836
+
struct nfc_shdlc *shdlc;
837
+
int r;
838
+
char name[32];
839
+
840
+
if (ops->xmit == NULL)
841
+
return NULL;
842
+
843
+
shdlc = kzalloc(sizeof(struct nfc_shdlc), GFP_KERNEL);
844
+
if (shdlc == NULL)
845
+
return NULL;
846
+
847
+
mutex_init(&shdlc->state_mutex);
848
+
shdlc->ops = ops;
849
+
shdlc->state = SHDLC_DISCONNECTED;
850
+
851
+
init_timer(&shdlc->connect_timer);
852
+
shdlc->connect_timer.data = (unsigned long)shdlc;
853
+
shdlc->connect_timer.function = nfc_shdlc_connect_timeout;
854
+
855
+
init_timer(&shdlc->t1_timer);
856
+
shdlc->t1_timer.data = (unsigned long)shdlc;
857
+
shdlc->t1_timer.function = nfc_shdlc_t1_timeout;
858
+
859
+
init_timer(&shdlc->t2_timer);
860
+
shdlc->t2_timer.data = (unsigned long)shdlc;
861
+
shdlc->t2_timer.function = nfc_shdlc_t2_timeout;
862
+
863
+
shdlc->w = SHDLC_MAX_WINDOW;
864
+
shdlc->srej_support = SHDLC_SREJ_SUPPORT;
865
+
866
+
skb_queue_head_init(&shdlc->rcv_q);
867
+
skb_queue_head_init(&shdlc->send_q);
868
+
skb_queue_head_init(&shdlc->ack_pending_q);
869
+
870
+
INIT_WORK(&shdlc->sm_work, nfc_shdlc_sm_work);
871
+
snprintf(name, sizeof(name), "%s_shdlc_sm_wq", devname);
872
+
shdlc->sm_wq = alloc_workqueue(name, WQ_NON_REENTRANT | WQ_UNBOUND |
873
+
WQ_MEM_RECLAIM, 1);
874
+
if (shdlc->sm_wq == NULL)
875
+
goto err_allocwq;
876
+
877
+
shdlc->client_headroom = tx_headroom;
878
+
shdlc->client_tailroom = tx_tailroom;
879
+
880
+
shdlc->hdev = nfc_hci_allocate_device(&shdlc_ops, init_data, protocols,
881
+
tx_headroom + SHDLC_LLC_HEAD_ROOM,
882
+
tx_tailroom + SHDLC_LLC_TAIL_ROOM,
883
+
max_link_payload);
884
+
if (shdlc->hdev == NULL)
885
+
goto err_allocdev;
886
+
887
+
nfc_hci_set_clientdata(shdlc->hdev, shdlc);
888
+
889
+
r = nfc_hci_register_device(shdlc->hdev);
890
+
if (r < 0)
891
+
goto err_regdev;
892
+
893
+
return shdlc;
894
+
895
+
err_regdev:
896
+
nfc_hci_free_device(shdlc->hdev);
897
+
898
+
err_allocdev:
899
+
destroy_workqueue(shdlc->sm_wq);
900
+
901
+
err_allocwq:
902
+
kfree(shdlc);
903
+
904
+
return NULL;
905
+
}
906
+
EXPORT_SYMBOL(nfc_shdlc_allocate);
907
+
908
+
void nfc_shdlc_free(struct nfc_shdlc *shdlc)
909
+
{
910
+
pr_debug("\n");
911
+
912
+
/* TODO: Check that this cannot be called while still in use */
913
+
914
+
nfc_hci_unregister_device(shdlc->hdev);
915
+
nfc_hci_free_device(shdlc->hdev);
916
+
917
+
destroy_workqueue(shdlc->sm_wq);
918
+
919
+
skb_queue_purge(&shdlc->rcv_q);
920
+
skb_queue_purge(&shdlc->send_q);
921
+
skb_queue_purge(&shdlc->ack_pending_q);
922
+
923
+
kfree(shdlc);
924
+
}
925
+
EXPORT_SYMBOL(nfc_shdlc_free);
926
+
927
+
void nfc_shdlc_set_clientdata(struct nfc_shdlc *shdlc, void *clientdata)
928
+
{
929
+
pr_debug("\n");
930
+
931
+
shdlc->clientdata = clientdata;
932
+
}
933
+
EXPORT_SYMBOL(nfc_shdlc_set_clientdata);
934
+
935
+
void *nfc_shdlc_get_clientdata(struct nfc_shdlc *shdlc)
936
+
{
937
+
return shdlc->clientdata;
938
+
}
939
+
EXPORT_SYMBOL(nfc_shdlc_get_clientdata);
940
+
941
+
struct nfc_hci_dev *nfc_shdlc_get_hci_dev(struct nfc_shdlc *shdlc)
942
+
{
943
+
return shdlc->hdev;
944
+
}
945
+
EXPORT_SYMBOL(nfc_shdlc_get_hci_dev);