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
16 struct mode_info {
17 const char *mode;
18 u32 magic;
19 struct list_head list;
20 };
21
get_reboot_mode_magic(struct reboot_mode_driver * reboot,const char * cmd)22 static 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
reboot_mode_notify(struct notifier_block * this,unsigned long mode,void * cmd)51 static 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 */
reboot_mode_register(struct reboot_mode_driver * reboot)71 int 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
118 error:
119 list_for_each_entry(info, &reboot->head, list)
120 kfree_const(info->mode);
121
122 return ret;
123 }
124 EXPORT_SYMBOL_GPL(reboot_mode_register);
125
126 /**
127 * reboot_mode_unregister - unregister a reboot mode driver
128 * @reboot: reboot mode driver
129 */
reboot_mode_unregister(struct reboot_mode_driver * reboot)130 int 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 }
141 EXPORT_SYMBOL_GPL(reboot_mode_unregister);
142
devm_reboot_mode_release(struct device * dev,void * res)143 static 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 */
devm_reboot_mode_register(struct device * dev,struct reboot_mode_driver * reboot)155 int 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 }
176 EXPORT_SYMBOL_GPL(devm_reboot_mode_register);
177
devm_reboot_mode_match(struct device * dev,void * res,void * data)178 static 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 */
devm_reboot_mode_unregister(struct device * dev,struct reboot_mode_driver * reboot)193 void 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 }
200 EXPORT_SYMBOL_GPL(devm_reboot_mode_unregister);
201
202 MODULE_AUTHOR("Andy Yan <andy.yan@rock-chips.com>");
203 MODULE_DESCRIPTION("System reboot mode core library");
204 MODULE_LICENSE("GPL v2");
205