reboot-mode.c 4.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * Copyright (c) 2016, Fuzhou Rockchip Electronics Co., Ltd
  4. */
  5. #include <linux/device.h>
  6. #include <linux/init.h>
  7. #include <linux/kernel.h>
  8. #include <linux/module.h>
  9. #include <linux/of.h>
  10. #include <linux/reboot.h>
  11. #include <linux/reboot-mode.h>
  12. #define PREFIX "mode-"
  13. struct mode_info {
  14. const char *mode;
  15. u32 magic;
  16. struct list_head list;
  17. };
  18. static unsigned int get_reboot_mode_magic(struct reboot_mode_driver *reboot,
  19. const char *cmd)
  20. {
  21. const char *normal = "normal";
  22. int magic = 0;
  23. struct mode_info *info;
  24. if (!cmd)
  25. cmd = normal;
  26. list_for_each_entry(info, &reboot->head, list) {
  27. if (!strcmp(info->mode, cmd)) {
  28. magic = info->magic;
  29. break;
  30. }
  31. }
  32. return magic;
  33. }
  34. static int reboot_mode_notify(struct notifier_block *this,
  35. unsigned long mode, void *cmd)
  36. {
  37. struct reboot_mode_driver *reboot;
  38. unsigned int magic;
  39. reboot = container_of(this, struct reboot_mode_driver, reboot_notifier);
  40. magic = get_reboot_mode_magic(reboot, cmd);
  41. if (magic)
  42. reboot->write(reboot, magic);
  43. return NOTIFY_DONE;
  44. }
  45. /**
  46. * reboot_mode_register - register a reboot mode driver
  47. * @reboot: reboot mode driver
  48. *
  49. * Returns: 0 on success or a negative error code on failure.
  50. */
  51. int reboot_mode_register(struct reboot_mode_driver *reboot)
  52. {
  53. struct mode_info *info;
  54. struct property *prop;
  55. struct device_node *np = reboot->dev->of_node;
  56. size_t len = strlen(PREFIX);
  57. int ret;
  58. INIT_LIST_HEAD(&reboot->head);
  59. for_each_property_of_node(np, prop) {
  60. if (strncmp(prop->name, PREFIX, len))
  61. continue;
  62. info = devm_kzalloc(reboot->dev, sizeof(*info), GFP_KERNEL);
  63. if (!info) {
  64. ret = -ENOMEM;
  65. goto error;
  66. }
  67. if (of_property_read_u32(np, prop->name, &info->magic)) {
  68. dev_err(reboot->dev, "reboot mode %s without magic number\n",
  69. info->mode);
  70. devm_kfree(reboot->dev, info);
  71. continue;
  72. }
  73. info->mode = kstrdup_const(prop->name + len, GFP_KERNEL);
  74. if (!info->mode) {
  75. ret = -ENOMEM;
  76. goto error;
  77. } else if (info->mode[0] == '\0') {
  78. kfree_const(info->mode);
  79. ret = -EINVAL;
  80. dev_err(reboot->dev, "invalid mode name(%s): too short!\n",
  81. prop->name);
  82. goto error;
  83. }
  84. list_add_tail(&info->list, &reboot->head);
  85. }
  86. reboot->reboot_notifier.notifier_call = reboot_mode_notify;
  87. register_reboot_notifier(&reboot->reboot_notifier);
  88. return 0;
  89. error:
  90. list_for_each_entry(info, &reboot->head, list)
  91. kfree_const(info->mode);
  92. return ret;
  93. }
  94. EXPORT_SYMBOL_GPL(reboot_mode_register);
  95. /**
  96. * reboot_mode_unregister - unregister a reboot mode driver
  97. * @reboot: reboot mode driver
  98. */
  99. int reboot_mode_unregister(struct reboot_mode_driver *reboot)
  100. {
  101. struct mode_info *info;
  102. unregister_reboot_notifier(&reboot->reboot_notifier);
  103. list_for_each_entry(info, &reboot->head, list)
  104. kfree_const(info->mode);
  105. return 0;
  106. }
  107. EXPORT_SYMBOL_GPL(reboot_mode_unregister);
  108. static void devm_reboot_mode_release(struct device *dev, void *res)
  109. {
  110. reboot_mode_unregister(*(struct reboot_mode_driver **)res);
  111. }
  112. /**
  113. * devm_reboot_mode_register() - resource managed reboot_mode_register()
  114. * @dev: device to associate this resource with
  115. * @reboot: reboot mode driver
  116. *
  117. * Returns: 0 on success or a negative error code on failure.
  118. */
  119. int devm_reboot_mode_register(struct device *dev,
  120. struct reboot_mode_driver *reboot)
  121. {
  122. struct reboot_mode_driver **dr;
  123. int rc;
  124. dr = devres_alloc(devm_reboot_mode_release, sizeof(*dr), GFP_KERNEL);
  125. if (!dr)
  126. return -ENOMEM;
  127. rc = reboot_mode_register(reboot);
  128. if (rc) {
  129. devres_free(dr);
  130. return rc;
  131. }
  132. *dr = reboot;
  133. devres_add(dev, dr);
  134. return 0;
  135. }
  136. EXPORT_SYMBOL_GPL(devm_reboot_mode_register);
  137. static int devm_reboot_mode_match(struct device *dev, void *res, void *data)
  138. {
  139. struct reboot_mode_driver **p = res;
  140. if (WARN_ON(!p || !*p))
  141. return 0;
  142. return *p == data;
  143. }
  144. /**
  145. * devm_reboot_mode_unregister() - resource managed reboot_mode_unregister()
  146. * @dev: device to associate this resource with
  147. * @reboot: reboot mode driver
  148. */
  149. void devm_reboot_mode_unregister(struct device *dev,
  150. struct reboot_mode_driver *reboot)
  151. {
  152. WARN_ON(devres_release(dev,
  153. devm_reboot_mode_release,
  154. devm_reboot_mode_match, reboot));
  155. }
  156. EXPORT_SYMBOL_GPL(devm_reboot_mode_unregister);
  157. MODULE_AUTHOR("Andy Yan <andy.yan@rock-chips.com>");
  158. MODULE_DESCRIPTION("System reboot mode core library");
  159. MODULE_LICENSE("GPL v2");