11a59d1b8SThomas Gleixner // SPDX-License-Identifier: GPL-2.0-or-later
27672d0b5SEvgeniy Polyakov /*
37672d0b5SEvgeniy Polyakov * cn_queue.c
47672d0b5SEvgeniy Polyakov *
5acb9c1b2SEvgeniy Polyakov * 2004+ Copyright (c) Evgeniy Polyakov <zbr@ioremap.net>
67672d0b5SEvgeniy Polyakov * All rights reserved.
77672d0b5SEvgeniy Polyakov */
87672d0b5SEvgeniy Polyakov
97672d0b5SEvgeniy Polyakov #include <linux/kernel.h>
107672d0b5SEvgeniy Polyakov #include <linux/module.h>
117672d0b5SEvgeniy Polyakov #include <linux/list.h>
127672d0b5SEvgeniy Polyakov #include <linux/workqueue.h>
137672d0b5SEvgeniy Polyakov #include <linux/spinlock.h>
147672d0b5SEvgeniy Polyakov #include <linux/slab.h>
157672d0b5SEvgeniy Polyakov #include <linux/skbuff.h>
167672d0b5SEvgeniy Polyakov #include <linux/suspend.h>
177672d0b5SEvgeniy Polyakov #include <linux/connector.h>
187672d0b5SEvgeniy Polyakov #include <linux/delay.h>
197672d0b5SEvgeniy Polyakov
200741241cSMike Frysinger static struct cn_callback_entry *
cn_queue_alloc_callback_entry(struct cn_queue_dev * dev,const char * name,const struct cb_id * id,void (* callback)(struct cn_msg *,struct netlink_skb_parms *))2104f482faSPatrick McHardy cn_queue_alloc_callback_entry(struct cn_queue_dev *dev, const char *name,
22*c18e6869SGeoff Levand const struct cb_id *id,
23f3c48eccSValentin Ilie void (*callback)(struct cn_msg *,
24f3c48eccSValentin Ilie struct netlink_skb_parms *))
257672d0b5SEvgeniy Polyakov {
267672d0b5SEvgeniy Polyakov struct cn_callback_entry *cbq;
277672d0b5SEvgeniy Polyakov
287672d0b5SEvgeniy Polyakov cbq = kzalloc(sizeof(*cbq), GFP_KERNEL);
297672d0b5SEvgeniy Polyakov if (!cbq) {
30f3c48eccSValentin Ilie pr_err("Failed to create new callback queue.\n");
317672d0b5SEvgeniy Polyakov return NULL;
327672d0b5SEvgeniy Polyakov }
337672d0b5SEvgeniy Polyakov
34e65f7ee3SElena Reshetova refcount_set(&cbq->refcnt, 1);
3504f482faSPatrick McHardy
3604f482faSPatrick McHardy atomic_inc(&dev->refcnt);
3704f482faSPatrick McHardy cbq->pdev = dev;
3804f482faSPatrick McHardy
39acd042bbSEvgeniy Polyakov snprintf(cbq->id.name, sizeof(cbq->id.name), "%s", name);
40acd042bbSEvgeniy Polyakov memcpy(&cbq->id.id, id, sizeof(struct cb_id));
4104f482faSPatrick McHardy cbq->callback = callback;
427672d0b5SEvgeniy Polyakov return cbq;
437672d0b5SEvgeniy Polyakov }
447672d0b5SEvgeniy Polyakov
cn_queue_release_callback(struct cn_callback_entry * cbq)4504f482faSPatrick McHardy void cn_queue_release_callback(struct cn_callback_entry *cbq)
467672d0b5SEvgeniy Polyakov {
47e65f7ee3SElena Reshetova if (!refcount_dec_and_test(&cbq->refcnt))
4804f482faSPatrick McHardy return;
4904f482faSPatrick McHardy
5004f482faSPatrick McHardy atomic_dec(&cbq->pdev->refcnt);
517672d0b5SEvgeniy Polyakov kfree(cbq);
527672d0b5SEvgeniy Polyakov }
537672d0b5SEvgeniy Polyakov
cn_cb_equal(const struct cb_id * i1,const struct cb_id * i2)54*c18e6869SGeoff Levand int cn_cb_equal(const struct cb_id *i1, const struct cb_id *i2)
557672d0b5SEvgeniy Polyakov {
567672d0b5SEvgeniy Polyakov return ((i1->idx == i2->idx) && (i1->val == i2->val));
577672d0b5SEvgeniy Polyakov }
587672d0b5SEvgeniy Polyakov
cn_queue_add_callback(struct cn_queue_dev * dev,const char * name,const struct cb_id * id,void (* callback)(struct cn_msg *,struct netlink_skb_parms *))59008536e8SJoe Perches int cn_queue_add_callback(struct cn_queue_dev *dev, const char *name,
60*c18e6869SGeoff Levand const struct cb_id *id,
61f3c48eccSValentin Ilie void (*callback)(struct cn_msg *,
62f3c48eccSValentin Ilie struct netlink_skb_parms *))
637672d0b5SEvgeniy Polyakov {
647672d0b5SEvgeniy Polyakov struct cn_callback_entry *cbq, *__cbq;
657672d0b5SEvgeniy Polyakov int found = 0;
667672d0b5SEvgeniy Polyakov
6704f482faSPatrick McHardy cbq = cn_queue_alloc_callback_entry(dev, name, id, callback);
687672d0b5SEvgeniy Polyakov if (!cbq)
697672d0b5SEvgeniy Polyakov return -ENOMEM;
707672d0b5SEvgeniy Polyakov
717672d0b5SEvgeniy Polyakov spin_lock_bh(&dev->queue_lock);
727672d0b5SEvgeniy Polyakov list_for_each_entry(__cbq, &dev->queue_list, callback_entry) {
73acd042bbSEvgeniy Polyakov if (cn_cb_equal(&__cbq->id.id, id)) {
747672d0b5SEvgeniy Polyakov found = 1;
757672d0b5SEvgeniy Polyakov break;
767672d0b5SEvgeniy Polyakov }
777672d0b5SEvgeniy Polyakov }
787672d0b5SEvgeniy Polyakov if (!found)
797672d0b5SEvgeniy Polyakov list_add_tail(&cbq->callback_entry, &dev->queue_list);
807672d0b5SEvgeniy Polyakov spin_unlock_bh(&dev->queue_lock);
817672d0b5SEvgeniy Polyakov
827672d0b5SEvgeniy Polyakov if (found) {
8304f482faSPatrick McHardy cn_queue_release_callback(cbq);
847672d0b5SEvgeniy Polyakov return -EINVAL;
857672d0b5SEvgeniy Polyakov }
867672d0b5SEvgeniy Polyakov
877672d0b5SEvgeniy Polyakov cbq->seq = 0;
88acd042bbSEvgeniy Polyakov cbq->group = cbq->id.id.idx;
897672d0b5SEvgeniy Polyakov
907672d0b5SEvgeniy Polyakov return 0;
917672d0b5SEvgeniy Polyakov }
927672d0b5SEvgeniy Polyakov
cn_queue_del_callback(struct cn_queue_dev * dev,const struct cb_id * id)93*c18e6869SGeoff Levand void cn_queue_del_callback(struct cn_queue_dev *dev, const struct cb_id *id)
947672d0b5SEvgeniy Polyakov {
957672d0b5SEvgeniy Polyakov struct cn_callback_entry *cbq, *n;
967672d0b5SEvgeniy Polyakov int found = 0;
977672d0b5SEvgeniy Polyakov
987672d0b5SEvgeniy Polyakov spin_lock_bh(&dev->queue_lock);
997672d0b5SEvgeniy Polyakov list_for_each_entry_safe(cbq, n, &dev->queue_list, callback_entry) {
100acd042bbSEvgeniy Polyakov if (cn_cb_equal(&cbq->id.id, id)) {
1017672d0b5SEvgeniy Polyakov list_del(&cbq->callback_entry);
1027672d0b5SEvgeniy Polyakov found = 1;
1037672d0b5SEvgeniy Polyakov break;
1047672d0b5SEvgeniy Polyakov }
1057672d0b5SEvgeniy Polyakov }
1067672d0b5SEvgeniy Polyakov spin_unlock_bh(&dev->queue_lock);
1077672d0b5SEvgeniy Polyakov
10804f482faSPatrick McHardy if (found)
10904f482faSPatrick McHardy cn_queue_release_callback(cbq);
1107672d0b5SEvgeniy Polyakov }
1117672d0b5SEvgeniy Polyakov
cn_queue_alloc_dev(const char * name,struct sock * nls)112008536e8SJoe Perches struct cn_queue_dev *cn_queue_alloc_dev(const char *name, struct sock *nls)
1137672d0b5SEvgeniy Polyakov {
1147672d0b5SEvgeniy Polyakov struct cn_queue_dev *dev;
1157672d0b5SEvgeniy Polyakov
1167672d0b5SEvgeniy Polyakov dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1177672d0b5SEvgeniy Polyakov if (!dev)
1187672d0b5SEvgeniy Polyakov return NULL;
1197672d0b5SEvgeniy Polyakov
1207672d0b5SEvgeniy Polyakov snprintf(dev->name, sizeof(dev->name), "%s", name);
1217672d0b5SEvgeniy Polyakov atomic_set(&dev->refcnt, 0);
1227672d0b5SEvgeniy Polyakov INIT_LIST_HEAD(&dev->queue_list);
1237672d0b5SEvgeniy Polyakov spin_lock_init(&dev->queue_lock);
1247672d0b5SEvgeniy Polyakov
1257672d0b5SEvgeniy Polyakov dev->nls = nls;
1267672d0b5SEvgeniy Polyakov
1277672d0b5SEvgeniy Polyakov return dev;
1287672d0b5SEvgeniy Polyakov }
1297672d0b5SEvgeniy Polyakov
cn_queue_free_dev(struct cn_queue_dev * dev)1307672d0b5SEvgeniy Polyakov void cn_queue_free_dev(struct cn_queue_dev *dev)
1317672d0b5SEvgeniy Polyakov {
1327672d0b5SEvgeniy Polyakov struct cn_callback_entry *cbq, *n;
1337672d0b5SEvgeniy Polyakov
1347672d0b5SEvgeniy Polyakov spin_lock_bh(&dev->queue_lock);
1357672d0b5SEvgeniy Polyakov list_for_each_entry_safe(cbq, n, &dev->queue_list, callback_entry)
1367672d0b5SEvgeniy Polyakov list_del(&cbq->callback_entry);
1377672d0b5SEvgeniy Polyakov spin_unlock_bh(&dev->queue_lock);
1387672d0b5SEvgeniy Polyakov
1397672d0b5SEvgeniy Polyakov while (atomic_read(&dev->refcnt)) {
140f3c48eccSValentin Ilie pr_info("Waiting for %s to become free: refcnt=%d.\n",
1417672d0b5SEvgeniy Polyakov dev->name, atomic_read(&dev->refcnt));
1427672d0b5SEvgeniy Polyakov msleep(1000);
1437672d0b5SEvgeniy Polyakov }
1447672d0b5SEvgeniy Polyakov
1457672d0b5SEvgeniy Polyakov kfree(dev);
1467672d0b5SEvgeniy Polyakov dev = NULL;
1477672d0b5SEvgeniy Polyakov }
148