blob: a7b9e9bb3e8d54017480857d2b7cc6dbceedcc6f [file] [log] [blame]
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -07001/*
2 * connector.c
3 *
4 * 2004-2005 Copyright (c) Evgeniy Polyakov <johnpol@2ka.mipt.ru>
5 * All rights reserved.
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
20 */
21
22#include <linux/kernel.h>
23#include <linux/module.h>
24#include <linux/list.h>
25#include <linux/skbuff.h>
26#include <linux/netlink.h>
27#include <linux/moduleparam.h>
28#include <linux/connector.h>
Arjan van de Ven8ed965d2006-03-23 03:00:21 -080029#include <linux/mutex.h>
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -070030
31#include <net/sock.h>
32
33MODULE_LICENSE("GPL");
34MODULE_AUTHOR("Evgeniy Polyakov <johnpol@2ka.mipt.ru>");
35MODULE_DESCRIPTION("Generic userspace <-> kernelspace connector.");
36
37static u32 cn_idx = CN_IDX_CONNECTOR;
38static u32 cn_val = CN_VAL_CONNECTOR;
39
40module_param(cn_idx, uint, 0);
41module_param(cn_val, uint, 0);
42MODULE_PARM_DESC(cn_idx, "Connector's main device idx.");
43MODULE_PARM_DESC(cn_val, "Connector's main device val.");
44
Arjan van de Ven8ed965d2006-03-23 03:00:21 -080045static DEFINE_MUTEX(notify_lock);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -070046static LIST_HEAD(notify_list);
47
48static struct cn_dev cdev;
49
50int cn_already_initialized = 0;
51
52/*
53 * msg->seq and msg->ack are used to determine message genealogy.
54 * When someone sends message it puts there locally unique sequence
55 * and random acknowledge numbers. Sequence number may be copied into
56 * nlmsghdr->nlmsg_seq too.
57 *
58 * Sequence number is incremented with each message to be sent.
59 *
60 * If we expect reply to our message then the sequence number in
61 * received message MUST be the same as in original message, and
62 * acknowledge number MUST be the same + 1.
63 *
64 * If we receive a message and its sequence number is not equal to the
65 * one we are expecting then it is a new message.
66 *
67 * If we receive a message and its sequence number is the same as one
68 * we are expecting but it's acknowledgement number is not equal to
69 * the acknowledgement number in the original message + 1, then it is
70 * a new message.
71 *
72 */
Al Virodd0fc662005-10-07 07:46:04 +010073int cn_netlink_send(struct cn_msg *msg, u32 __group, gfp_t gfp_mask)
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -070074{
75 struct cn_callback_entry *__cbq;
76 unsigned int size;
77 struct sk_buff *skb;
78 struct nlmsghdr *nlh;
79 struct cn_msg *data;
80 struct cn_dev *dev = &cdev;
81 u32 group = 0;
82 int found = 0;
83
84 if (!__group) {
85 spin_lock_bh(&dev->cbdev->queue_lock);
86 list_for_each_entry(__cbq, &dev->cbdev->queue_list,
87 callback_entry) {
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -070088 if (cn_cb_equal(&__cbq->id.id, &msg->id)) {
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -070089 found = 1;
90 group = __cbq->group;
91 }
92 }
93 spin_unlock_bh(&dev->cbdev->queue_lock);
94
95 if (!found)
96 return -ENODEV;
97 } else {
98 group = __group;
99 }
100
Evgeniy Polyakovb191ba02006-03-20 22:21:40 -0800101 if (!netlink_has_listeners(dev->nls, group))
102 return -ESRCH;
103
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700104 size = NLMSG_SPACE(sizeof(*msg) + msg->len);
105
106 skb = alloc_skb(size, gfp_mask);
107 if (!skb)
108 return -ENOMEM;
109
110 nlh = NLMSG_PUT(skb, 0, msg->seq, NLMSG_DONE, size - sizeof(*nlh));
111
112 data = NLMSG_DATA(nlh);
113
114 memcpy(data, msg, sizeof(*data) + msg->len);
115
116 NETLINK_CB(skb).dst_group = group;
117
Evgeniy Polyakovb191ba02006-03-20 22:21:40 -0800118 return netlink_broadcast(dev->nls, skb, 0, group, gfp_mask);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700119
120nlmsg_failure:
121 kfree_skb(skb);
122 return -EINVAL;
123}
Andrew Mortondeb0e9b2006-06-23 02:05:46 -0700124EXPORT_SYMBOL_GPL(cn_netlink_send);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700125
126/*
127 * Callback helper - queues work and setup destructor for given data.
128 */
129static int cn_call_callback(struct cn_msg *msg, void (*destruct_data)(void *), void *data)
130{
Philipp Reisner05e52dd2007-03-07 12:55:39 -0800131 struct cn_callback_entry *__cbq, *__new_cbq;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700132 struct cn_dev *dev = &cdev;
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700133 int err = -ENODEV;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700134
135 spin_lock_bh(&dev->cbdev->queue_lock);
136 list_for_each_entry(__cbq, &dev->cbdev->queue_list, callback_entry) {
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700137 if (cn_cb_equal(&__cbq->id.id, &msg->id)) {
Evgeniy Polyakova240d9f2006-12-18 01:53:58 -0800138 if (likely(!work_pending(&__cbq->work) &&
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700139 __cbq->data.ddata == NULL)) {
140 __cbq->data.callback_priv = msg;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700141
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700142 __cbq->data.ddata = data;
143 __cbq->data.destruct_data = destruct_data;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700144
Evgeniy Polyakova240d9f2006-12-18 01:53:58 -0800145 if (queue_work(dev->cbdev->cn_queue,
146 &__cbq->work))
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700147 err = 0;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700148 } else {
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700149 struct cn_callback_data *d;
150
Philipp Reisner05e52dd2007-03-07 12:55:39 -0800151 err = -ENOMEM;
152 __new_cbq = kzalloc(sizeof(struct cn_callback_entry), GFP_ATOMIC);
153 if (__new_cbq) {
154 d = &__new_cbq->data;
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700155 d->callback_priv = msg;
156 d->callback = __cbq->data.callback;
157 d->ddata = data;
158 d->destruct_data = destruct_data;
Philipp Reisner05e52dd2007-03-07 12:55:39 -0800159 d->free = __new_cbq;
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700160
Philipp Reisner05e52dd2007-03-07 12:55:39 -0800161 INIT_WORK(&__new_cbq->work,
Evgeniy Polyakova240d9f2006-12-18 01:53:58 -0800162 &cn_queue_wrapper);
Philipp Reisner05e52dd2007-03-07 12:55:39 -0800163
Evgeniy Polyakova240d9f2006-12-18 01:53:58 -0800164 if (queue_work(dev->cbdev->cn_queue,
Philipp Reisner05e52dd2007-03-07 12:55:39 -0800165 &__new_cbq->work))
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700166 err = 0;
167 else {
Philipp Reisner05e52dd2007-03-07 12:55:39 -0800168 kfree(__new_cbq);
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700169 err = -EINVAL;
170 }
Philipp Reisner05e52dd2007-03-07 12:55:39 -0800171 }
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700172 }
173 break;
174 }
175 }
176 spin_unlock_bh(&dev->cbdev->queue_lock);
177
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700178 return err;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700179}
180
181/*
182 * Skb receive helper - checks skb and msg size and calls callback
183 * helper.
184 */
185static int __cn_rx_skb(struct sk_buff *skb, struct nlmsghdr *nlh)
186{
187 u32 pid, uid, seq, group;
188 struct cn_msg *msg;
189
190 pid = NETLINK_CREDS(skb)->pid;
191 uid = NETLINK_CREDS(skb)->uid;
192 seq = nlh->nlmsg_seq;
193 group = NETLINK_CB((skb)).dst_group;
194 msg = NLMSG_DATA(nlh);
195
196 return cn_call_callback(msg, (void (*)(void *))kfree_skb, skb);
197}
198
199/*
200 * Main netlink receiving function.
201 *
202 * It checks skb and netlink header sizes and calls the skb receive
203 * helper with a shared skb.
204 */
205static void cn_rx_skb(struct sk_buff *__skb)
206{
207 struct nlmsghdr *nlh;
208 u32 len;
209 int err;
210 struct sk_buff *skb;
211
212 skb = skb_get(__skb);
213
214 if (skb->len >= NLMSG_SPACE(0)) {
Arnaldo Carvalho de Melob529ccf2007-04-25 19:08:35 -0700215 nlh = nlmsg_hdr(skb);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700216
217 if (nlh->nlmsg_len < sizeof(struct cn_msg) ||
218 skb->len < nlh->nlmsg_len ||
219 nlh->nlmsg_len > CONNECTOR_MAX_MSG_SIZE) {
220 kfree_skb(skb);
221 goto out;
222 }
223
224 len = NLMSG_ALIGN(nlh->nlmsg_len);
225 if (len > skb->len)
226 len = skb->len;
227
228 err = __cn_rx_skb(skb, nlh);
229 if (err < 0)
230 kfree_skb(skb);
231 }
232
233out:
234 kfree_skb(__skb);
235}
236
237/*
238 * Netlink socket input callback - dequeues the skbs and calls the
239 * main netlink receiving function.
240 */
241static void cn_input(struct sock *sk, int len)
242{
243 struct sk_buff *skb;
244
245 while ((skb = skb_dequeue(&sk->sk_receive_queue)) != NULL)
246 cn_rx_skb(skb);
247}
248
249/*
250 * Notification routing.
251 *
252 * Gets id and checks if there are notification request for it's idx
253 * and val. If there are such requests notify the listeners with the
254 * given notify event.
255 *
256 */
257static void cn_notify(struct cb_id *id, u32 notify_event)
258{
259 struct cn_ctl_entry *ent;
260
Arjan van de Ven8ed965d2006-03-23 03:00:21 -0800261 mutex_lock(&notify_lock);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700262 list_for_each_entry(ent, &notify_list, notify_entry) {
263 int i;
264 struct cn_notify_req *req;
265 struct cn_ctl_msg *ctl = ent->msg;
266 int idx_found, val_found;
267
268 idx_found = val_found = 0;
269
270 req = (struct cn_notify_req *)ctl->data;
271 for (i = 0; i < ctl->idx_notify_num; ++i, ++req) {
272 if (id->idx >= req->first &&
273 id->idx < req->first + req->range) {
274 idx_found = 1;
275 break;
276 }
277 }
278
279 for (i = 0; i < ctl->val_notify_num; ++i, ++req) {
280 if (id->val >= req->first &&
281 id->val < req->first + req->range) {
282 val_found = 1;
283 break;
284 }
285 }
286
287 if (idx_found && val_found) {
288 struct cn_msg m = { .ack = notify_event, };
289
290 memcpy(&m.id, id, sizeof(m.id));
291 cn_netlink_send(&m, ctl->group, GFP_KERNEL);
292 }
293 }
Arjan van de Ven8ed965d2006-03-23 03:00:21 -0800294 mutex_unlock(&notify_lock);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700295}
296
297/*
298 * Callback add routing - adds callback with given ID and name.
299 * If there is registered callback with the same ID it will not be added.
300 *
301 * May sleep.
302 */
303int cn_add_callback(struct cb_id *id, char *name, void (*callback)(void *))
304{
305 int err;
306 struct cn_dev *dev = &cdev;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700307
Evgeniy Polyakovd6cc7f12006-06-19 23:42:53 -0700308 if (!cn_already_initialized)
309 return -EAGAIN;
310
Evgeniy Polyakovacd042b2005-09-26 15:06:50 -0700311 err = cn_queue_add_callback(dev->cbdev, name, id, callback);
312 if (err)
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700313 return err;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700314
315 cn_notify(id, 0);
316
317 return 0;
318}
Andrew Mortondeb0e9b2006-06-23 02:05:46 -0700319EXPORT_SYMBOL_GPL(cn_add_callback);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700320
321/*
322 * Callback remove routing - removes callback
323 * with given ID.
324 * If there is no registered callback with given
325 * ID nothing happens.
326 *
327 * May sleep while waiting for reference counter to become zero.
328 */
329void cn_del_callback(struct cb_id *id)
330{
331 struct cn_dev *dev = &cdev;
332
333 cn_queue_del_callback(dev->cbdev, id);
334 cn_notify(id, 1);
335}
Andrew Mortondeb0e9b2006-06-23 02:05:46 -0700336EXPORT_SYMBOL_GPL(cn_del_callback);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700337
338/*
339 * Checks two connector's control messages to be the same.
340 * Returns 1 if they are the same or if the first one is corrupted.
341 */
342static int cn_ctl_msg_equals(struct cn_ctl_msg *m1, struct cn_ctl_msg *m2)
343{
344 int i;
345 struct cn_notify_req *req1, *req2;
346
347 if (m1->idx_notify_num != m2->idx_notify_num)
348 return 0;
349
350 if (m1->val_notify_num != m2->val_notify_num)
351 return 0;
352
353 if (m1->len != m2->len)
354 return 0;
355
356 if ((m1->idx_notify_num + m1->val_notify_num) * sizeof(*req1) !=
357 m1->len)
358 return 1;
359
360 req1 = (struct cn_notify_req *)m1->data;
361 req2 = (struct cn_notify_req *)m2->data;
362
363 for (i = 0; i < m1->idx_notify_num; ++i) {
364 if (req1->first != req2->first || req1->range != req2->range)
365 return 0;
366 req1++;
367 req2++;
368 }
369
370 for (i = 0; i < m1->val_notify_num; ++i) {
371 if (req1->first != req2->first || req1->range != req2->range)
372 return 0;
373 req1++;
374 req2++;
375 }
376
377 return 1;
378}
379
380/*
381 * Main connector device's callback.
382 *
383 * Used for notification of a request's processing.
384 */
385static void cn_callback(void *data)
386{
387 struct cn_msg *msg = data;
388 struct cn_ctl_msg *ctl;
389 struct cn_ctl_entry *ent;
390 u32 size;
391
392 if (msg->len < sizeof(*ctl))
393 return;
394
395 ctl = (struct cn_ctl_msg *)msg->data;
396
397 size = (sizeof(*ctl) + ((ctl->idx_notify_num +
398 ctl->val_notify_num) *
399 sizeof(struct cn_notify_req)));
400
401 if (msg->len != size)
402 return;
403
404 if (ctl->len + sizeof(*ctl) != msg->len)
405 return;
406
407 /*
408 * Remove notification.
409 */
410 if (ctl->group == 0) {
411 struct cn_ctl_entry *n;
412
Arjan van de Ven8ed965d2006-03-23 03:00:21 -0800413 mutex_lock(&notify_lock);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700414 list_for_each_entry_safe(ent, n, &notify_list, notify_entry) {
415 if (cn_ctl_msg_equals(ent->msg, ctl)) {
416 list_del(&ent->notify_entry);
417 kfree(ent);
418 }
419 }
Arjan van de Ven8ed965d2006-03-23 03:00:21 -0800420 mutex_unlock(&notify_lock);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700421
422 return;
423 }
424
425 size += sizeof(*ent);
426
427 ent = kzalloc(size, GFP_KERNEL);
428 if (!ent)
429 return;
430
431 ent->msg = (struct cn_ctl_msg *)(ent + 1);
432
433 memcpy(ent->msg, ctl, size - sizeof(*ent));
434
Arjan van de Ven8ed965d2006-03-23 03:00:21 -0800435 mutex_lock(&notify_lock);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700436 list_add(&ent->notify_entry, &notify_list);
Arjan van de Ven8ed965d2006-03-23 03:00:21 -0800437 mutex_unlock(&notify_lock);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700438}
439
Evgeniy Polyakovd6cc7f12006-06-19 23:42:53 -0700440static int __devinit cn_init(void)
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700441{
442 struct cn_dev *dev = &cdev;
443 int err;
444
445 dev->input = cn_input;
446 dev->id.idx = cn_idx;
447 dev->id.val = cn_val;
448
449 dev->nls = netlink_kernel_create(NETLINK_CONNECTOR,
450 CN_NETLINK_USERS + 0xf,
Patrick McHardyaf65bdf2007-04-20 14:14:21 -0700451 dev->input, NULL, THIS_MODULE);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700452 if (!dev->nls)
453 return -EIO;
454
455 dev->cbdev = cn_queue_alloc_dev("cqueue", dev->nls);
456 if (!dev->cbdev) {
457 if (dev->nls->sk_socket)
458 sock_release(dev->nls->sk_socket);
459 return -EINVAL;
460 }
Evgeniy Polyakovd6cc7f12006-06-19 23:42:53 -0700461
462 cn_already_initialized = 1;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700463
464 err = cn_add_callback(&dev->id, "connector", &cn_callback);
465 if (err) {
Evgeniy Polyakovd6cc7f12006-06-19 23:42:53 -0700466 cn_already_initialized = 0;
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700467 cn_queue_free_dev(dev->cbdev);
468 if (dev->nls->sk_socket)
469 sock_release(dev->nls->sk_socket);
470 return -EINVAL;
471 }
472
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700473 return 0;
474}
475
Evgeniy Polyakovd6cc7f12006-06-19 23:42:53 -0700476static void __devexit cn_fini(void)
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700477{
478 struct cn_dev *dev = &cdev;
479
480 cn_already_initialized = 0;
481
482 cn_del_callback(&dev->id);
483 cn_queue_free_dev(dev->cbdev);
484 if (dev->nls->sk_socket)
485 sock_release(dev->nls->sk_socket);
486}
487
Evgeniy Polyakovd6cc7f12006-06-19 23:42:53 -0700488subsys_initcall(cn_init);
Evgeniy Polyakov7672d0b2005-09-11 19:15:07 -0700489module_exit(cn_fini);