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