1fa765ca7SBjoern A. Zeeb /*-
2fa765ca7SBjoern A. Zeeb * SPDX-License-Identifier: BSD-2-Clause
3fa765ca7SBjoern A. Zeeb *
4fa765ca7SBjoern A. Zeeb * Copyright (c) 2020-2021 The FreeBSD Foundation
5fa765ca7SBjoern A. Zeeb *
6fa765ca7SBjoern A. Zeeb * This software was developed by Bj\xc3\xb6rn Zeeb under sponsorship from
7fa765ca7SBjoern A. Zeeb * the FreeBSD Foundation.
8fa765ca7SBjoern A. Zeeb *
9fa765ca7SBjoern A. Zeeb * Redistribution and use in source and binary forms, with or without
10fa765ca7SBjoern A. Zeeb * modification, are permitted provided that the following conditions
11fa765ca7SBjoern A. Zeeb * are met:
12fa765ca7SBjoern A. Zeeb * 1. Redistributions of source code must retain the above copyright
13fa765ca7SBjoern A. Zeeb * notice, this list of conditions and the following disclaimer.
14fa765ca7SBjoern A. Zeeb * 2. Redistributions in binary form must reproduce the above copyright
15fa765ca7SBjoern A. Zeeb * notice, this list of conditions and the following disclaimer in the
16fa765ca7SBjoern A. Zeeb * documentation and/or other materials provided with the distribution.
17fa765ca7SBjoern A. Zeeb *
18fa765ca7SBjoern A. Zeeb * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
19fa765ca7SBjoern A. Zeeb * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20fa765ca7SBjoern A. Zeeb * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21fa765ca7SBjoern A. Zeeb * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
22fa765ca7SBjoern A. Zeeb * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23fa765ca7SBjoern A. Zeeb * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
24fa765ca7SBjoern A. Zeeb * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25fa765ca7SBjoern A. Zeeb * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
26fa765ca7SBjoern A. Zeeb * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
27fa765ca7SBjoern A. Zeeb * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
28fa765ca7SBjoern A. Zeeb * SUCH DAMAGE.
29fa765ca7SBjoern A. Zeeb */
30fa765ca7SBjoern A. Zeeb
31fa765ca7SBjoern A. Zeeb #include <sys/cdefs.h>
32fa765ca7SBjoern A. Zeeb #include <linux/kernel.h>
33fa765ca7SBjoern A. Zeeb #include <linux/device.h>
34fa765ca7SBjoern A. Zeeb #include <linux/slab.h>
35fa765ca7SBjoern A. Zeeb #include <linux/list.h>
36fa765ca7SBjoern A. Zeeb
37fa765ca7SBjoern A. Zeeb /*
38fa765ca7SBjoern A. Zeeb * Linux devres KPI implementation.
39fa765ca7SBjoern A. Zeeb */
40fa765ca7SBjoern A. Zeeb
41fa765ca7SBjoern A. Zeeb struct devres {
42fa765ca7SBjoern A. Zeeb struct list_head entry;
43fa765ca7SBjoern A. Zeeb void (*release)(struct device *, void *);
44fa765ca7SBjoern A. Zeeb
45fa765ca7SBjoern A. Zeeb /* Must come last. */
46fa765ca7SBjoern A. Zeeb uint8_t __drdata[0] __aligned(CACHE_LINE_SIZE);
47fa765ca7SBjoern A. Zeeb };
48fa765ca7SBjoern A. Zeeb
49fa765ca7SBjoern A. Zeeb void *
lkpi_devres_alloc(void (* release)(struct device *,void *),size_t size,gfp_t gfp)50fa765ca7SBjoern A. Zeeb lkpi_devres_alloc(void(*release)(struct device *, void *),
51fa765ca7SBjoern A. Zeeb size_t size, gfp_t gfp)
52fa765ca7SBjoern A. Zeeb {
53fa765ca7SBjoern A. Zeeb void *p;
54fa765ca7SBjoern A. Zeeb struct devres *dr;
55fa765ca7SBjoern A. Zeeb size_t total;
56fa765ca7SBjoern A. Zeeb
57fa765ca7SBjoern A. Zeeb if (size == 0)
58fa765ca7SBjoern A. Zeeb return (NULL);
59fa765ca7SBjoern A. Zeeb
60fa765ca7SBjoern A. Zeeb total = sizeof(*dr) + size;
61fa765ca7SBjoern A. Zeeb dr = kmalloc(total, gfp);
62fa765ca7SBjoern A. Zeeb if (dr == NULL)
63fa765ca7SBjoern A. Zeeb return (NULL);
64fa765ca7SBjoern A. Zeeb
65fa765ca7SBjoern A. Zeeb INIT_LIST_HEAD(&dr->entry);
66fa765ca7SBjoern A. Zeeb dr->release = release;
67fa765ca7SBjoern A. Zeeb p = (void *)(dr+1);
68fa765ca7SBjoern A. Zeeb
69fa765ca7SBjoern A. Zeeb return (p);
70fa765ca7SBjoern A. Zeeb }
71fa765ca7SBjoern A. Zeeb
72fa765ca7SBjoern A. Zeeb static void
lkpi_devres_free_dr(struct devres * dr)73fa765ca7SBjoern A. Zeeb lkpi_devres_free_dr(struct devres *dr)
74fa765ca7SBjoern A. Zeeb {
75fa765ca7SBjoern A. Zeeb
76fa765ca7SBjoern A. Zeeb /*
77fa765ca7SBjoern A. Zeeb * We have no dev, so cannot lock. This means someone else has
78fa765ca7SBjoern A. Zeeb * to do this prior to us if devres_add() had been called.
79fa765ca7SBjoern A. Zeeb */
80fa765ca7SBjoern A. Zeeb KASSERT(list_empty_careful(&dr->entry),
81fa765ca7SBjoern A. Zeeb ("%s: dr %p still on devres_head\n", __func__, dr));
82fa765ca7SBjoern A. Zeeb kfree(dr);
83fa765ca7SBjoern A. Zeeb }
84fa765ca7SBjoern A. Zeeb
85fa765ca7SBjoern A. Zeeb void
lkpi_devres_free(void * p)86fa765ca7SBjoern A. Zeeb lkpi_devres_free(void *p)
87fa765ca7SBjoern A. Zeeb {
88fa765ca7SBjoern A. Zeeb struct devres *dr;
89fa765ca7SBjoern A. Zeeb
90fa765ca7SBjoern A. Zeeb if (p == NULL)
91fa765ca7SBjoern A. Zeeb return;
92fa765ca7SBjoern A. Zeeb
93fa765ca7SBjoern A. Zeeb dr = container_of(p, struct devres, __drdata);
94fa765ca7SBjoern A. Zeeb lkpi_devres_free_dr(dr);
95fa765ca7SBjoern A. Zeeb }
96fa765ca7SBjoern A. Zeeb
97fa765ca7SBjoern A. Zeeb void
lkpi_devres_add(struct device * dev,void * p)98fa765ca7SBjoern A. Zeeb lkpi_devres_add(struct device *dev, void *p)
99fa765ca7SBjoern A. Zeeb {
100fa765ca7SBjoern A. Zeeb struct devres *dr;
101fa765ca7SBjoern A. Zeeb
102fa765ca7SBjoern A. Zeeb KASSERT(dev != NULL && p != NULL, ("%s: dev %p p %p\n",
103fa765ca7SBjoern A. Zeeb __func__, dev, p));
104fa765ca7SBjoern A. Zeeb
105fa765ca7SBjoern A. Zeeb dr = container_of(p, struct devres, __drdata);
106fa765ca7SBjoern A. Zeeb spin_lock(&dev->devres_lock);
107fa765ca7SBjoern A. Zeeb list_add(&dr->entry, &dev->devres_head);
108fa765ca7SBjoern A. Zeeb spin_unlock(&dev->devres_lock);
109fa765ca7SBjoern A. Zeeb }
110fa765ca7SBjoern A. Zeeb
111fa765ca7SBjoern A. Zeeb static struct devres *
lkpi_devres_find_dr(struct device * dev,void (* release)(struct device *,void *),int (* match)(struct device *,void *,void *),void * mp)112fa765ca7SBjoern A. Zeeb lkpi_devres_find_dr(struct device *dev, void(*release)(struct device *, void *),
113fa765ca7SBjoern A. Zeeb int (*match)(struct device *, void *, void *), void *mp)
114fa765ca7SBjoern A. Zeeb {
115fa765ca7SBjoern A. Zeeb struct devres *dr, *next;
116fa765ca7SBjoern A. Zeeb void *p;
117fa765ca7SBjoern A. Zeeb
118fa765ca7SBjoern A. Zeeb KASSERT(dev != NULL, ("%s: dev %p\n", __func__, dev));
119fa765ca7SBjoern A. Zeeb assert_spin_locked(&dev->devres_lock);
120fa765ca7SBjoern A. Zeeb
121fa765ca7SBjoern A. Zeeb list_for_each_entry_safe(dr, next, &dev->devres_head, entry) {
122fa765ca7SBjoern A. Zeeb if (dr->release != release)
123fa765ca7SBjoern A. Zeeb continue;
124fa765ca7SBjoern A. Zeeb p = (void *)(dr+1);
125fa765ca7SBjoern A. Zeeb if (match != NULL && match(dev, p, mp) == false)
126fa765ca7SBjoern A. Zeeb continue;
127fa765ca7SBjoern A. Zeeb return (dr);
128fa765ca7SBjoern A. Zeeb }
129fa765ca7SBjoern A. Zeeb
130fa765ca7SBjoern A. Zeeb return (NULL);
131fa765ca7SBjoern A. Zeeb }
132fa765ca7SBjoern A. Zeeb
133fa765ca7SBjoern A. Zeeb void *
lkpi_devres_find(struct device * dev,void (* release)(struct device *,void *),int (* match)(struct device *,void *,void *),void * mp)134fa765ca7SBjoern A. Zeeb lkpi_devres_find(struct device *dev, void(*release)(struct device *, void *),
135fa765ca7SBjoern A. Zeeb int (*match)(struct device *, void *, void *), void *mp)
136fa765ca7SBjoern A. Zeeb {
137fa765ca7SBjoern A. Zeeb struct devres *dr;
138fa765ca7SBjoern A. Zeeb
139fa765ca7SBjoern A. Zeeb KASSERT(dev != NULL, ("%s: dev %p\n", __func__, dev));
140fa765ca7SBjoern A. Zeeb
141fa765ca7SBjoern A. Zeeb spin_lock(&dev->devres_lock);
142fa765ca7SBjoern A. Zeeb dr = lkpi_devres_find_dr(dev, release, match, mp);
143fa765ca7SBjoern A. Zeeb spin_unlock(&dev->devres_lock);
144fa765ca7SBjoern A. Zeeb
145fa765ca7SBjoern A. Zeeb if (dr == NULL)
146fa765ca7SBjoern A. Zeeb return (NULL);
147fa765ca7SBjoern A. Zeeb
148fa765ca7SBjoern A. Zeeb return ((void *)(dr + 1));
149fa765ca7SBjoern A. Zeeb }
150fa765ca7SBjoern A. Zeeb
151fa765ca7SBjoern A. Zeeb static void
lkpi_devres_unlink_locked(struct device * dev,struct devres * dr)152fa765ca7SBjoern A. Zeeb lkpi_devres_unlink_locked(struct device *dev, struct devres *dr)
153fa765ca7SBjoern A. Zeeb {
154fa765ca7SBjoern A. Zeeb KASSERT(dev != NULL, ("%s: dev %p\n", __func__, dev));
155fa765ca7SBjoern A. Zeeb KASSERT(dr != NULL, ("%s: dr %p\n", __func__, dr));
156fa765ca7SBjoern A. Zeeb assert_spin_locked(&dev->devres_lock);
157fa765ca7SBjoern A. Zeeb
158fa765ca7SBjoern A. Zeeb list_del_init(&dr->entry);
159fa765ca7SBjoern A. Zeeb }
160fa765ca7SBjoern A. Zeeb
161fa765ca7SBjoern A. Zeeb void
lkpi_devres_unlink(struct device * dev,void * p)162fa765ca7SBjoern A. Zeeb lkpi_devres_unlink(struct device *dev, void *p)
163fa765ca7SBjoern A. Zeeb {
164fa765ca7SBjoern A. Zeeb struct devres *dr;
165fa765ca7SBjoern A. Zeeb
166fa765ca7SBjoern A. Zeeb KASSERT(dev != NULL && p != NULL, ("%s: dev %p p %p\n",
167fa765ca7SBjoern A. Zeeb __func__, dev, p));
168fa765ca7SBjoern A. Zeeb
169fa765ca7SBjoern A. Zeeb dr = container_of(p, struct devres, __drdata);
170fa765ca7SBjoern A. Zeeb spin_lock(&dev->devres_lock);
171fa765ca7SBjoern A. Zeeb lkpi_devres_unlink_locked(dev, dr);
172fa765ca7SBjoern A. Zeeb spin_unlock(&dev->devres_lock);
173fa765ca7SBjoern A. Zeeb }
174fa765ca7SBjoern A. Zeeb
175fa765ca7SBjoern A. Zeeb /* This is called on device free. */
176fa765ca7SBjoern A. Zeeb void
lkpi_devres_release_free_list(struct device * dev)177fa765ca7SBjoern A. Zeeb lkpi_devres_release_free_list(struct device *dev)
178fa765ca7SBjoern A. Zeeb {
179fa765ca7SBjoern A. Zeeb struct devres *dr, *next;
180fa765ca7SBjoern A. Zeeb void *p;
181fa765ca7SBjoern A. Zeeb
182fa765ca7SBjoern A. Zeeb /* Free any resources allocated on the device. */
183fa765ca7SBjoern A. Zeeb /* No need to lock anymore. */
184fa765ca7SBjoern A. Zeeb list_for_each_entry_safe(dr, next, &dev->devres_head, entry) {
185fa765ca7SBjoern A. Zeeb p = (void *)(dr+1);
186fa765ca7SBjoern A. Zeeb if (dr->release != NULL)
187fa765ca7SBjoern A. Zeeb dr->release(dev, p);
188fa765ca7SBjoern A. Zeeb /* This should probably be a function of some kind. */
189fa765ca7SBjoern A. Zeeb list_del_init(&dr->entry);
190fa765ca7SBjoern A. Zeeb lkpi_devres_free(p);
191fa765ca7SBjoern A. Zeeb }
192fa765ca7SBjoern A. Zeeb }
193fa765ca7SBjoern A. Zeeb
194fa765ca7SBjoern A. Zeeb int
lkpi_devres_destroy(struct device * dev,void (* release)(struct device *,void *),int (* match)(struct device *,void *,void *),void * mp)195fa765ca7SBjoern A. Zeeb lkpi_devres_destroy(struct device *dev, void(*release)(struct device *, void *),
196fa765ca7SBjoern A. Zeeb int (*match)(struct device *, void *, void *), void *mp)
197fa765ca7SBjoern A. Zeeb {
198fa765ca7SBjoern A. Zeeb struct devres *dr;
199fa765ca7SBjoern A. Zeeb
200fa765ca7SBjoern A. Zeeb spin_lock(&dev->devres_lock);
201fa765ca7SBjoern A. Zeeb dr = lkpi_devres_find_dr(dev, release, match, mp);
202fa765ca7SBjoern A. Zeeb if (dr != NULL)
203fa765ca7SBjoern A. Zeeb lkpi_devres_unlink_locked(dev, dr);
204fa765ca7SBjoern A. Zeeb spin_unlock(&dev->devres_lock);
205fa765ca7SBjoern A. Zeeb
206fa765ca7SBjoern A. Zeeb if (dr == NULL)
207fa765ca7SBjoern A. Zeeb return (-ENOENT);
208fa765ca7SBjoern A. Zeeb lkpi_devres_free_dr(dr);
209fa765ca7SBjoern A. Zeeb
210fa765ca7SBjoern A. Zeeb return (0);
211fa765ca7SBjoern A. Zeeb }
212fa765ca7SBjoern A. Zeeb
213fa765ca7SBjoern A. Zeeb /*
214fa765ca7SBjoern A. Zeeb * Devres release function for k*malloc().
215fa765ca7SBjoern A. Zeeb * While there is nothing to do here adding, e.g., tracing would be
216fa765ca7SBjoern A. Zeeb * possible so we leave the empty function here.
217fa765ca7SBjoern A. Zeeb * Also good for documentation as it is the simplest example.
218fa765ca7SBjoern A. Zeeb */
219fa765ca7SBjoern A. Zeeb void
lkpi_devm_kmalloc_release(struct device * dev __unused,void * p __unused)220fa765ca7SBjoern A. Zeeb lkpi_devm_kmalloc_release(struct device *dev __unused, void *p __unused)
221fa765ca7SBjoern A. Zeeb {
222fa765ca7SBjoern A. Zeeb
223fa765ca7SBjoern A. Zeeb /* Nothing to do. Freed with the devres. */
224fa765ca7SBjoern A. Zeeb }
225aaf6129cSEmmanuel Vadot
226aaf6129cSEmmanuel Vadot struct devres_action {
227aaf6129cSEmmanuel Vadot void *data;
228aaf6129cSEmmanuel Vadot void (*action)(void *);
229aaf6129cSEmmanuel Vadot };
230aaf6129cSEmmanuel Vadot
231aaf6129cSEmmanuel Vadot static void
lkpi_devm_action_release(struct device * dev,void * res)232aaf6129cSEmmanuel Vadot lkpi_devm_action_release(struct device *dev, void *res)
233aaf6129cSEmmanuel Vadot {
234aaf6129cSEmmanuel Vadot struct devres_action *devres;
235aaf6129cSEmmanuel Vadot
236aaf6129cSEmmanuel Vadot devres = (struct devres_action *)res;
237aaf6129cSEmmanuel Vadot devres->action(devres->data);
238aaf6129cSEmmanuel Vadot }
239aaf6129cSEmmanuel Vadot
240aaf6129cSEmmanuel Vadot int
lkpi_devm_add_action(struct device * dev,void (* action)(void *),void * data)241aaf6129cSEmmanuel Vadot lkpi_devm_add_action(struct device *dev, void (*action)(void *), void *data)
242aaf6129cSEmmanuel Vadot {
243aaf6129cSEmmanuel Vadot struct devres_action *devres;
244aaf6129cSEmmanuel Vadot
245aaf6129cSEmmanuel Vadot KASSERT(action != NULL, ("%s: action is NULL\n", __func__));
246aaf6129cSEmmanuel Vadot devres = lkpi_devres_alloc(lkpi_devm_action_release,
247aaf6129cSEmmanuel Vadot sizeof(struct devres_action), GFP_KERNEL);
248aaf6129cSEmmanuel Vadot if (devres == NULL)
249aaf6129cSEmmanuel Vadot return (-ENOMEM);
250aaf6129cSEmmanuel Vadot devres->data = data;
251aaf6129cSEmmanuel Vadot devres->action = action;
252aaf6129cSEmmanuel Vadot devres_add(dev, devres);
253aaf6129cSEmmanuel Vadot
254aaf6129cSEmmanuel Vadot return (0);
255aaf6129cSEmmanuel Vadot }
256aaf6129cSEmmanuel Vadot
257aaf6129cSEmmanuel Vadot int
lkpi_devm_add_action_or_reset(struct device * dev,void (* action)(void *),void * data)258aaf6129cSEmmanuel Vadot lkpi_devm_add_action_or_reset(struct device *dev, void (*action)(void *), void *data)
259aaf6129cSEmmanuel Vadot {
260aaf6129cSEmmanuel Vadot int rv;
261aaf6129cSEmmanuel Vadot
262aaf6129cSEmmanuel Vadot rv = lkpi_devm_add_action(dev, action, data);
263aaf6129cSEmmanuel Vadot if (rv != 0)
264aaf6129cSEmmanuel Vadot action(data);
265aaf6129cSEmmanuel Vadot
266aaf6129cSEmmanuel Vadot return (rv);
267aaf6129cSEmmanuel Vadot }
268