drivers/power/reset/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
  16 struct mode_info {
  17         const char *mode;
  18         u32 magic;
  19         struct list_head list;
  20 };
  21
  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
  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  */
  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  */
 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
 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  */
 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
 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  */
 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 <[email protected]>");
 194 MODULE_DESCRIPTION("System reboot mode core library");
 195 MODULE_LICENSE("GPL v2");