1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * The NFC Controller Interface is the communication protocol between an |
4 | * NFC Controller (NFCC) and a Device Host (DH). |
5 | * This is the HCI over NCI implementation, as specified in the 10.2 |
6 | * section of the NCI 1.1 specification. |
7 | * |
8 | * Copyright (C) 2014 STMicroelectronics SAS. All rights reserved. |
9 | */ |
10 | |
11 | #include <linux/skbuff.h> |
12 | |
13 | #include "../nfc.h" |
14 | #include <net/nfc/nci.h> |
15 | #include <net/nfc/nci_core.h> |
16 | #include <linux/nfc.h> |
17 | #include <linux/kcov.h> |
18 | |
19 | struct nci_data { |
20 | u8 conn_id; |
21 | u8 pipe; |
22 | u8 cmd; |
23 | const u8 *data; |
24 | u32 data_len; |
25 | } __packed; |
26 | |
27 | struct nci_hci_create_pipe_params { |
28 | u8 src_gate; |
29 | u8 dest_host; |
30 | u8 dest_gate; |
31 | } __packed; |
32 | |
33 | struct nci_hci_create_pipe_resp { |
34 | u8 src_host; |
35 | u8 src_gate; |
36 | u8 dest_host; |
37 | u8 dest_gate; |
38 | u8 pipe; |
39 | } __packed; |
40 | |
41 | struct nci_hci_delete_pipe_noti { |
42 | u8 pipe; |
43 | } __packed; |
44 | |
45 | struct nci_hci_all_pipe_cleared_noti { |
46 | u8 host; |
47 | } __packed; |
48 | |
49 | struct nci_hcp_message { |
50 | u8 ; /* type -cmd,evt,rsp- + instruction */ |
51 | u8 data[]; |
52 | } __packed; |
53 | |
54 | struct nci_hcp_packet { |
55 | u8 ; /* cbit+pipe */ |
56 | struct nci_hcp_message message; |
57 | } __packed; |
58 | |
59 | #define NCI_HCI_ANY_SET_PARAMETER 0x01 |
60 | #define NCI_HCI_ANY_GET_PARAMETER 0x02 |
61 | #define NCI_HCI_ANY_CLOSE_PIPE 0x04 |
62 | #define NCI_HCI_ADM_CLEAR_ALL_PIPE 0x14 |
63 | |
64 | #define NCI_HFP_NO_CHAINING 0x80 |
65 | |
66 | #define NCI_NFCEE_ID_HCI 0x80 |
67 | |
68 | #define NCI_EVT_HOT_PLUG 0x03 |
69 | |
70 | #define NCI_HCI_ADMIN_PARAM_SESSION_IDENTITY 0x01 |
71 | #define NCI_HCI_ADM_CREATE_PIPE 0x10 |
72 | #define NCI_HCI_ADM_DELETE_PIPE 0x11 |
73 | |
74 | /* HCP headers */ |
75 | #define 1 |
76 | #define 1 |
77 | #define 2 |
78 | |
79 | /* HCP types */ |
80 | #define NCI_HCI_HCP_COMMAND 0x00 |
81 | #define NCI_HCI_HCP_EVENT 0x01 |
82 | #define NCI_HCI_HCP_RESPONSE 0x02 |
83 | |
84 | #define NCI_HCI_ADM_NOTIFY_PIPE_CREATED 0x12 |
85 | #define NCI_HCI_ADM_NOTIFY_PIPE_DELETED 0x13 |
86 | #define NCI_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED 0x15 |
87 | |
88 | #define NCI_HCI_FRAGMENT 0x7f |
89 | #define (type, instr) ((((type) & 0x03) << 6) |\ |
90 | ((instr) & 0x3f)) |
91 | |
92 | #define NCI_HCP_MSG_GET_TYPE(header) ((header & 0xc0) >> 6) |
93 | #define NCI_HCP_MSG_GET_CMD(header) (header & 0x3f) |
94 | #define NCI_HCP_MSG_GET_PIPE(header) (header & 0x7f) |
95 | |
96 | static int nci_hci_result_to_errno(u8 result) |
97 | { |
98 | switch (result) { |
99 | case NCI_HCI_ANY_OK: |
100 | return 0; |
101 | case NCI_HCI_ANY_E_REG_PAR_UNKNOWN: |
102 | return -EOPNOTSUPP; |
103 | case NCI_HCI_ANY_E_TIMEOUT: |
104 | return -ETIME; |
105 | default: |
106 | return -1; |
107 | } |
108 | } |
109 | |
110 | /* HCI core */ |
111 | static void nci_hci_reset_pipes(struct nci_hci_dev *hdev) |
112 | { |
113 | int i; |
114 | |
115 | for (i = 0; i < NCI_HCI_MAX_PIPES; i++) { |
116 | hdev->pipes[i].gate = NCI_HCI_INVALID_GATE; |
117 | hdev->pipes[i].host = NCI_HCI_INVALID_HOST; |
118 | } |
119 | memset(hdev->gate2pipe, NCI_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe)); |
120 | } |
121 | |
122 | static void nci_hci_reset_pipes_per_host(struct nci_dev *ndev, u8 host) |
123 | { |
124 | int i; |
125 | |
126 | for (i = 0; i < NCI_HCI_MAX_PIPES; i++) { |
127 | if (ndev->hci_dev->pipes[i].host == host) { |
128 | ndev->hci_dev->pipes[i].gate = NCI_HCI_INVALID_GATE; |
129 | ndev->hci_dev->pipes[i].host = NCI_HCI_INVALID_HOST; |
130 | } |
131 | } |
132 | } |
133 | |
134 | /* Fragment HCI data over NCI packet. |
135 | * NFC Forum NCI 10.2.2 Data Exchange: |
136 | * The payload of the Data Packets sent on the Logical Connection SHALL be |
137 | * valid HCP packets, as defined within [ETSI_102622]. Each Data Packet SHALL |
138 | * contain a single HCP packet. NCI Segmentation and Reassembly SHALL NOT be |
139 | * applied to Data Messages in either direction. The HCI fragmentation mechanism |
140 | * is used if required. |
141 | */ |
142 | static int nci_hci_send_data(struct nci_dev *ndev, u8 pipe, |
143 | const u8 data_type, const u8 *data, |
144 | size_t data_len) |
145 | { |
146 | const struct nci_conn_info *conn_info; |
147 | struct sk_buff *skb; |
148 | int len, i, r; |
149 | u8 cb = pipe; |
150 | |
151 | conn_info = ndev->hci_dev->conn_info; |
152 | if (!conn_info) |
153 | return -EPROTO; |
154 | |
155 | i = 0; |
156 | skb = nci_skb_alloc(ndev, len: conn_info->max_pkt_payload_len + |
157 | NCI_DATA_HDR_SIZE, GFP_ATOMIC); |
158 | if (!skb) |
159 | return -ENOMEM; |
160 | |
161 | skb_reserve(skb, NCI_DATA_HDR_SIZE + 2); |
162 | *(u8 *)skb_push(skb, len: 1) = data_type; |
163 | |
164 | do { |
165 | /* If last packet add NCI_HFP_NO_CHAINING */ |
166 | if (i + conn_info->max_pkt_payload_len - |
167 | (skb->len + 1) >= data_len) { |
168 | cb |= NCI_HFP_NO_CHAINING; |
169 | len = data_len - i; |
170 | } else { |
171 | len = conn_info->max_pkt_payload_len - skb->len - 1; |
172 | } |
173 | |
174 | *(u8 *)skb_push(skb, len: 1) = cb; |
175 | |
176 | if (len > 0) |
177 | skb_put_data(skb, data: data + i, len); |
178 | |
179 | r = nci_send_data(ndev, conn_id: conn_info->conn_id, skb); |
180 | if (r < 0) |
181 | return r; |
182 | |
183 | i += len; |
184 | |
185 | if (i < data_len) { |
186 | skb = nci_skb_alloc(ndev, |
187 | len: conn_info->max_pkt_payload_len + |
188 | NCI_DATA_HDR_SIZE, GFP_ATOMIC); |
189 | if (!skb) |
190 | return -ENOMEM; |
191 | |
192 | skb_reserve(skb, NCI_DATA_HDR_SIZE + 1); |
193 | } |
194 | } while (i < data_len); |
195 | |
196 | return i; |
197 | } |
198 | |
199 | static void nci_hci_send_data_req(struct nci_dev *ndev, const void *opt) |
200 | { |
201 | const struct nci_data *data = opt; |
202 | |
203 | nci_hci_send_data(ndev, pipe: data->pipe, data_type: data->cmd, |
204 | data: data->data, data_len: data->data_len); |
205 | } |
206 | |
207 | int nci_hci_send_event(struct nci_dev *ndev, u8 gate, u8 event, |
208 | const u8 *param, size_t param_len) |
209 | { |
210 | u8 pipe = ndev->hci_dev->gate2pipe[gate]; |
211 | |
212 | if (pipe == NCI_HCI_INVALID_PIPE) |
213 | return -EADDRNOTAVAIL; |
214 | |
215 | return nci_hci_send_data(ndev, pipe, |
216 | NCI_HCP_HEADER(NCI_HCI_HCP_EVENT, event), |
217 | data: param, data_len: param_len); |
218 | } |
219 | EXPORT_SYMBOL(nci_hci_send_event); |
220 | |
221 | int nci_hci_send_cmd(struct nci_dev *ndev, u8 gate, u8 cmd, |
222 | const u8 *param, size_t param_len, |
223 | struct sk_buff **skb) |
224 | { |
225 | const struct nci_hcp_message *message; |
226 | const struct nci_conn_info *conn_info; |
227 | struct nci_data data; |
228 | int r; |
229 | u8 pipe = ndev->hci_dev->gate2pipe[gate]; |
230 | |
231 | if (pipe == NCI_HCI_INVALID_PIPE) |
232 | return -EADDRNOTAVAIL; |
233 | |
234 | conn_info = ndev->hci_dev->conn_info; |
235 | if (!conn_info) |
236 | return -EPROTO; |
237 | |
238 | data.conn_id = conn_info->conn_id; |
239 | data.pipe = pipe; |
240 | data.cmd = NCI_HCP_HEADER(NCI_HCI_HCP_COMMAND, cmd); |
241 | data.data = param; |
242 | data.data_len = param_len; |
243 | |
244 | r = nci_request(ndev, req: nci_hci_send_data_req, opt: &data, |
245 | timeout: msecs_to_jiffies(NCI_DATA_TIMEOUT)); |
246 | if (r == NCI_STATUS_OK) { |
247 | message = (struct nci_hcp_message *)conn_info->rx_skb->data; |
248 | r = nci_hci_result_to_errno( |
249 | NCI_HCP_MSG_GET_CMD(message->header)); |
250 | skb_pull(skb: conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN); |
251 | |
252 | if (!r && skb) |
253 | *skb = conn_info->rx_skb; |
254 | } |
255 | |
256 | return r; |
257 | } |
258 | EXPORT_SYMBOL(nci_hci_send_cmd); |
259 | |
260 | int nci_hci_clear_all_pipes(struct nci_dev *ndev) |
261 | { |
262 | int r; |
263 | |
264 | r = nci_hci_send_cmd(ndev, NCI_HCI_ADMIN_GATE, |
265 | NCI_HCI_ADM_CLEAR_ALL_PIPE, NULL, 0, NULL); |
266 | if (r < 0) |
267 | return r; |
268 | |
269 | nci_hci_reset_pipes(hdev: ndev->hci_dev); |
270 | return r; |
271 | } |
272 | EXPORT_SYMBOL(nci_hci_clear_all_pipes); |
273 | |
274 | static void nci_hci_event_received(struct nci_dev *ndev, u8 pipe, |
275 | u8 event, struct sk_buff *skb) |
276 | { |
277 | if (ndev->ops->hci_event_received) |
278 | ndev->ops->hci_event_received(ndev, pipe, event, skb); |
279 | } |
280 | |
281 | static void nci_hci_cmd_received(struct nci_dev *ndev, u8 pipe, |
282 | u8 cmd, struct sk_buff *skb) |
283 | { |
284 | u8 gate = ndev->hci_dev->pipes[pipe].gate; |
285 | u8 status = NCI_HCI_ANY_OK | ~NCI_HCI_FRAGMENT; |
286 | u8 dest_gate, new_pipe; |
287 | struct nci_hci_create_pipe_resp *create_info; |
288 | struct nci_hci_delete_pipe_noti *delete_info; |
289 | struct nci_hci_all_pipe_cleared_noti *cleared_info; |
290 | |
291 | pr_debug("from gate %x pipe %x cmd %x\n" , gate, pipe, cmd); |
292 | |
293 | switch (cmd) { |
294 | case NCI_HCI_ADM_NOTIFY_PIPE_CREATED: |
295 | if (skb->len != 5) { |
296 | status = NCI_HCI_ANY_E_NOK; |
297 | goto exit; |
298 | } |
299 | create_info = (struct nci_hci_create_pipe_resp *)skb->data; |
300 | dest_gate = create_info->dest_gate; |
301 | new_pipe = create_info->pipe; |
302 | if (new_pipe >= NCI_HCI_MAX_PIPES) { |
303 | status = NCI_HCI_ANY_E_NOK; |
304 | goto exit; |
305 | } |
306 | |
307 | /* Save the new created pipe and bind with local gate, |
308 | * the description for skb->data[3] is destination gate id |
309 | * but since we received this cmd from host controller, we |
310 | * are the destination and it is our local gate |
311 | */ |
312 | ndev->hci_dev->gate2pipe[dest_gate] = new_pipe; |
313 | ndev->hci_dev->pipes[new_pipe].gate = dest_gate; |
314 | ndev->hci_dev->pipes[new_pipe].host = |
315 | create_info->src_host; |
316 | break; |
317 | case NCI_HCI_ANY_OPEN_PIPE: |
318 | /* If the pipe is not created report an error */ |
319 | if (gate == NCI_HCI_INVALID_GATE) { |
320 | status = NCI_HCI_ANY_E_NOK; |
321 | goto exit; |
322 | } |
323 | break; |
324 | case NCI_HCI_ADM_NOTIFY_PIPE_DELETED: |
325 | if (skb->len != 1) { |
326 | status = NCI_HCI_ANY_E_NOK; |
327 | goto exit; |
328 | } |
329 | delete_info = (struct nci_hci_delete_pipe_noti *)skb->data; |
330 | if (delete_info->pipe >= NCI_HCI_MAX_PIPES) { |
331 | status = NCI_HCI_ANY_E_NOK; |
332 | goto exit; |
333 | } |
334 | |
335 | ndev->hci_dev->pipes[delete_info->pipe].gate = |
336 | NCI_HCI_INVALID_GATE; |
337 | ndev->hci_dev->pipes[delete_info->pipe].host = |
338 | NCI_HCI_INVALID_HOST; |
339 | break; |
340 | case NCI_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED: |
341 | if (skb->len != 1) { |
342 | status = NCI_HCI_ANY_E_NOK; |
343 | goto exit; |
344 | } |
345 | |
346 | cleared_info = |
347 | (struct nci_hci_all_pipe_cleared_noti *)skb->data; |
348 | nci_hci_reset_pipes_per_host(ndev, host: cleared_info->host); |
349 | break; |
350 | default: |
351 | pr_debug("Discarded unknown cmd %x to gate %x\n" , cmd, gate); |
352 | break; |
353 | } |
354 | |
355 | if (ndev->ops->hci_cmd_received) |
356 | ndev->ops->hci_cmd_received(ndev, pipe, cmd, skb); |
357 | |
358 | exit: |
359 | nci_hci_send_data(ndev, pipe, data_type: status, NULL, data_len: 0); |
360 | |
361 | kfree_skb(skb); |
362 | } |
363 | |
364 | static void nci_hci_resp_received(struct nci_dev *ndev, u8 pipe, |
365 | struct sk_buff *skb) |
366 | { |
367 | struct nci_conn_info *conn_info; |
368 | |
369 | conn_info = ndev->hci_dev->conn_info; |
370 | if (!conn_info) |
371 | goto exit; |
372 | |
373 | conn_info->rx_skb = skb; |
374 | |
375 | exit: |
376 | nci_req_complete(ndev, NCI_STATUS_OK); |
377 | } |
378 | |
379 | /* Receive hcp message for pipe, with type and cmd. |
380 | * skb contains optional message data only. |
381 | */ |
382 | static void nci_hci_hcp_message_rx(struct nci_dev *ndev, u8 pipe, |
383 | u8 type, u8 instruction, struct sk_buff *skb) |
384 | { |
385 | switch (type) { |
386 | case NCI_HCI_HCP_RESPONSE: |
387 | nci_hci_resp_received(ndev, pipe, skb); |
388 | break; |
389 | case NCI_HCI_HCP_COMMAND: |
390 | nci_hci_cmd_received(ndev, pipe, cmd: instruction, skb); |
391 | break; |
392 | case NCI_HCI_HCP_EVENT: |
393 | nci_hci_event_received(ndev, pipe, event: instruction, skb); |
394 | break; |
395 | default: |
396 | pr_err("UNKNOWN MSG Type %d, instruction=%d\n" , |
397 | type, instruction); |
398 | kfree_skb(skb); |
399 | break; |
400 | } |
401 | |
402 | nci_req_complete(ndev, NCI_STATUS_OK); |
403 | } |
404 | |
405 | static void nci_hci_msg_rx_work(struct work_struct *work) |
406 | { |
407 | struct nci_hci_dev *hdev = |
408 | container_of(work, struct nci_hci_dev, msg_rx_work); |
409 | struct sk_buff *skb; |
410 | const struct nci_hcp_message *message; |
411 | u8 pipe, type, instruction; |
412 | |
413 | for (; (skb = skb_dequeue(list: &hdev->msg_rx_queue)); kcov_remote_stop()) { |
414 | kcov_remote_start_common(id: skb_get_kcov_handle(skb)); |
415 | pipe = NCI_HCP_MSG_GET_PIPE(skb->data[0]); |
416 | skb_pull(skb, NCI_HCI_HCP_PACKET_HEADER_LEN); |
417 | message = (struct nci_hcp_message *)skb->data; |
418 | type = NCI_HCP_MSG_GET_TYPE(message->header); |
419 | instruction = NCI_HCP_MSG_GET_CMD(message->header); |
420 | skb_pull(skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN); |
421 | |
422 | nci_hci_hcp_message_rx(ndev: hdev->ndev, pipe, |
423 | type, instruction, skb); |
424 | } |
425 | } |
426 | |
427 | void nci_hci_data_received_cb(void *context, |
428 | struct sk_buff *skb, int err) |
429 | { |
430 | struct nci_dev *ndev = (struct nci_dev *)context; |
431 | struct nci_hcp_packet *packet; |
432 | u8 pipe, type; |
433 | struct sk_buff *hcp_skb; |
434 | struct sk_buff *frag_skb; |
435 | int msg_len; |
436 | |
437 | if (err) { |
438 | nci_req_complete(ndev, result: err); |
439 | return; |
440 | } |
441 | |
442 | packet = (struct nci_hcp_packet *)skb->data; |
443 | if ((packet->header & ~NCI_HCI_FRAGMENT) == 0) { |
444 | skb_queue_tail(list: &ndev->hci_dev->rx_hcp_frags, newsk: skb); |
445 | return; |
446 | } |
447 | |
448 | /* it's the last fragment. Does it need re-aggregation? */ |
449 | if (skb_queue_len(list_: &ndev->hci_dev->rx_hcp_frags)) { |
450 | pipe = NCI_HCP_MSG_GET_PIPE(packet->header); |
451 | skb_queue_tail(list: &ndev->hci_dev->rx_hcp_frags, newsk: skb); |
452 | |
453 | msg_len = 0; |
454 | skb_queue_walk(&ndev->hci_dev->rx_hcp_frags, frag_skb) { |
455 | msg_len += (frag_skb->len - |
456 | NCI_HCI_HCP_PACKET_HEADER_LEN); |
457 | } |
458 | |
459 | hcp_skb = nfc_alloc_recv_skb(NCI_HCI_HCP_PACKET_HEADER_LEN + |
460 | msg_len, GFP_KERNEL); |
461 | if (!hcp_skb) { |
462 | nci_req_complete(ndev, result: -ENOMEM); |
463 | return; |
464 | } |
465 | |
466 | skb_put_u8(skb: hcp_skb, val: pipe); |
467 | |
468 | skb_queue_walk(&ndev->hci_dev->rx_hcp_frags, frag_skb) { |
469 | msg_len = frag_skb->len - NCI_HCI_HCP_PACKET_HEADER_LEN; |
470 | skb_put_data(skb: hcp_skb, |
471 | data: frag_skb->data + NCI_HCI_HCP_PACKET_HEADER_LEN, |
472 | len: msg_len); |
473 | } |
474 | |
475 | skb_queue_purge(list: &ndev->hci_dev->rx_hcp_frags); |
476 | } else { |
477 | packet->header &= NCI_HCI_FRAGMENT; |
478 | hcp_skb = skb; |
479 | } |
480 | |
481 | /* if this is a response, dispatch immediately to |
482 | * unblock waiting cmd context. Otherwise, enqueue to dispatch |
483 | * in separate context where handler can also execute command. |
484 | */ |
485 | packet = (struct nci_hcp_packet *)hcp_skb->data; |
486 | type = NCI_HCP_MSG_GET_TYPE(packet->message.header); |
487 | if (type == NCI_HCI_HCP_RESPONSE) { |
488 | pipe = NCI_HCP_MSG_GET_PIPE(packet->header); |
489 | skb_pull(skb: hcp_skb, NCI_HCI_HCP_PACKET_HEADER_LEN); |
490 | nci_hci_hcp_message_rx(ndev, pipe, type, |
491 | NCI_STATUS_OK, skb: hcp_skb); |
492 | } else { |
493 | skb_queue_tail(list: &ndev->hci_dev->msg_rx_queue, newsk: hcp_skb); |
494 | schedule_work(work: &ndev->hci_dev->msg_rx_work); |
495 | } |
496 | } |
497 | |
498 | int nci_hci_open_pipe(struct nci_dev *ndev, u8 pipe) |
499 | { |
500 | struct nci_data data; |
501 | const struct nci_conn_info *conn_info; |
502 | |
503 | conn_info = ndev->hci_dev->conn_info; |
504 | if (!conn_info) |
505 | return -EPROTO; |
506 | |
507 | data.conn_id = conn_info->conn_id; |
508 | data.pipe = pipe; |
509 | data.cmd = NCI_HCP_HEADER(NCI_HCI_HCP_COMMAND, |
510 | NCI_HCI_ANY_OPEN_PIPE); |
511 | data.data = NULL; |
512 | data.data_len = 0; |
513 | |
514 | return nci_request(ndev, req: nci_hci_send_data_req, opt: &data, |
515 | timeout: msecs_to_jiffies(NCI_DATA_TIMEOUT)); |
516 | } |
517 | EXPORT_SYMBOL(nci_hci_open_pipe); |
518 | |
519 | static u8 nci_hci_create_pipe(struct nci_dev *ndev, u8 dest_host, |
520 | u8 dest_gate, int *result) |
521 | { |
522 | u8 pipe; |
523 | struct sk_buff *skb; |
524 | struct nci_hci_create_pipe_params params; |
525 | const struct nci_hci_create_pipe_resp *resp; |
526 | |
527 | pr_debug("gate=%d\n" , dest_gate); |
528 | |
529 | params.src_gate = NCI_HCI_ADMIN_GATE; |
530 | params.dest_host = dest_host; |
531 | params.dest_gate = dest_gate; |
532 | |
533 | *result = nci_hci_send_cmd(ndev, NCI_HCI_ADMIN_GATE, |
534 | NCI_HCI_ADM_CREATE_PIPE, |
535 | (u8 *)¶ms, sizeof(params), &skb); |
536 | if (*result < 0) |
537 | return NCI_HCI_INVALID_PIPE; |
538 | |
539 | resp = (struct nci_hci_create_pipe_resp *)skb->data; |
540 | pipe = resp->pipe; |
541 | kfree_skb(skb); |
542 | |
543 | pr_debug("pipe created=%d\n" , pipe); |
544 | |
545 | return pipe; |
546 | } |
547 | |
548 | static int nci_hci_delete_pipe(struct nci_dev *ndev, u8 pipe) |
549 | { |
550 | return nci_hci_send_cmd(ndev, NCI_HCI_ADMIN_GATE, |
551 | NCI_HCI_ADM_DELETE_PIPE, &pipe, 1, NULL); |
552 | } |
553 | |
554 | int nci_hci_set_param(struct nci_dev *ndev, u8 gate, u8 idx, |
555 | const u8 *param, size_t param_len) |
556 | { |
557 | const struct nci_hcp_message *message; |
558 | const struct nci_conn_info *conn_info; |
559 | struct nci_data data; |
560 | int r; |
561 | u8 *tmp; |
562 | u8 pipe = ndev->hci_dev->gate2pipe[gate]; |
563 | |
564 | pr_debug("idx=%d to gate %d\n" , idx, gate); |
565 | |
566 | if (pipe == NCI_HCI_INVALID_PIPE) |
567 | return -EADDRNOTAVAIL; |
568 | |
569 | conn_info = ndev->hci_dev->conn_info; |
570 | if (!conn_info) |
571 | return -EPROTO; |
572 | |
573 | tmp = kmalloc(size: 1 + param_len, GFP_KERNEL); |
574 | if (!tmp) |
575 | return -ENOMEM; |
576 | |
577 | *tmp = idx; |
578 | memcpy(tmp + 1, param, param_len); |
579 | |
580 | data.conn_id = conn_info->conn_id; |
581 | data.pipe = pipe; |
582 | data.cmd = NCI_HCP_HEADER(NCI_HCI_HCP_COMMAND, |
583 | NCI_HCI_ANY_SET_PARAMETER); |
584 | data.data = tmp; |
585 | data.data_len = param_len + 1; |
586 | |
587 | r = nci_request(ndev, req: nci_hci_send_data_req, opt: &data, |
588 | timeout: msecs_to_jiffies(NCI_DATA_TIMEOUT)); |
589 | if (r == NCI_STATUS_OK) { |
590 | message = (struct nci_hcp_message *)conn_info->rx_skb->data; |
591 | r = nci_hci_result_to_errno( |
592 | NCI_HCP_MSG_GET_CMD(message->header)); |
593 | skb_pull(skb: conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN); |
594 | } |
595 | |
596 | kfree(objp: tmp); |
597 | return r; |
598 | } |
599 | EXPORT_SYMBOL(nci_hci_set_param); |
600 | |
601 | int nci_hci_get_param(struct nci_dev *ndev, u8 gate, u8 idx, |
602 | struct sk_buff **skb) |
603 | { |
604 | const struct nci_hcp_message *message; |
605 | const struct nci_conn_info *conn_info; |
606 | struct nci_data data; |
607 | int r; |
608 | u8 pipe = ndev->hci_dev->gate2pipe[gate]; |
609 | |
610 | pr_debug("idx=%d to gate %d\n" , idx, gate); |
611 | |
612 | if (pipe == NCI_HCI_INVALID_PIPE) |
613 | return -EADDRNOTAVAIL; |
614 | |
615 | conn_info = ndev->hci_dev->conn_info; |
616 | if (!conn_info) |
617 | return -EPROTO; |
618 | |
619 | data.conn_id = conn_info->conn_id; |
620 | data.pipe = pipe; |
621 | data.cmd = NCI_HCP_HEADER(NCI_HCI_HCP_COMMAND, |
622 | NCI_HCI_ANY_GET_PARAMETER); |
623 | data.data = &idx; |
624 | data.data_len = 1; |
625 | |
626 | r = nci_request(ndev, req: nci_hci_send_data_req, opt: &data, |
627 | timeout: msecs_to_jiffies(NCI_DATA_TIMEOUT)); |
628 | |
629 | if (r == NCI_STATUS_OK) { |
630 | message = (struct nci_hcp_message *)conn_info->rx_skb->data; |
631 | r = nci_hci_result_to_errno( |
632 | NCI_HCP_MSG_GET_CMD(message->header)); |
633 | skb_pull(skb: conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN); |
634 | |
635 | if (!r && skb) |
636 | *skb = conn_info->rx_skb; |
637 | } |
638 | |
639 | return r; |
640 | } |
641 | EXPORT_SYMBOL(nci_hci_get_param); |
642 | |
643 | int nci_hci_connect_gate(struct nci_dev *ndev, |
644 | u8 dest_host, u8 dest_gate, u8 pipe) |
645 | { |
646 | bool pipe_created = false; |
647 | int r; |
648 | |
649 | if (pipe == NCI_HCI_DO_NOT_OPEN_PIPE) |
650 | return 0; |
651 | |
652 | if (ndev->hci_dev->gate2pipe[dest_gate] != NCI_HCI_INVALID_PIPE) |
653 | return -EADDRINUSE; |
654 | |
655 | if (pipe != NCI_HCI_INVALID_PIPE) |
656 | goto open_pipe; |
657 | |
658 | switch (dest_gate) { |
659 | case NCI_HCI_LINK_MGMT_GATE: |
660 | pipe = NCI_HCI_LINK_MGMT_PIPE; |
661 | break; |
662 | case NCI_HCI_ADMIN_GATE: |
663 | pipe = NCI_HCI_ADMIN_PIPE; |
664 | break; |
665 | default: |
666 | pipe = nci_hci_create_pipe(ndev, dest_host, dest_gate, result: &r); |
667 | if (pipe == NCI_HCI_INVALID_PIPE) |
668 | return r; |
669 | pipe_created = true; |
670 | break; |
671 | } |
672 | |
673 | open_pipe: |
674 | r = nci_hci_open_pipe(ndev, pipe); |
675 | if (r < 0) { |
676 | if (pipe_created) { |
677 | if (nci_hci_delete_pipe(ndev, pipe) < 0) { |
678 | /* TODO: Cannot clean by deleting pipe... |
679 | * -> inconsistent state |
680 | */ |
681 | } |
682 | } |
683 | return r; |
684 | } |
685 | |
686 | ndev->hci_dev->pipes[pipe].gate = dest_gate; |
687 | ndev->hci_dev->pipes[pipe].host = dest_host; |
688 | ndev->hci_dev->gate2pipe[dest_gate] = pipe; |
689 | |
690 | return 0; |
691 | } |
692 | EXPORT_SYMBOL(nci_hci_connect_gate); |
693 | |
694 | static int nci_hci_dev_connect_gates(struct nci_dev *ndev, |
695 | u8 gate_count, |
696 | const struct nci_hci_gate *gates) |
697 | { |
698 | int r; |
699 | |
700 | while (gate_count--) { |
701 | r = nci_hci_connect_gate(ndev, gates->dest_host, |
702 | gates->gate, gates->pipe); |
703 | if (r < 0) |
704 | return r; |
705 | gates++; |
706 | } |
707 | |
708 | return 0; |
709 | } |
710 | |
711 | int nci_hci_dev_session_init(struct nci_dev *ndev) |
712 | { |
713 | struct nci_conn_info *conn_info; |
714 | struct sk_buff *skb; |
715 | int r; |
716 | |
717 | ndev->hci_dev->count_pipes = 0; |
718 | ndev->hci_dev->expected_pipes = 0; |
719 | |
720 | conn_info = ndev->hci_dev->conn_info; |
721 | if (!conn_info) |
722 | return -EPROTO; |
723 | |
724 | conn_info->data_exchange_cb = nci_hci_data_received_cb; |
725 | conn_info->data_exchange_cb_context = ndev; |
726 | |
727 | nci_hci_reset_pipes(hdev: ndev->hci_dev); |
728 | |
729 | if (ndev->hci_dev->init_data.gates[0].gate != NCI_HCI_ADMIN_GATE) |
730 | return -EPROTO; |
731 | |
732 | r = nci_hci_connect_gate(ndev, |
733 | ndev->hci_dev->init_data.gates[0].dest_host, |
734 | ndev->hci_dev->init_data.gates[0].gate, |
735 | ndev->hci_dev->init_data.gates[0].pipe); |
736 | if (r < 0) |
737 | return r; |
738 | |
739 | r = nci_hci_get_param(ndev, NCI_HCI_ADMIN_GATE, |
740 | NCI_HCI_ADMIN_PARAM_SESSION_IDENTITY, &skb); |
741 | if (r < 0) |
742 | return r; |
743 | |
744 | if (skb->len && |
745 | skb->len == strlen(ndev->hci_dev->init_data.session_id) && |
746 | !memcmp(p: ndev->hci_dev->init_data.session_id, q: skb->data, size: skb->len) && |
747 | ndev->ops->hci_load_session) { |
748 | /* Restore gate<->pipe table from some proprietary location. */ |
749 | r = ndev->ops->hci_load_session(ndev); |
750 | } else { |
751 | r = nci_hci_clear_all_pipes(ndev); |
752 | if (r < 0) |
753 | goto exit; |
754 | |
755 | r = nci_hci_dev_connect_gates(ndev, |
756 | gate_count: ndev->hci_dev->init_data.gate_count, |
757 | gates: ndev->hci_dev->init_data.gates); |
758 | if (r < 0) |
759 | goto exit; |
760 | |
761 | r = nci_hci_set_param(ndev, NCI_HCI_ADMIN_GATE, |
762 | NCI_HCI_ADMIN_PARAM_SESSION_IDENTITY, |
763 | ndev->hci_dev->init_data.session_id, |
764 | strlen(ndev->hci_dev->init_data.session_id)); |
765 | } |
766 | |
767 | exit: |
768 | kfree_skb(skb); |
769 | |
770 | return r; |
771 | } |
772 | EXPORT_SYMBOL(nci_hci_dev_session_init); |
773 | |
774 | struct nci_hci_dev *nci_hci_allocate(struct nci_dev *ndev) |
775 | { |
776 | struct nci_hci_dev *hdev; |
777 | |
778 | hdev = kzalloc(size: sizeof(*hdev), GFP_KERNEL); |
779 | if (!hdev) |
780 | return NULL; |
781 | |
782 | skb_queue_head_init(list: &hdev->rx_hcp_frags); |
783 | INIT_WORK(&hdev->msg_rx_work, nci_hci_msg_rx_work); |
784 | skb_queue_head_init(list: &hdev->msg_rx_queue); |
785 | hdev->ndev = ndev; |
786 | |
787 | return hdev; |
788 | } |
789 | |
790 | void nci_hci_deallocate(struct nci_dev *ndev) |
791 | { |
792 | kfree(objp: ndev->hci_dev); |
793 | } |
794 | |