323 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * driver.c - device id matching, driver model, etc.4 *5 * Copyright 2002 Adam Belay <ambx1@neo.rr.com>6 */7 8#include <linux/string.h>9#include <linux/list.h>10#include <linux/module.h>11#include <linux/ctype.h>12#include <linux/slab.h>13#include <linux/pnp.h>14#include "base.h"15 16static int compare_func(const char *ida, const char *idb)17{18 int i;19 20 /* we only need to compare the last 4 chars */21 for (i = 3; i < 7; i++) {22 if (ida[i] != 'X' &&23 idb[i] != 'X' && toupper(ida[i]) != toupper(idb[i]))24 return 0;25 }26 return 1;27}28 29int compare_pnp_id(struct pnp_id *pos, const char *id)30{31 if (!pos || !id || (strlen(id) != 7))32 return 0;33 if (memcmp(id, "ANYDEVS", 7) == 0)34 return 1;35 while (pos) {36 if (memcmp(pos->id, id, 3) == 0)37 if (compare_func(pos->id, id) == 1)38 return 1;39 pos = pos->next;40 }41 return 0;42}43 44static const struct pnp_device_id *match_device(const struct pnp_driver *drv,45 struct pnp_dev *dev)46{47 const struct pnp_device_id *drv_id = drv->id_table;48 49 if (!drv_id)50 return NULL;51 52 while (*drv_id->id) {53 if (compare_pnp_id(dev->id, drv_id->id))54 return drv_id;55 drv_id++;56 }57 return NULL;58}59 60int pnp_device_attach(struct pnp_dev *pnp_dev)61{62 mutex_lock(&pnp_lock);63 if (pnp_dev->status != PNP_READY) {64 mutex_unlock(&pnp_lock);65 return -EBUSY;66 }67 pnp_dev->status = PNP_ATTACHED;68 mutex_unlock(&pnp_lock);69 return 0;70}71EXPORT_SYMBOL(pnp_device_attach);72 73void pnp_device_detach(struct pnp_dev *pnp_dev)74{75 mutex_lock(&pnp_lock);76 if (pnp_dev->status == PNP_ATTACHED)77 pnp_dev->status = PNP_READY;78 mutex_unlock(&pnp_lock);79}80EXPORT_SYMBOL(pnp_device_detach);81 82static int pnp_device_probe(struct device *dev)83{84 int error;85 struct pnp_driver *pnp_drv;86 struct pnp_dev *pnp_dev;87 const struct pnp_device_id *dev_id = NULL;88 pnp_dev = to_pnp_dev(dev);89 pnp_drv = to_pnp_driver(dev->driver);90 91 error = pnp_device_attach(pnp_dev);92 if (error < 0)93 return error;94 95 if (pnp_dev->active == 0) {96 if (!(pnp_drv->flags & PNP_DRIVER_RES_DO_NOT_CHANGE)) {97 error = pnp_activate_dev(pnp_dev);98 if (error < 0)99 return error;100 }101 } else if ((pnp_drv->flags & PNP_DRIVER_RES_DISABLE)102 == PNP_DRIVER_RES_DISABLE) {103 error = pnp_disable_dev(pnp_dev);104 if (error < 0)105 return error;106 }107 error = 0;108 if (pnp_drv->probe) {109 dev_id = match_device(pnp_drv, pnp_dev);110 if (dev_id != NULL)111 error = pnp_drv->probe(pnp_dev, dev_id);112 }113 if (error >= 0) {114 pnp_dev->driver = pnp_drv;115 error = 0;116 } else117 goto fail;118 119 return error;120 121fail:122 pnp_device_detach(pnp_dev);123 return error;124}125 126static void pnp_device_remove(struct device *dev)127{128 struct pnp_dev *pnp_dev = to_pnp_dev(dev);129 struct pnp_driver *drv = pnp_dev->driver;130 131 if (drv) {132 if (drv->remove)133 drv->remove(pnp_dev);134 pnp_dev->driver = NULL;135 }136 137 if (pnp_dev->active &&138 (!drv || !(drv->flags & PNP_DRIVER_RES_DO_NOT_CHANGE)))139 pnp_disable_dev(pnp_dev);140 141 pnp_device_detach(pnp_dev);142}143 144static void pnp_device_shutdown(struct device *dev)145{146 struct pnp_dev *pnp_dev = to_pnp_dev(dev);147 struct pnp_driver *drv = pnp_dev->driver;148 149 if (drv && drv->shutdown)150 drv->shutdown(pnp_dev);151}152 153static int pnp_bus_match(struct device *dev, const struct device_driver *drv)154{155 struct pnp_dev *pnp_dev = to_pnp_dev(dev);156 const struct pnp_driver *pnp_drv = to_pnp_driver(drv);157 158 if (match_device(pnp_drv, pnp_dev) == NULL)159 return 0;160 return 1;161}162 163static int __pnp_bus_suspend(struct device *dev, pm_message_t state)164{165 struct pnp_dev *pnp_dev = to_pnp_dev(dev);166 struct pnp_driver *pnp_drv = pnp_dev->driver;167 int error;168 169 if (!pnp_drv)170 return 0;171 172 if (pnp_drv->driver.pm && pnp_drv->driver.pm->suspend) {173 error = pnp_drv->driver.pm->suspend(dev);174 suspend_report_result(dev, pnp_drv->driver.pm->suspend, error);175 if (error)176 return error;177 }178 179 if (pnp_drv->suspend) {180 error = pnp_drv->suspend(pnp_dev, state);181 if (error)182 return error;183 }184 185 /* can_write is necessary to be able to re-start the device on resume */186 if (pnp_can_disable(pnp_dev) && pnp_can_write(pnp_dev)) {187 error = pnp_stop_dev(pnp_dev);188 if (error)189 return error;190 }191 192 if (pnp_can_suspend(pnp_dev))193 pnp_dev->protocol->suspend(pnp_dev, state);194 return 0;195}196 197static int pnp_bus_suspend(struct device *dev)198{199 return __pnp_bus_suspend(dev, PMSG_SUSPEND);200}201 202static int pnp_bus_freeze(struct device *dev)203{204 return __pnp_bus_suspend(dev, PMSG_FREEZE);205}206 207static int pnp_bus_poweroff(struct device *dev)208{209 return __pnp_bus_suspend(dev, PMSG_HIBERNATE);210}211 212static int pnp_bus_resume(struct device *dev)213{214 struct pnp_dev *pnp_dev = to_pnp_dev(dev);215 struct pnp_driver *pnp_drv = pnp_dev->driver;216 int error;217 218 if (!pnp_drv)219 return 0;220 221 if (pnp_dev->protocol->resume) {222 error = pnp_dev->protocol->resume(pnp_dev);223 if (error)224 return error;225 }226 227 if (pnp_can_write(pnp_dev)) {228 error = pnp_start_dev(pnp_dev);229 if (error)230 return error;231 }232 233 if (pnp_drv->driver.pm && pnp_drv->driver.pm->resume) {234 error = pnp_drv->driver.pm->resume(dev);235 if (error)236 return error;237 }238 239 if (pnp_drv->resume) {240 error = pnp_drv->resume(pnp_dev);241 if (error)242 return error;243 }244 245 return 0;246}247 248static const struct dev_pm_ops pnp_bus_dev_pm_ops = {249 /* Suspend callbacks */250 .suspend = pnp_bus_suspend,251 .resume = pnp_bus_resume,252 /* Hibernate callbacks */253 .freeze = pnp_bus_freeze,254 .thaw = pnp_bus_resume,255 .poweroff = pnp_bus_poweroff,256 .restore = pnp_bus_resume,257};258 259const struct bus_type pnp_bus_type = {260 .name = "pnp",261 .match = pnp_bus_match,262 .probe = pnp_device_probe,263 .remove = pnp_device_remove,264 .shutdown = pnp_device_shutdown,265 .pm = &pnp_bus_dev_pm_ops,266 .dev_groups = pnp_dev_groups,267};268 269bool dev_is_pnp(const struct device *dev)270{271 return dev->bus == &pnp_bus_type;272}273EXPORT_SYMBOL_GPL(dev_is_pnp);274 275int pnp_register_driver(struct pnp_driver *drv)276{277 drv->driver.name = drv->name;278 drv->driver.bus = &pnp_bus_type;279 280 return driver_register(&drv->driver);281}282EXPORT_SYMBOL(pnp_register_driver);283 284void pnp_unregister_driver(struct pnp_driver *drv)285{286 driver_unregister(&drv->driver);287}288EXPORT_SYMBOL(pnp_unregister_driver);289 290/**291 * pnp_add_id - adds an EISA id to the specified device292 * @dev: pointer to the desired device293 * @id: pointer to an EISA id string294 */295struct pnp_id *pnp_add_id(struct pnp_dev *dev, const char *id)296{297 struct pnp_id *dev_id, *ptr;298 299 dev_id = kzalloc(sizeof(struct pnp_id), GFP_KERNEL);300 if (!dev_id)301 return NULL;302 303 dev_id->id[0] = id[0];304 dev_id->id[1] = id[1];305 dev_id->id[2] = id[2];306 dev_id->id[3] = tolower(id[3]);307 dev_id->id[4] = tolower(id[4]);308 dev_id->id[5] = tolower(id[5]);309 dev_id->id[6] = tolower(id[6]);310 dev_id->id[7] = '\0';311 312 dev_id->next = NULL;313 ptr = dev->id;314 while (ptr && ptr->next)315 ptr = ptr->next;316 if (ptr)317 ptr->next = dev_id;318 else319 dev->id = dev_id;320 321 return dev_id;322}323