target/linux/generic/files/drivers/platform/mikrotik/routerboot.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Driver for MikroTik RouterBoot flash data. Common routines.
   4  *
   5  * Copyright (C) 2020 Thibaut VARÈNE <hacks+kernel@slashdirt.org>
   6  *
   7  * This program is free software; you can redistribute it and/or modify it
   8  * under the terms of the GNU General Public License version 2 as published
   9  * by the Free Software Foundation.
  10  */
  11
  12 #include <linux/types.h>
  13 #include <linux/module.h>
  14 #include <linux/kernel.h>
  15 #include <linux/sysfs.h>
  16 #include <linux/mtd/mtd.h>
  17
  18 #include "routerboot.h"
  19
  20 static struct kobject *rb_kobj;
  21
  22 /**
  23  * routerboot_tag_find() - Locate a given tag in routerboot config data.
  24  * @bufhead: the buffer to look into. Must start with a tag node.
  25  * @buflen: size of bufhead
  26  * @tag_id: the tag identifier to look for
  27  * @pld_ofs: will be updated with tag payload offset in bufhead, if tag found
  28  * @pld_len: will be updated with tag payload size, if tag found
  29  *
  30  * This incarnation of tag_find() does only that: it finds a specific routerboot
  31  * tag node in the input buffer. Routerboot tag nodes are u32 values:
  32  * - The low nibble is the tag identification number,
  33  * - The high nibble is the tag payload length (node excluded) in bytes.
  34  * The payload immediately follows the tag node. Tag nodes are 32bit-aligned.
  35  * The returned pld_ofs will always be aligned. pld_len may not end on 32bit
  36  * boundary (the only known case is when parsing ERD data).
  37  * The nodes are cpu-endian on the flash media. The payload is cpu-endian when
  38  * applicable. Tag nodes are not ordered (by ID) on flash.
  39  *
  40  * Return: 0 on success (tag found) or errno
  41  */
  42 int routerboot_tag_find(const u8 *bufhead, const size_t buflen, const u16 tag_id,
  43                         u16 *pld_ofs, u16 *pld_len)
  44 {
  45         const u32 *datum, *bufend;
  46         u32 node;
  47         u16 id, len;
  48         int ret;
  49
  50         if (!bufhead || !tag_id)
  51                 return -EINVAL;
  52
  53         ret = -ENOENT;
  54         datum = (const u32 *)bufhead;
  55         bufend = (const u32 *)(bufhead + buflen);
  56
  57         while (datum < bufend) {
  58                 node = *datum++;
  59
  60                 /* Tag list ends with null node */
  61                 if (!node)
  62                         break;
  63
  64                 id = node & 0xFFFF;
  65                 len = node >> 16;
  66
  67                 if (tag_id == id) {
  68                         if (datum >= bufend)
  69                                 break;
  70
  71                         if (pld_ofs)
  72                                 *pld_ofs = (u16)((u8 *)datum - bufhead);
  73                         if (pld_len)
  74                                 *pld_len = len;
  75
  76                         ret = 0;
  77                         break;
  78                 }
  79
  80                 /*
  81                  * The only known situation where len may not end on 32bit
  82                  * boundary is within ERD data. Since we're only extracting
  83                  * one tag (the first and only one) from that data, we should
  84                  * never need to forcefully ALIGN(). Do it anyway, this is not a
  85                  * performance path.
  86                  */
  87                 len = ALIGN(len, sizeof(*datum));
  88                 datum += len / sizeof(*datum);
  89         }
  90
  91         return ret;
  92 }
  93
  94 /**
  95  * routerboot_rle_decode() - Simple RLE (MikroTik variant) decoding routine.
  96  * @in: input buffer to decode
  97  * @inlen: size of in
  98  * @out: output buffer to write decoded data to
  99  * @outlen: pointer to out size when function is called, will be updated with
 100  * size of decoded output on return
 101  *
 102  * MikroTik's variant of RLE operates as follows, considering a signed run byte:
 103  * - positive run => classic RLE
 104  * - negative run => the next -<run> bytes must be copied verbatim
 105  * The API is matched to the lzo1x routines for convenience.
 106  *
 107  * NB: The output buffer cannot overlap with the input buffer.
 108  *
 109  * Return: 0 on success or errno
 110  */
 111 int routerboot_rle_decode(const u8 *in, size_t inlen, u8 *out, size_t *outlen)
 112 {
 113         int ret, run, nbytes;   // use native types for speed
 114         u8 byte;
 115
 116         if (!in || (inlen < 2) || !out)
 117                 return -EINVAL;
 118
 119         ret = -ENOSPC;
 120         nbytes = 0;
 121         while (inlen >= 2) {
 122                 run = *in++;
 123                 inlen--;
 124
 125                 /* Verbatim copies */
 126                 if (run & 0x80) {
 127                         /* Invert run byte sign */
 128                         run = ~run & 0xFF;
 129                         run++;
 130
 131                         if (run > inlen)
 132                                 goto fail;
 133
 134                         inlen -= run;
 135
 136                         nbytes += run;
 137                         if (nbytes > *outlen)
 138                                 goto fail;
 139
 140                         /* Basic memcpy */
 141                         while (run-- > 0)
 142                                 *out++ = *in++;
 143                 }
 144                 /* Stream of half-words RLE: <run><byte>. run == 0 is ignored */
 145                 else {
 146                         byte = *in++;
 147                         inlen--;
 148
 149                         nbytes += run;
 150                         if (nbytes > *outlen)
 151                                 goto fail;
 152
 153                         while (run-- > 0)
 154                                 *out++ = byte;
 155                 }
 156         }
 157
 158         ret = 0;
 159 fail:
 160         *outlen = nbytes;
 161         return ret;
 162 }
 163
 164 static void routerboot_mtd_notifier_add(struct mtd_info *mtd)
 165 {
 166         /* Currently routerboot is only known to live on NOR flash */
 167         if (mtd->type != MTD_NORFLASH)
 168                 return;
 169
 170         /*
 171          * We ignore the following return values and always register.
 172          * These init() routines are designed so that their failed state is
 173          * always manageable by the corresponding exit() calls.
 174          * Notifier is called with MTD mutex held: use __get/__put variants.
 175          * TODO: allow partition names override
 176          */
 177         if (!strcmp(mtd->name, RB_MTD_HARD_CONFIG))
 178                 rb_hardconfig_init(rb_kobj, mtd);
 179         else if (!strcmp(mtd->name, RB_MTD_SOFT_CONFIG))
 180                 rb_softconfig_init(rb_kobj, mtd);
 181 }
 182
 183 static void routerboot_mtd_notifier_remove(struct mtd_info *mtd)
 184 {
 185         if (mtd->type != MTD_NORFLASH)
 186                 return;
 187
 188         if (!strcmp(mtd->name, RB_MTD_HARD_CONFIG))
 189                 rb_hardconfig_exit();
 190         else if (!strcmp(mtd->name, RB_MTD_SOFT_CONFIG))
 191                 rb_softconfig_exit();
 192 }
 193
 194 /* Note: using a notifier prevents qualifying init()/exit() functions with __init/__exit */
 195 static struct mtd_notifier routerboot_mtd_notifier = {
 196         .add = routerboot_mtd_notifier_add,
 197         .remove = routerboot_mtd_notifier_remove,
 198 };
 199
 200 static int __init routerboot_init(void)
 201 {
 202         rb_kobj = kobject_create_and_add("mikrotik", firmware_kobj);
 203         if (!rb_kobj)
 204                 return -ENOMEM;
 205
 206         register_mtd_user(&routerboot_mtd_notifier);
 207
 208         return 0;
 209 }
 210
 211 static void __exit routerboot_exit(void)
 212 {
 213         unregister_mtd_user(&routerboot_mtd_notifier);
 214         /* Exit routines are idempotent */
 215         rb_softconfig_exit();
 216         rb_hardconfig_exit();
 217         kobject_put(rb_kobj);   // recursive afaict
 218 }
 219
 220 /* Common routines */
 221
 222 ssize_t routerboot_tag_show_string(const u8 *pld, u16 pld_len, char *buf)
 223 {
 224         return scnprintf(buf, pld_len+1, "%s\n", pld);
 225 }
 226
 227 ssize_t routerboot_tag_show_u32s(const u8 *pld, u16 pld_len, char *buf)
 228 {
 229         char *out = buf;
 230         u32 *data;      // cpu-endian
 231
 232         /* Caller ensures pld_len > 0 */
 233         if (pld_len % sizeof(*data))
 234                 return -EINVAL;
 235
 236         data = (u32 *)pld;
 237
 238         do {
 239                 out += sprintf(out, "0x%08x\n", *data);
 240                 data++;
 241         } while ((pld_len -= sizeof(*data)));
 242
 243         return out - buf;
 244 }
 245
 246 module_init(routerboot_init);
 247 module_exit(routerboot_exit);
 248
 249 MODULE_LICENSE("GPL v2");
 250 MODULE_DESCRIPTION("MikroTik RouterBoot sysfs support");
 251 MODULE_AUTHOR("Thibaut VARENE");