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