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 * 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 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 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 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 * 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 * 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 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 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 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 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 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 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 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 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