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