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 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
reboot_mode_notify(struct notifier_block * this,unsigned long mode,void * cmd)42 static 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 */
reboot_mode_register(struct reboot_mode_driver * reboot)62 int 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
109 error:
110 list_for_each_entry(info, &reboot->head, list)
111 kfree_const(info->mode);
112
113 return ret;
114 }
115 EXPORT_SYMBOL_GPL(reboot_mode_register);
116
117 /**
118 * reboot_mode_unregister - unregister a reboot mode driver
119 * @reboot: reboot mode driver
120 */
reboot_mode_unregister(struct reboot_mode_driver * reboot)121 int 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 }
132 EXPORT_SYMBOL_GPL(reboot_mode_unregister);
133
devm_reboot_mode_release(struct device * dev,void * res)134 static 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 */
devm_reboot_mode_register(struct device * dev,struct reboot_mode_driver * reboot)146 int 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 }
167 EXPORT_SYMBOL_GPL(devm_reboot_mode_register);
168
devm_reboot_mode_match(struct device * dev,void * res,void * data)169 static 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 */
devm_reboot_mode_unregister(struct device * dev,struct reboot_mode_driver * reboot)184 void 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 }
191 EXPORT_SYMBOL_GPL(devm_reboot_mode_unregister);
192
193 MODULE_AUTHOR("Andy Yan <andy.yan@rock-chips.com>");
194 MODULE_DESCRIPTION("System reboot mode core library");
195 MODULE_LICENSE("GPL v2");
196