Bluetooth: Add support for set_powered management command
[linux-2.6.git] / net / bluetooth / mgmt.c
1 /*
2    BlueZ - Bluetooth protocol stack for Linux
3    Copyright (C) 2010  Nokia Corporation
4
5    This program is free software; you can redistribute it and/or modify
6    it under the terms of the GNU General Public License version 2 as
7    published by the Free Software Foundation;
8
9    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
10    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
11    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
12    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
13    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
14    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
15    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
16    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
17
18    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
19    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
20    SOFTWARE IS DISCLAIMED.
21 */
22
23 /* Bluetooth HCI Management interface */
24
25 #include <asm/uaccess.h>
26 #include <asm/unaligned.h>
27
28 #include <net/bluetooth/bluetooth.h>
29 #include <net/bluetooth/hci_core.h>
30 #include <net/bluetooth/mgmt.h>
31
32 #define MGMT_VERSION    0
33 #define MGMT_REVISION   1
34
35 struct pending_cmd {
36         struct list_head list;
37         __u16 opcode;
38         int index;
39         void *cmd;
40         struct sock *sk;
41 };
42
43 LIST_HEAD(cmd_list);
44
45 static int cmd_status(struct sock *sk, u16 cmd, u8 status)
46 {
47         struct sk_buff *skb;
48         struct mgmt_hdr *hdr;
49         struct mgmt_ev_cmd_status *ev;
50
51         BT_DBG("sock %p", sk);
52
53         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_ATOMIC);
54         if (!skb)
55                 return -ENOMEM;
56
57         hdr = (void *) skb_put(skb, sizeof(*hdr));
58
59         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
60         hdr->len = cpu_to_le16(sizeof(*ev));
61
62         ev = (void *) skb_put(skb, sizeof(*ev));
63         ev->status = status;
64         put_unaligned_le16(cmd, &ev->opcode);
65
66         if (sock_queue_rcv_skb(sk, skb) < 0)
67                 kfree_skb(skb);
68
69         return 0;
70 }
71
72 static int read_version(struct sock *sk)
73 {
74         struct sk_buff *skb;
75         struct mgmt_hdr *hdr;
76         struct mgmt_ev_cmd_complete *ev;
77         struct mgmt_rp_read_version *rp;
78
79         BT_DBG("sock %p", sk);
80
81         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
82         if (!skb)
83                 return -ENOMEM;
84
85         hdr = (void *) skb_put(skb, sizeof(*hdr));
86         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
87         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
88
89         ev = (void *) skb_put(skb, sizeof(*ev));
90         put_unaligned_le16(MGMT_OP_READ_VERSION, &ev->opcode);
91
92         rp = (void *) skb_put(skb, sizeof(*rp));
93         rp->version = MGMT_VERSION;
94         put_unaligned_le16(MGMT_REVISION, &rp->revision);
95
96         if (sock_queue_rcv_skb(sk, skb) < 0)
97                 kfree_skb(skb);
98
99         return 0;
100 }
101
102 static int read_index_list(struct sock *sk)
103 {
104         struct sk_buff *skb;
105         struct mgmt_hdr *hdr;
106         struct mgmt_ev_cmd_complete *ev;
107         struct mgmt_rp_read_index_list *rp;
108         struct list_head *p;
109         size_t body_len;
110         u16 count;
111         int i;
112
113         BT_DBG("sock %p", sk);
114
115         read_lock(&hci_dev_list_lock);
116
117         count = 0;
118         list_for_each(p, &hci_dev_list) {
119                 count++;
120         }
121
122         body_len = sizeof(*ev) + sizeof(*rp) + (2 * count);
123         skb = alloc_skb(sizeof(*hdr) + body_len, GFP_ATOMIC);
124         if (!skb) {
125                 read_unlock(&hci_dev_list_lock);
126                 return -ENOMEM;
127         }
128
129         hdr = (void *) skb_put(skb, sizeof(*hdr));
130         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
131         hdr->len = cpu_to_le16(body_len);
132
133         ev = (void *) skb_put(skb, sizeof(*ev));
134         put_unaligned_le16(MGMT_OP_READ_INDEX_LIST, &ev->opcode);
135
136         rp = (void *) skb_put(skb, sizeof(*rp) + (2 * count));
137         put_unaligned_le16(count, &rp->num_controllers);
138
139         i = 0;
140         list_for_each(p, &hci_dev_list) {
141                 struct hci_dev *d = list_entry(p, struct hci_dev, list);
142
143                 hci_del_off_timer(d);
144
145                 if (test_bit(HCI_SETUP, &d->flags))
146                         continue;
147
148                 put_unaligned_le16(d->id, &rp->index[i++]);
149                 BT_DBG("Added hci%u", d->id);
150         }
151
152         read_unlock(&hci_dev_list_lock);
153
154         if (sock_queue_rcv_skb(sk, skb) < 0)
155                 kfree_skb(skb);
156
157         return 0;
158 }
159
160 static int read_controller_info(struct sock *sk, unsigned char *data, u16 len)
161 {
162         struct sk_buff *skb;
163         struct mgmt_hdr *hdr;
164         struct mgmt_ev_cmd_complete *ev;
165         struct mgmt_rp_read_info *rp;
166         struct mgmt_cp_read_info *cp;
167         struct hci_dev *hdev;
168         u16 dev_id;
169
170         BT_DBG("sock %p", sk);
171
172         if (len != 2)
173                 return cmd_status(sk, MGMT_OP_READ_INFO, EINVAL);
174
175         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
176         if (!skb)
177                 return -ENOMEM;
178
179         hdr = (void *) skb_put(skb, sizeof(*hdr));
180         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
181         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
182
183         ev = (void *) skb_put(skb, sizeof(*ev));
184         put_unaligned_le16(MGMT_OP_READ_INFO, &ev->opcode);
185
186         rp = (void *) skb_put(skb, sizeof(*rp));
187
188         cp = (void *) data;
189         dev_id = get_unaligned_le16(&cp->index);
190
191         BT_DBG("request for hci%u", dev_id);
192
193         hdev = hci_dev_get(dev_id);
194         if (!hdev) {
195                 kfree_skb(skb);
196                 return cmd_status(sk, MGMT_OP_READ_INFO, ENODEV);
197         }
198
199         hci_del_off_timer(hdev);
200
201         hci_dev_lock_bh(hdev);
202
203         put_unaligned_le16(hdev->id, &rp->index);
204         rp->type = hdev->dev_type;
205
206         rp->powered = test_bit(HCI_UP, &hdev->flags);
207         rp->discoverable = test_bit(HCI_ISCAN, &hdev->flags);
208         rp->pairable = test_bit(HCI_PSCAN, &hdev->flags);
209
210         if (test_bit(HCI_AUTH, &hdev->flags))
211                 rp->sec_mode = 3;
212         else if (hdev->ssp_mode > 0)
213                 rp->sec_mode = 4;
214         else
215                 rp->sec_mode = 2;
216
217         bacpy(&rp->bdaddr, &hdev->bdaddr);
218         memcpy(rp->features, hdev->features, 8);
219         memcpy(rp->dev_class, hdev->dev_class, 3);
220         put_unaligned_le16(hdev->manufacturer, &rp->manufacturer);
221         rp->hci_ver = hdev->hci_ver;
222         put_unaligned_le16(hdev->hci_rev, &rp->hci_rev);
223
224         hci_dev_unlock_bh(hdev);
225         hci_dev_put(hdev);
226
227         if (sock_queue_rcv_skb(sk, skb) < 0)
228                 kfree_skb(skb);
229
230         return 0;
231 }
232
233 static void mgmt_pending_free(struct pending_cmd *cmd)
234 {
235         sock_put(cmd->sk);
236         kfree(cmd->cmd);
237         kfree(cmd);
238 }
239
240 static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
241                                                         void *data, u16 len)
242 {
243         struct pending_cmd *cmd;
244
245         cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
246         if (!cmd)
247                 return -ENOMEM;
248
249         cmd->opcode = opcode;
250         cmd->index = index;
251
252         cmd->cmd = kmalloc(len, GFP_ATOMIC);
253         if (!cmd->cmd) {
254                 kfree(cmd);
255                 return -ENOMEM;
256         }
257
258         memcpy(cmd->cmd, data, len);
259
260         cmd->sk = sk;
261         sock_hold(sk);
262
263         list_add(&cmd->list, &cmd_list);
264
265         return 0;
266 }
267
268 static void mgmt_pending_foreach(u16 opcode, int index,
269                                 void (*cb)(struct pending_cmd *cmd, void *data),
270                                 void *data)
271 {
272         struct list_head *p, *n;
273
274         list_for_each_safe(p, n, &cmd_list) {
275                 struct pending_cmd *cmd;
276
277                 cmd = list_entry(p, struct pending_cmd, list);
278
279                 if (cmd->opcode != opcode)
280                         continue;
281
282                 if (index >= 0 && cmd->index != index)
283                         continue;
284
285                 cb(cmd, data);
286         }
287 }
288
289 static struct pending_cmd *mgmt_pending_find(u16 opcode, int index)
290 {
291         struct list_head *p;
292
293         list_for_each(p, &cmd_list) {
294                 struct pending_cmd *cmd;
295
296                 cmd = list_entry(p, struct pending_cmd, list);
297
298                 if (cmd->opcode != opcode)
299                         continue;
300
301                 if (index >= 0 && cmd->index != index)
302                         continue;
303
304                 return cmd;
305         }
306
307         return NULL;
308 }
309
310 static int set_powered(struct sock *sk, unsigned char *data, u16 len)
311 {
312         struct mgmt_cp_set_powered *cp;
313         struct hci_dev *hdev;
314         u16 dev_id;
315         int ret, up;
316
317         cp = (void *) data;
318         dev_id = get_unaligned_le16(&cp->index);
319
320         BT_DBG("request for hci%u", dev_id);
321
322         hdev = hci_dev_get(dev_id);
323         if (!hdev)
324                 return cmd_status(sk, MGMT_OP_SET_POWERED, ENODEV);
325
326         hci_dev_lock_bh(hdev);
327
328         up = test_bit(HCI_UP, &hdev->flags);
329         if ((cp->powered && up) || (!cp->powered && !up)) {
330                 ret = cmd_status(sk, MGMT_OP_SET_POWERED, EALREADY);
331                 goto failed;
332         }
333
334         if (mgmt_pending_find(MGMT_OP_SET_POWERED, dev_id)) {
335                 ret = cmd_status(sk, MGMT_OP_SET_POWERED, EBUSY);
336                 goto failed;
337         }
338
339         ret = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, dev_id, data, len);
340         if (ret < 0)
341                 goto failed;
342
343         if (cp->powered)
344                 queue_work(hdev->workqueue, &hdev->power_on);
345         else
346                 queue_work(hdev->workqueue, &hdev->power_off);
347
348         ret = 0;
349
350 failed:
351         hci_dev_unlock_bh(hdev);
352         hci_dev_put(hdev);
353         return ret;
354 }
355
356 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
357 {
358         unsigned char *buf;
359         struct mgmt_hdr *hdr;
360         u16 opcode, len;
361         int err;
362
363         BT_DBG("got %zu bytes", msglen);
364
365         if (msglen < sizeof(*hdr))
366                 return -EINVAL;
367
368         buf = kmalloc(msglen, GFP_ATOMIC);
369         if (!buf)
370                 return -ENOMEM;
371
372         if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) {
373                 err = -EFAULT;
374                 goto done;
375         }
376
377         hdr = (struct mgmt_hdr *) buf;
378         opcode = get_unaligned_le16(&hdr->opcode);
379         len = get_unaligned_le16(&hdr->len);
380
381         if (len != msglen - sizeof(*hdr)) {
382                 err = -EINVAL;
383                 goto done;
384         }
385
386         switch (opcode) {
387         case MGMT_OP_READ_VERSION:
388                 err = read_version(sk);
389                 break;
390         case MGMT_OP_READ_INDEX_LIST:
391                 err = read_index_list(sk);
392                 break;
393         case MGMT_OP_READ_INFO:
394                 err = read_controller_info(sk, buf + sizeof(*hdr), len);
395                 break;
396         case MGMT_OP_SET_POWERED:
397                 err = set_powered(sk, buf + sizeof(*hdr), len);
398                 break;
399         default:
400                 BT_DBG("Unknown op %u", opcode);
401                 err = cmd_status(sk, opcode, 0x01);
402                 break;
403         }
404
405         if (err < 0)
406                 goto done;
407
408         err = msglen;
409
410 done:
411         kfree(buf);
412         return err;
413 }
414
415 static int mgmt_event(u16 event, void *data, u16 data_len, struct sock *skip_sk)
416 {
417         struct sk_buff *skb;
418         struct mgmt_hdr *hdr;
419
420         skb = alloc_skb(sizeof(*hdr) + data_len, GFP_ATOMIC);
421         if (!skb)
422                 return -ENOMEM;
423
424         bt_cb(skb)->channel = HCI_CHANNEL_CONTROL;
425
426         hdr = (void *) skb_put(skb, sizeof(*hdr));
427         hdr->opcode = cpu_to_le16(event);
428         hdr->len = cpu_to_le16(data_len);
429
430         memcpy(skb_put(skb, data_len), data, data_len);
431
432         hci_send_to_sock(NULL, skb, skip_sk);
433         kfree_skb(skb);
434
435         return 0;
436 }
437
438 int mgmt_index_added(u16 index)
439 {
440         struct mgmt_ev_index_added ev;
441
442         put_unaligned_le16(index, &ev.index);
443
444         return mgmt_event(MGMT_EV_INDEX_ADDED, &ev, sizeof(ev), NULL);
445 }
446
447 int mgmt_index_removed(u16 index)
448 {
449         struct mgmt_ev_index_added ev;
450
451         put_unaligned_le16(index, &ev.index);
452
453         return mgmt_event(MGMT_EV_INDEX_REMOVED, &ev, sizeof(ev), NULL);
454 }
455
456 struct powered_lookup {
457         u8 powered;
458         struct sock *sk;
459 };
460
461 static void power_rsp(struct pending_cmd *cmd, void *data)
462 {
463         struct mgmt_hdr *hdr;
464         struct mgmt_ev_cmd_complete *ev;
465         struct mgmt_rp_set_powered *rp;
466         struct mgmt_cp_set_powered *cp = cmd->cmd;
467         struct sk_buff *skb;
468         struct powered_lookup *match = data;
469
470         if (cp->powered != match->powered)
471                 return;
472
473         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
474         if (!skb)
475                 return;
476
477         hdr = (void *) skb_put(skb, sizeof(*hdr));
478         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
479         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
480
481         ev = (void *) skb_put(skb, sizeof(*ev));
482         put_unaligned_le16(cmd->opcode, &ev->opcode);
483
484         rp = (void *) skb_put(skb, sizeof(*rp));
485         put_unaligned_le16(cmd->index, &rp->index);
486         rp->powered = cp->powered;
487
488         if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
489                 kfree_skb(skb);
490
491         list_del(&cmd->list);
492
493         if (match->sk == NULL) {
494                 match->sk = cmd->sk;
495                 sock_hold(match->sk);
496         }
497
498         mgmt_pending_free(cmd);
499 }
500
501 int mgmt_powered(u16 index, u8 powered)
502 {
503         struct mgmt_ev_powered ev;
504         struct powered_lookup match = { powered, NULL };
505         int ret;
506
507         put_unaligned_le16(index, &ev.index);
508         ev.powered = powered;
509
510         mgmt_pending_foreach(MGMT_OP_SET_POWERED, index, power_rsp, &match);
511
512         ret = mgmt_event(MGMT_EV_POWERED, &ev, sizeof(ev), match.sk);
513
514         if (match.sk)
515                 sock_put(match.sk);
516
517         return ret;
518 }