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	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 driver
58 * @reboot: reboot mode driver
59 *
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 driver
119 * @reboot: reboot mode driver
120 */
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 with
142 * @reboot: reboot mode driver
143 *
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 with
182 * @reboot: reboot mode driver
183 */
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