1
2
3
4
5
6
7
8
9
10
11
12
13
14
15#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
16
17#include <linux/types.h>
18#include <linux/interrupt.h>
19#include <linux/bitops.h>
20#include <linux/skbuff.h>
21
22#include "../nfc.h"
23#include <net/nfc/nci.h>
24#include <net/nfc/nci_core.h>
25
26
27
28static void nci_core_reset_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
29{
30 struct nci_core_reset_rsp *rsp = (void *) skb->data;
31
32 pr_debug("status 0x%x\n", rsp->status);
33
34
35 if (skb->len != 1) {
36 if (rsp->status == NCI_STATUS_OK) {
37 ndev->nci_ver = rsp->nci_ver;
38 pr_debug("nci_ver 0x%x, config_status 0x%x\n",
39 rsp->nci_ver, rsp->config_status);
40 }
41
42 nci_req_complete(ndev, rsp->status);
43 }
44}
45
46static u8 nci_core_init_rsp_packet_v1(struct nci_dev *ndev, struct sk_buff *skb)
47{
48 struct nci_core_init_rsp_1 *rsp_1 = (void *) skb->data;
49 struct nci_core_init_rsp_2 *rsp_2;
50
51 pr_debug("status 0x%x\n", rsp_1->status);
52
53 if (rsp_1->status != NCI_STATUS_OK)
54 return rsp_1->status;
55
56 ndev->nfcc_features = __le32_to_cpu(rsp_1->nfcc_features);
57 ndev->num_supported_rf_interfaces = rsp_1->num_supported_rf_interfaces;
58
59 ndev->num_supported_rf_interfaces =
60 min((int)ndev->num_supported_rf_interfaces,
61 NCI_MAX_SUPPORTED_RF_INTERFACES);
62
63 memcpy(ndev->supported_rf_interfaces,
64 rsp_1->supported_rf_interfaces,
65 ndev->num_supported_rf_interfaces);
66
67 rsp_2 = (void *) (skb->data + 6 + rsp_1->num_supported_rf_interfaces);
68
69 ndev->max_logical_connections = rsp_2->max_logical_connections;
70 ndev->max_routing_table_size =
71 __le16_to_cpu(rsp_2->max_routing_table_size);
72 ndev->max_ctrl_pkt_payload_len =
73 rsp_2->max_ctrl_pkt_payload_len;
74 ndev->max_size_for_large_params =
75 __le16_to_cpu(rsp_2->max_size_for_large_params);
76 ndev->manufact_id =
77 rsp_2->manufact_id;
78 ndev->manufact_specific_info =
79 __le32_to_cpu(rsp_2->manufact_specific_info);
80
81 return NCI_STATUS_OK;
82}
83
84static u8 nci_core_init_rsp_packet_v2(struct nci_dev *ndev, struct sk_buff *skb)
85{
86 struct nci_core_init_rsp_nci_ver2 *rsp = (void *)skb->data;
87 u8 *supported_rf_interface = rsp->supported_rf_interfaces;
88 u8 rf_interface_idx = 0;
89 u8 rf_extension_cnt = 0;
90
91 pr_debug("status %x\n", rsp->status);
92
93 if (rsp->status != NCI_STATUS_OK)
94 return rsp->status;
95
96 ndev->nfcc_features = __le32_to_cpu(rsp->nfcc_features);
97 ndev->num_supported_rf_interfaces = rsp->num_supported_rf_interfaces;
98
99 ndev->num_supported_rf_interfaces =
100 min((int)ndev->num_supported_rf_interfaces,
101 NCI_MAX_SUPPORTED_RF_INTERFACES);
102
103 while (rf_interface_idx < ndev->num_supported_rf_interfaces) {
104 ndev->supported_rf_interfaces[rf_interface_idx++] = *supported_rf_interface++;
105
106
107 rf_extension_cnt = *supported_rf_interface++;
108 supported_rf_interface += rf_extension_cnt;
109 }
110
111 ndev->max_logical_connections = rsp->max_logical_connections;
112 ndev->max_routing_table_size =
113 __le16_to_cpu(rsp->max_routing_table_size);
114 ndev->max_ctrl_pkt_payload_len =
115 rsp->max_ctrl_pkt_payload_len;
116 ndev->max_size_for_large_params = NCI_MAX_LARGE_PARAMS_NCI_v2;
117
118 return NCI_STATUS_OK;
119}
120
121static void nci_core_init_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
122{
123 u8 status = 0;
124
125 if (!(ndev->nci_ver & NCI_VER_2_MASK))
126 status = nci_core_init_rsp_packet_v1(ndev, skb);
127 else
128 status = nci_core_init_rsp_packet_v2(ndev, skb);
129
130 if (status != NCI_STATUS_OK)
131 goto exit;
132
133 pr_debug("nfcc_features 0x%x\n",
134 ndev->nfcc_features);
135 pr_debug("num_supported_rf_interfaces %d\n",
136 ndev->num_supported_rf_interfaces);
137 pr_debug("supported_rf_interfaces[0] 0x%x\n",
138 ndev->supported_rf_interfaces[0]);
139 pr_debug("supported_rf_interfaces[1] 0x%x\n",
140 ndev->supported_rf_interfaces[1]);
141 pr_debug("supported_rf_interfaces[2] 0x%x\n",
142 ndev->supported_rf_interfaces[2]);
143 pr_debug("supported_rf_interfaces[3] 0x%x\n",
144 ndev->supported_rf_interfaces[3]);
145 pr_debug("max_logical_connections %d\n",
146 ndev->max_logical_connections);
147 pr_debug("max_routing_table_size %d\n",
148 ndev->max_routing_table_size);
149 pr_debug("max_ctrl_pkt_payload_len %d\n",
150 ndev->max_ctrl_pkt_payload_len);
151 pr_debug("max_size_for_large_params %d\n",
152 ndev->max_size_for_large_params);
153 pr_debug("manufact_id 0x%x\n",
154 ndev->manufact_id);
155 pr_debug("manufact_specific_info 0x%x\n",
156 ndev->manufact_specific_info);
157
158exit:
159 nci_req_complete(ndev, status);
160}
161
162static void nci_core_set_config_rsp_packet(struct nci_dev *ndev,
163 struct sk_buff *skb)
164{
165 struct nci_core_set_config_rsp *rsp = (void *) skb->data;
166
167 pr_debug("status 0x%x\n", rsp->status);
168
169 nci_req_complete(ndev, rsp->status);
170}
171
172static void nci_rf_disc_map_rsp_packet(struct nci_dev *ndev,
173 struct sk_buff *skb)
174{
175 __u8 status = skb->data[0];
176
177 pr_debug("status 0x%x\n", status);
178
179 nci_req_complete(ndev, status);
180}
181
182static void nci_rf_disc_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
183{
184 struct nci_conn_info *conn_info;
185 __u8 status = skb->data[0];
186
187 pr_debug("status 0x%x\n", status);
188
189 if (status == NCI_STATUS_OK) {
190 atomic_set(&ndev->state, NCI_DISCOVERY);
191
192 conn_info = ndev->rf_conn_info;
193 if (!conn_info) {
194 conn_info = devm_kzalloc(&ndev->nfc_dev->dev,
195 sizeof(struct nci_conn_info),
196 GFP_KERNEL);
197 if (!conn_info) {
198 status = NCI_STATUS_REJECTED;
199 goto exit;
200 }
201 conn_info->conn_id = NCI_STATIC_RF_CONN_ID;
202 INIT_LIST_HEAD(&conn_info->list);
203 list_add(&conn_info->list, &ndev->conn_info_list);
204 ndev->rf_conn_info = conn_info;
205 }
206 }
207
208exit:
209 nci_req_complete(ndev, status);
210}
211
212static void nci_rf_disc_select_rsp_packet(struct nci_dev *ndev,
213 struct sk_buff *skb)
214{
215 __u8 status = skb->data[0];
216
217 pr_debug("status 0x%x\n", status);
218
219
220 if (status != NCI_STATUS_OK)
221 nci_req_complete(ndev, status);
222}
223
224static void nci_rf_deactivate_rsp_packet(struct nci_dev *ndev,
225 struct sk_buff *skb)
226{
227 __u8 status = skb->data[0];
228
229 pr_debug("status 0x%x\n", status);
230
231
232 if ((status != NCI_STATUS_OK) ||
233 (atomic_read(&ndev->state) != NCI_POLL_ACTIVE)) {
234 nci_clear_target_list(ndev);
235 atomic_set(&ndev->state, NCI_IDLE);
236 nci_req_complete(ndev, status);
237 }
238}
239
240static void nci_nfcee_discover_rsp_packet(struct nci_dev *ndev,
241 struct sk_buff *skb)
242{
243 struct nci_nfcee_discover_rsp *discover_rsp;
244
245 if (skb->len != 2) {
246 nci_req_complete(ndev, NCI_STATUS_NFCEE_PROTOCOL_ERROR);
247 return;
248 }
249
250 discover_rsp = (struct nci_nfcee_discover_rsp *)skb->data;
251
252 if (discover_rsp->status != NCI_STATUS_OK ||
253 discover_rsp->num_nfcee == 0)
254 nci_req_complete(ndev, discover_rsp->status);
255}
256
257static void nci_nfcee_mode_set_rsp_packet(struct nci_dev *ndev,
258 struct sk_buff *skb)
259{
260 __u8 status = skb->data[0];
261
262 pr_debug("status 0x%x\n", status);
263 nci_req_complete(ndev, status);
264}
265
266static void nci_core_conn_create_rsp_packet(struct nci_dev *ndev,
267 struct sk_buff *skb)
268{
269 __u8 status = skb->data[0];
270 struct nci_conn_info *conn_info = NULL;
271 struct nci_core_conn_create_rsp *rsp;
272
273 pr_debug("status 0x%x\n", status);
274
275 if (status == NCI_STATUS_OK) {
276 rsp = (struct nci_core_conn_create_rsp *)skb->data;
277
278 conn_info = devm_kzalloc(&ndev->nfc_dev->dev,
279 sizeof(*conn_info), GFP_KERNEL);
280 if (!conn_info) {
281 status = NCI_STATUS_REJECTED;
282 goto exit;
283 }
284
285 conn_info->dest_params = devm_kzalloc(&ndev->nfc_dev->dev,
286 sizeof(struct dest_spec_params),
287 GFP_KERNEL);
288 if (!conn_info->dest_params) {
289 status = NCI_STATUS_REJECTED;
290 goto free_conn_info;
291 }
292
293 conn_info->dest_type = ndev->cur_dest_type;
294 conn_info->dest_params->id = ndev->cur_params.id;
295 conn_info->dest_params->protocol = ndev->cur_params.protocol;
296 conn_info->conn_id = rsp->conn_id;
297
298
299
300
301
302 INIT_LIST_HEAD(&conn_info->list);
303 list_add(&conn_info->list, &ndev->conn_info_list);
304
305 if (ndev->cur_params.id == ndev->hci_dev->nfcee_id)
306 ndev->hci_dev->conn_info = conn_info;
307
308 conn_info->conn_id = rsp->conn_id;
309 conn_info->max_pkt_payload_len = rsp->max_ctrl_pkt_payload_len;
310 atomic_set(&conn_info->credits_cnt, rsp->credits_cnt);
311 }
312
313free_conn_info:
314 if (status == NCI_STATUS_REJECTED)
315 devm_kfree(&ndev->nfc_dev->dev, conn_info);
316exit:
317
318 nci_req_complete(ndev, status);
319}
320
321static void nci_core_conn_close_rsp_packet(struct nci_dev *ndev,
322 struct sk_buff *skb)
323{
324 struct nci_conn_info *conn_info;
325 __u8 status = skb->data[0];
326
327 pr_debug("status 0x%x\n", status);
328 if (status == NCI_STATUS_OK) {
329 conn_info = nci_get_conn_info_by_conn_id(ndev,
330 ndev->cur_conn_id);
331 if (conn_info) {
332 list_del(&conn_info->list);
333 devm_kfree(&ndev->nfc_dev->dev, conn_info);
334 }
335 }
336 nci_req_complete(ndev, status);
337}
338
339void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
340{
341 __u16 rsp_opcode = nci_opcode(skb->data);
342
343
344 del_timer(&ndev->cmd_timer);
345
346 pr_debug("NCI RX: MT=rsp, PBF=%d, GID=0x%x, OID=0x%x, plen=%d\n",
347 nci_pbf(skb->data),
348 nci_opcode_gid(rsp_opcode),
349 nci_opcode_oid(rsp_opcode),
350 nci_plen(skb->data));
351
352
353 skb_pull(skb, NCI_CTRL_HDR_SIZE);
354
355 if (nci_opcode_gid(rsp_opcode) == NCI_GID_PROPRIETARY) {
356 if (nci_prop_rsp_packet(ndev, rsp_opcode, skb) == -ENOTSUPP) {
357 pr_err("unsupported rsp opcode 0x%x\n",
358 rsp_opcode);
359 }
360
361 goto end;
362 }
363
364 switch (rsp_opcode) {
365 case NCI_OP_CORE_RESET_RSP:
366 nci_core_reset_rsp_packet(ndev, skb);
367 break;
368
369 case NCI_OP_CORE_INIT_RSP:
370 nci_core_init_rsp_packet(ndev, skb);
371 break;
372
373 case NCI_OP_CORE_SET_CONFIG_RSP:
374 nci_core_set_config_rsp_packet(ndev, skb);
375 break;
376
377 case NCI_OP_CORE_CONN_CREATE_RSP:
378 nci_core_conn_create_rsp_packet(ndev, skb);
379 break;
380
381 case NCI_OP_CORE_CONN_CLOSE_RSP:
382 nci_core_conn_close_rsp_packet(ndev, skb);
383 break;
384
385 case NCI_OP_RF_DISCOVER_MAP_RSP:
386 nci_rf_disc_map_rsp_packet(ndev, skb);
387 break;
388
389 case NCI_OP_RF_DISCOVER_RSP:
390 nci_rf_disc_rsp_packet(ndev, skb);
391 break;
392
393 case NCI_OP_RF_DISCOVER_SELECT_RSP:
394 nci_rf_disc_select_rsp_packet(ndev, skb);
395 break;
396
397 case NCI_OP_RF_DEACTIVATE_RSP:
398 nci_rf_deactivate_rsp_packet(ndev, skb);
399 break;
400
401 case NCI_OP_NFCEE_DISCOVER_RSP:
402 nci_nfcee_discover_rsp_packet(ndev, skb);
403 break;
404
405 case NCI_OP_NFCEE_MODE_SET_RSP:
406 nci_nfcee_mode_set_rsp_packet(ndev, skb);
407 break;
408
409 default:
410 pr_err("unknown rsp opcode 0x%x\n", rsp_opcode);
411 break;
412 }
413
414 nci_core_rsp_packet(ndev, rsp_opcode, skb);
415end:
416 kfree_skb(skb);
417
418
419 atomic_set(&ndev->cmd_cnt, 1);
420 if (!skb_queue_empty(&ndev->cmd_q))
421 queue_work(ndev->cmd_wq, &ndev->cmd_work);
422}
423
