drivers/power/reset/reboot-mode.c

   1 /*
   2  * Copyright (c) 2016, Fuzhou Rockchip Electronics Co., Ltd
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  */
   9
  10 #include <linux/device.h>
  11 #include <linux/init.h>
  12 #include <linux/kernel.h>
  13 #include <linux/module.h>
  14 #include <linux/of.h>
  15 #include <linux/reboot.h>
  16 #include <linux/reboot-mode.h>
  17
  18 #define PREFIX "mode-"
  19
  20 struct mode_info {
  21         const char *mode;
  22         u32 magic;
  23         struct list_head list;
  24 };
  25
  26 static unsigned int get_reboot_mode_magic(struct reboot_mode_driver *reboot,
  27                                           const char *cmd)
  28 {
  29         const char *normal = "normal";
  30         int magic = 0;
  31         struct mode_info *info;
  32
  33         if (!cmd)
  34                 cmd = normal;
  35
  36         list_for_each_entry(info, &reboot->head, list) {
  37                 if (!strcmp(info->mode, cmd)) {
  38                         magic = info->magic;
  39                         break;
  40                 }
  41         }
  42
  43         return magic;
  44 }
  45
  46 static int reboot_mode_notify(struct notifier_block *this,
  47                               unsigned long mode, void *cmd)
  48 {
  49         struct reboot_mode_driver *reboot;
  50         unsigned int magic;
  51
  52         reboot = container_of(this, struct reboot_mode_driver, reboot_notifier);
  53         magic = get_reboot_mode_magic(reboot, cmd);
  54         if (magic)
  55                 reboot->write(reboot, magic);
  56
  57         return NOTIFY_DONE;
  58 }
  59
  60 /**
  61  * reboot_mode_register - register a reboot mode driver
  62  * @reboot: reboot mode driver
  63  *
  64  * Returns: 0 on success or a negative error code on failure.
  65  */
  66 int reboot_mode_register(struct reboot_mode_driver *reboot)
  67 {
  68         struct mode_info *info;
  69         struct property *prop;
  70         struct device_node *np = reboot->dev->of_node;
  71         size_t len = strlen(PREFIX);
  72         int ret;
  73
  74         INIT_LIST_HEAD(&reboot->head);
  75
  76         for_each_property_of_node(np, prop) {
  77                 if (strncmp(prop->name, PREFIX, len))
  78                         continue;
  79
  80                 info = devm_kzalloc(reboot->dev, sizeof(*info), GFP_KERNEL);
  81                 if (!info) {
  82                         ret = -ENOMEM;
  83                         goto error;
  84                 }
  85
  86                 if (of_property_read_u32(np, prop->name, &info->magic)) {
  87                         dev_err(reboot->dev, "reboot mode %s without magic number\n",
  88                                 info->mode);
  89                         devm_kfree(reboot->dev, info);
  90                         continue;
  91                 }
  92
  93                 info->mode = kstrdup_const(prop->name + len, GFP_KERNEL);
  94                 if (!info->mode) {
  95                         ret =  -ENOMEM;
  96                         goto error;
  97                 } else if (info->mode[0] == '\0') {
  98                         kfree_const(info->mode);
  99                         ret = -EINVAL;
 100                         dev_err(reboot->dev, "invalid mode name(%s): too short!\n",
 101                                 prop->name);
 102                         goto error;
 103                 }
 104
 105                 list_add_tail(&info->list, &reboot->head);
 106         }
 107
 108         reboot->reboot_notifier.notifier_call = reboot_mode_notify;
 109         register_reboot_notifier(&reboot->reboot_notifier);
 110
 111         return 0;
 112
 113 error:
 114         list_for_each_entry(info, &reboot->head, list)
 115                 kfree_const(info->mode);
 116
 117         return ret;
 118 }
 119 EXPORT_SYMBOL_GPL(reboot_mode_register);
 120
 121 /**
 122  * reboot_mode_unregister - unregister a reboot mode driver
 123  * @reboot: reboot mode driver
 124  */
 125 int reboot_mode_unregister(struct reboot_mode_driver *reboot)
 126 {
 127         struct mode_info *info;
 128
 129         unregister_reboot_notifier(&reboot->reboot_notifier);
 130
 131         list_for_each_entry(info, &reboot->head, list)
 132                 kfree_const(info->mode);
 133
 134         return 0;
 135 }
 136 EXPORT_SYMBOL_GPL(reboot_mode_unregister);
 137
 138 static void devm_reboot_mode_release(struct device *dev, void *res)
 139 {
 140         reboot_mode_unregister(*(struct reboot_mode_driver **)res);
 141 }
 142
 143 /**
 144  * devm_reboot_mode_register() - resource managed reboot_mode_register()
 145  * @dev: device to associate this resource with
 146  * @reboot: reboot mode driver
 147  *
 148  * Returns: 0 on success or a negative error code on failure.
 149  */
 150 int devm_reboot_mode_register(struct device *dev,
 151                               struct reboot_mode_driver *reboot)
 152 {
 153         struct reboot_mode_driver **dr;
 154         int rc;
 155
 156         dr = devres_alloc(devm_reboot_mode_release, sizeof(*dr), GFP_KERNEL);
 157         if (!dr)
 158                 return -ENOMEM;
 159
 160         rc = reboot_mode_register(reboot);
 161         if (rc) {
 162                 devres_free(dr);
 163                 return rc;
 164         }
 165
 166         *dr = reboot;
 167         devres_add(dev, dr);
 168
 169         return 0;
 170 }
 171 EXPORT_SYMBOL_GPL(devm_reboot_mode_register);
 172
 173 static int devm_reboot_mode_match(struct device *dev, void *res, void *data)
 174 {
 175         struct reboot_mode_driver **p = res;
 176
 177         if (WARN_ON(!p || !*p))
 178                 return 0;
 179
 180         return *p == data;
 181 }
 182
 183 /**
 184  * devm_reboot_mode_unregister() - resource managed reboot_mode_unregister()
 185  * @dev: device to associate this resource with
 186  * @reboot: reboot mode driver
 187  */
 188 void devm_reboot_mode_unregister(struct device *dev,
 189                                  struct reboot_mode_driver *reboot)
 190 {
 191         WARN_ON(devres_release(dev,
 192                                devm_reboot_mode_release,
 193                                devm_reboot_mode_match, reboot));
 194 }
 195 EXPORT_SYMBOL_GPL(devm_reboot_mode_unregister);
 196
 197 MODULE_AUTHOR("Andy Yan <andy.yan@rock-chips.com");
 198 MODULE_DESCRIPTION("System reboot mode core library");
 199 MODULE_LICENSE("GPL v2");