1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * cn_queue.c |
4 | * |
5 | * 2004+ Copyright (c) Evgeniy Polyakov <zbr@ioremap.net> |
6 | * All rights reserved. |
7 | */ |
8 | |
9 | #include <linux/kernel.h> |
10 | #include <linux/module.h> |
11 | #include <linux/list.h> |
12 | #include <linux/workqueue.h> |
13 | #include <linux/spinlock.h> |
14 | #include <linux/slab.h> |
15 | #include <linux/skbuff.h> |
16 | #include <linux/suspend.h> |
17 | #include <linux/connector.h> |
18 | #include <linux/delay.h> |
19 | |
20 | static struct cn_callback_entry * |
21 | cn_queue_alloc_callback_entry(struct cn_queue_dev *dev, const char *name, |
22 | const struct cb_id *id, |
23 | void (*callback)(struct cn_msg *, |
24 | struct netlink_skb_parms *)) |
25 | { |
26 | struct cn_callback_entry *cbq; |
27 | |
28 | cbq = kzalloc(size: sizeof(*cbq), GFP_KERNEL); |
29 | if (!cbq) { |
30 | pr_err("Failed to create new callback queue.\n" ); |
31 | return NULL; |
32 | } |
33 | |
34 | refcount_set(r: &cbq->refcnt, n: 1); |
35 | |
36 | atomic_inc(v: &dev->refcnt); |
37 | cbq->pdev = dev; |
38 | |
39 | snprintf(buf: cbq->id.name, size: sizeof(cbq->id.name), fmt: "%s" , name); |
40 | memcpy(&cbq->id.id, id, sizeof(struct cb_id)); |
41 | cbq->callback = callback; |
42 | return cbq; |
43 | } |
44 | |
45 | void cn_queue_release_callback(struct cn_callback_entry *cbq) |
46 | { |
47 | if (!refcount_dec_and_test(r: &cbq->refcnt)) |
48 | return; |
49 | |
50 | atomic_dec(v: &cbq->pdev->refcnt); |
51 | kfree(objp: cbq); |
52 | } |
53 | |
54 | int cn_cb_equal(const struct cb_id *i1, const struct cb_id *i2) |
55 | { |
56 | return ((i1->idx == i2->idx) && (i1->val == i2->val)); |
57 | } |
58 | |
59 | int cn_queue_add_callback(struct cn_queue_dev *dev, const char *name, |
60 | const struct cb_id *id, |
61 | void (*callback)(struct cn_msg *, |
62 | struct netlink_skb_parms *)) |
63 | { |
64 | struct cn_callback_entry *cbq, *__cbq; |
65 | int found = 0; |
66 | |
67 | cbq = cn_queue_alloc_callback_entry(dev, name, id, callback); |
68 | if (!cbq) |
69 | return -ENOMEM; |
70 | |
71 | spin_lock_bh(lock: &dev->queue_lock); |
72 | list_for_each_entry(__cbq, &dev->queue_list, callback_entry) { |
73 | if (cn_cb_equal(i1: &__cbq->id.id, i2: id)) { |
74 | found = 1; |
75 | break; |
76 | } |
77 | } |
78 | if (!found) |
79 | list_add_tail(new: &cbq->callback_entry, head: &dev->queue_list); |
80 | spin_unlock_bh(lock: &dev->queue_lock); |
81 | |
82 | if (found) { |
83 | cn_queue_release_callback(cbq); |
84 | return -EINVAL; |
85 | } |
86 | |
87 | cbq->seq = 0; |
88 | cbq->group = cbq->id.id.idx; |
89 | |
90 | return 0; |
91 | } |
92 | |
93 | void cn_queue_del_callback(struct cn_queue_dev *dev, const struct cb_id *id) |
94 | { |
95 | struct cn_callback_entry *cbq, *n; |
96 | int found = 0; |
97 | |
98 | spin_lock_bh(lock: &dev->queue_lock); |
99 | list_for_each_entry_safe(cbq, n, &dev->queue_list, callback_entry) { |
100 | if (cn_cb_equal(i1: &cbq->id.id, i2: id)) { |
101 | list_del(entry: &cbq->callback_entry); |
102 | found = 1; |
103 | break; |
104 | } |
105 | } |
106 | spin_unlock_bh(lock: &dev->queue_lock); |
107 | |
108 | if (found) |
109 | cn_queue_release_callback(cbq); |
110 | } |
111 | |
112 | struct cn_queue_dev *cn_queue_alloc_dev(const char *name, struct sock *nls) |
113 | { |
114 | struct cn_queue_dev *dev; |
115 | |
116 | dev = kzalloc(size: sizeof(*dev), GFP_KERNEL); |
117 | if (!dev) |
118 | return NULL; |
119 | |
120 | snprintf(buf: dev->name, size: sizeof(dev->name), fmt: "%s" , name); |
121 | atomic_set(v: &dev->refcnt, i: 0); |
122 | INIT_LIST_HEAD(list: &dev->queue_list); |
123 | spin_lock_init(&dev->queue_lock); |
124 | |
125 | dev->nls = nls; |
126 | |
127 | return dev; |
128 | } |
129 | |
130 | void cn_queue_free_dev(struct cn_queue_dev *dev) |
131 | { |
132 | struct cn_callback_entry *cbq, *n; |
133 | |
134 | spin_lock_bh(lock: &dev->queue_lock); |
135 | list_for_each_entry_safe(cbq, n, &dev->queue_list, callback_entry) |
136 | list_del(entry: &cbq->callback_entry); |
137 | spin_unlock_bh(lock: &dev->queue_lock); |
138 | |
139 | while (atomic_read(v: &dev->refcnt)) { |
140 | pr_info("Waiting for %s to become free: refcnt=%d.\n" , |
141 | dev->name, atomic_read(&dev->refcnt)); |
142 | msleep(msecs: 1000); |
143 | } |
144 | |
145 | kfree(objp: dev); |
146 | dev = NULL; |
147 | } |
148 | |