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