driver.c 6.4 KB

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