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 __FBSDID("$FreeBSD$"); 33 34 #include <linux/kernel.h> 35 #include <linux/device.h> 36 #include <linux/slab.h> 37 #include <linux/list.h> 38 39 /* 40 * Linux devres KPI implementation. 41 */ 42 43 struct devres { 44 struct list_head entry; 45 void (*release)(struct device *, void *); 46 47 /* Must come last. */ 48 uint8_t __drdata[0] __aligned(CACHE_LINE_SIZE); 49 }; 50 51 void * 52 lkpi_devres_alloc(void(*release)(struct device *, void *), 53 size_t size, gfp_t gfp) 54 { 55 void *p; 56 struct devres *dr; 57 size_t total; 58 59 if (size == 0) 60 return (NULL); 61 62 total = sizeof(*dr) + size; 63 dr = kmalloc(total, gfp); 64 if (dr == NULL) 65 return (NULL); 66 67 INIT_LIST_HEAD(&dr->entry); 68 dr->release = release; 69 p = (void *)(dr+1); 70 71 return (p); 72 } 73 74 static void 75 lkpi_devres_free_dr(struct devres *dr) 76 { 77 78 /* 79 * We have no dev, so cannot lock. This means someone else has 80 * to do this prior to us if devres_add() had been called. 81 */ 82 KASSERT(list_empty_careful(&dr->entry), 83 ("%s: dr %p still on devres_head\n", __func__, dr)); 84 kfree(dr); 85 } 86 87 void 88 lkpi_devres_free(void *p) 89 { 90 struct devres *dr; 91 92 if (p == NULL) 93 return; 94 95 dr = container_of(p, struct devres, __drdata); 96 lkpi_devres_free_dr(dr); 97 } 98 99 void 100 lkpi_devres_add(struct device *dev, void *p) 101 { 102 struct devres *dr; 103 104 KASSERT(dev != NULL && p != NULL, ("%s: dev %p p %p\n", 105 __func__, dev, p)); 106 107 dr = container_of(p, struct devres, __drdata); 108 spin_lock(&dev->devres_lock); 109 list_add(&dr->entry, &dev->devres_head); 110 spin_unlock(&dev->devres_lock); 111 } 112 113 static struct devres * 114 lkpi_devres_find_dr(struct device *dev, void(*release)(struct device *, void *), 115 int (*match)(struct device *, void *, void *), void *mp) 116 { 117 struct devres *dr, *next; 118 void *p; 119 120 KASSERT(dev != NULL, ("%s: dev %p\n", __func__, dev)); 121 assert_spin_locked(&dev->devres_lock); 122 123 list_for_each_entry_safe(dr, next, &dev->devres_head, entry) { 124 if (dr->release != release) 125 continue; 126 p = (void *)(dr+1); 127 if (match != NULL && match(dev, p, mp) == false) 128 continue; 129 return (dr); 130 } 131 132 return (NULL); 133 } 134 135 void * 136 lkpi_devres_find(struct device *dev, void(*release)(struct device *, void *), 137 int (*match)(struct device *, void *, void *), void *mp) 138 { 139 struct devres *dr; 140 141 KASSERT(dev != NULL, ("%s: dev %p\n", __func__, dev)); 142 143 spin_lock(&dev->devres_lock); 144 dr = lkpi_devres_find_dr(dev, release, match, mp); 145 spin_unlock(&dev->devres_lock); 146 147 if (dr == NULL) 148 return (NULL); 149 150 return ((void *)(dr + 1)); 151 } 152 153 static void 154 lkpi_devres_unlink_locked(struct device *dev, struct devres *dr) 155 { 156 KASSERT(dev != NULL, ("%s: dev %p\n", __func__, dev)); 157 KASSERT(dr != NULL, ("%s: dr %p\n", __func__, dr)); 158 assert_spin_locked(&dev->devres_lock); 159 160 list_del_init(&dr->entry); 161 } 162 163 void 164 lkpi_devres_unlink(struct device *dev, void *p) 165 { 166 struct devres *dr; 167 168 KASSERT(dev != NULL && p != NULL, ("%s: dev %p p %p\n", 169 __func__, dev, p)); 170 171 dr = container_of(p, struct devres, __drdata); 172 spin_lock(&dev->devres_lock); 173 lkpi_devres_unlink_locked(dev, dr); 174 spin_unlock(&dev->devres_lock); 175 } 176 177 /* This is called on device free. */ 178 void 179 lkpi_devres_release_free_list(struct device *dev) 180 { 181 struct devres *dr, *next; 182 void *p; 183 184 /* Free any resources allocated on the device. */ 185 /* No need to lock anymore. */ 186 list_for_each_entry_safe(dr, next, &dev->devres_head, entry) { 187 p = (void *)(dr+1); 188 if (dr->release != NULL) 189 dr->release(dev, p); 190 /* This should probably be a function of some kind. */ 191 list_del_init(&dr->entry); 192 lkpi_devres_free(p); 193 } 194 } 195 196 int 197 lkpi_devres_destroy(struct device *dev, void(*release)(struct device *, void *), 198 int (*match)(struct device *, void *, void *), void *mp) 199 { 200 struct devres *dr; 201 202 spin_lock(&dev->devres_lock); 203 dr = lkpi_devres_find_dr(dev, release, match, mp); 204 if (dr != NULL) 205 lkpi_devres_unlink_locked(dev, dr); 206 spin_unlock(&dev->devres_lock); 207 208 if (dr == NULL) 209 return (-ENOENT); 210 lkpi_devres_free_dr(dr); 211 212 return (0); 213 } 214 215 /* 216 * Devres release function for k*malloc(). 217 * While there is nothing to do here adding, e.g., tracing would be 218 * possible so we leave the empty function here. 219 * Also good for documentation as it is the simplest example. 220 */ 221 void 222 lkpi_devm_kmalloc_release(struct device *dev __unused, void *p __unused) 223 { 224 225 /* Nothing to do. Freed with the devres. */ 226 } 227 228 struct devres_action { 229 void *data; 230 void (*action)(void *); 231 }; 232 233 static void 234 lkpi_devm_action_release(struct device *dev, void *res) 235 { 236 struct devres_action *devres; 237 238 devres = (struct devres_action *)res; 239 devres->action(devres->data); 240 } 241 242 int 243 lkpi_devm_add_action(struct device *dev, void (*action)(void *), void *data) 244 { 245 struct devres_action *devres; 246 247 KASSERT(action != NULL, ("%s: action is NULL\n", __func__)); 248 devres = lkpi_devres_alloc(lkpi_devm_action_release, 249 sizeof(struct devres_action), GFP_KERNEL); 250 if (devres == NULL) 251 return (-ENOMEM); 252 devres->data = data; 253 devres->action = action; 254 devres_add(dev, devres); 255 256 return (0); 257 } 258 259 int 260 lkpi_devm_add_action_or_reset(struct device *dev, void (*action)(void *), void *data) 261 { 262 int rv; 263 264 rv = lkpi_devm_add_action(dev, action, data); 265 if (rv != 0) 266 action(data); 267 268 return (rv); 269 } 270