reboot-mode.c

  1// SPDX-License-Identifier: GPL-2.0-or-later
  2/*
  3 * Copyright (c) 2016, Fuzhou Rockchip Electronics Co., Ltd
  4 */
  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	struct mode_info *info;
 27	char cmd_[110];
 28
 29	if (!cmd)
 30		cmd = normal;
 31
 32	list_for_each_entry(info, &reboot->head, list)
 33		if (!strcmp(info->mode, cmd))
 34			return info->magic;
 35
 36	/* try to match again, replacing characters impossible in DT */
 37	if (strscpy(cmd_, cmd, sizeof(cmd_)) == -E2BIG)
 38		return 0;
 39
 40	strreplace(cmd_, ' ', '-');
 41	strreplace(cmd_, ',', '-');
 42	strreplace(cmd_, '/', '-');
 43
 44	list_for_each_entry(info, &reboot->head, list)
 45		if (!strcmp(info->mode, cmd_))
 46			return info->magic;
 47
 48	return 0;
 49}
 50
 51static int reboot_mode_notify(struct notifier_block *this,
 52			      unsigned long mode, void *cmd)
 53{
 54	struct reboot_mode_driver *reboot;
 55	unsigned int magic;
 56
 57	reboot = container_of(this, struct reboot_mode_driver, reboot_notifier);
 58	magic = get_reboot_mode_magic(reboot, cmd);
 59	if (magic)
 60		reboot->write(reboot, magic);
 61
 62	return NOTIFY_DONE;
 63}
 64
 65/**
 66 * reboot_mode_register - register a reboot mode driver
 67 * @reboot: reboot mode driver
 68 *
 69 * Returns: 0 on success or a negative error code on failure.
 70 */
 71int reboot_mode_register(struct reboot_mode_driver *reboot)
 72{
 73	struct mode_info *info;
 74	struct property *prop;
 75	struct device_node *np = reboot->dev->of_node;
 76	size_t len = strlen(PREFIX);
 77	int ret;
 78
 79	INIT_LIST_HEAD(&reboot->head);
 80
 81	for_each_property_of_node(np, prop) {
 82		if (strncmp(prop->name, PREFIX, len))
 83			continue;
 84
 85		info = devm_kzalloc(reboot->dev, sizeof(*info), GFP_KERNEL);
 86		if (!info) {
 87			ret = -ENOMEM;
 88			goto error;
 89		}
 90
 91		if (of_property_read_u32(np, prop->name, &info->magic)) {
 92			dev_err(reboot->dev, "reboot mode %s without magic number\n",
 93				info->mode);
 94			devm_kfree(reboot->dev, info);
 95			continue;
 96		}
 97
 98		info->mode = kstrdup_const(prop->name + len, GFP_KERNEL);
 99		if (!info->mode) {
100			ret =  -ENOMEM;
101			goto error;
102		} else if (info->mode[0] == '\0') {
103			kfree_const(info->mode);
104			ret = -EINVAL;
105			dev_err(reboot->dev, "invalid mode name(%s): too short!\n",
106				prop->name);
107			goto error;
108		}
109
110		list_add_tail(&info->list, &reboot->head);
111	}
112
113	reboot->reboot_notifier.notifier_call = reboot_mode_notify;
114	register_reboot_notifier(&reboot->reboot_notifier);
115
116	return 0;
117
118error:
119	list_for_each_entry(info, &reboot->head, list)
120		kfree_const(info->mode);
121
122	return ret;
123}
124EXPORT_SYMBOL_GPL(reboot_mode_register);
125
126/**
127 * reboot_mode_unregister - unregister a reboot mode driver
128 * @reboot: reboot mode driver
129 */
130int reboot_mode_unregister(struct reboot_mode_driver *reboot)
131{
132	struct mode_info *info;
133
134	unregister_reboot_notifier(&reboot->reboot_notifier);
135
136	list_for_each_entry(info, &reboot->head, list)
137		kfree_const(info->mode);
138
139	return 0;
140}
141EXPORT_SYMBOL_GPL(reboot_mode_unregister);
142
143static void devm_reboot_mode_release(struct device *dev, void *res)
144{
145	reboot_mode_unregister(*(struct reboot_mode_driver **)res);
146}
147
148/**
149 * devm_reboot_mode_register() - resource managed reboot_mode_register()
150 * @dev: device to associate this resource with
151 * @reboot: reboot mode driver
152 *
153 * Returns: 0 on success or a negative error code on failure.
154 */
155int devm_reboot_mode_register(struct device *dev,
156			      struct reboot_mode_driver *reboot)
157{
158	struct reboot_mode_driver **dr;
159	int rc;
160
161	dr = devres_alloc(devm_reboot_mode_release, sizeof(*dr), GFP_KERNEL);
162	if (!dr)
163		return -ENOMEM;
164
165	rc = reboot_mode_register(reboot);
166	if (rc) {
167		devres_free(dr);
168		return rc;
169	}
170
171	*dr = reboot;
172	devres_add(dev, dr);
173
174	return 0;
175}
176EXPORT_SYMBOL_GPL(devm_reboot_mode_register);
177
178static int devm_reboot_mode_match(struct device *dev, void *res, void *data)
179{
180	struct reboot_mode_driver **p = res;
181
182	if (WARN_ON(!p || !*p))
183		return 0;
184
185	return *p == data;
186}
187
188/**
189 * devm_reboot_mode_unregister() - resource managed reboot_mode_unregister()
190 * @dev: device to associate this resource with
191 * @reboot: reboot mode driver
192 */
193void devm_reboot_mode_unregister(struct device *dev,
194				 struct reboot_mode_driver *reboot)
195{
196	WARN_ON(devres_release(dev,
197			       devm_reboot_mode_release,
198			       devm_reboot_mode_match, reboot));
199}
200EXPORT_SYMBOL_GPL(devm_reboot_mode_unregister);
201
202MODULE_AUTHOR("Andy Yan <andy.yan@rock-chips.com>");
203MODULE_DESCRIPTION("System reboot mode core library");
204MODULE_LICENSE("GPL v2");