target/linux/ar71xx/files/arch/mips/ath79/routerboot.c

   1 /*
   2  *  RouterBoot helper routines
   3  *
   4  *  Copyright (C) 2012 Gabor Juhos <juhosg@openwrt.org>
   5  *
   6  *  This program is free software; you can redistribute it and/or modify it
   7  *  under the terms of the GNU General Public License version 2 as published
   8  *  by the Free Software Foundation.
   9  */
  10
  11 #define pr_fmt(fmt) "rb: " fmt
  12
  13 #include <linux/kernel.h>
  14 #include <linux/slab.h>
  15 #include <linux/errno.h>
  16 #include <linux/routerboot.h>
  17 #include <linux/rle.h>
  18 #include <linux/lzo.h>
  19
  20 #include "routerboot.h"
  21
  22 #define RB_BLOCK_SIZE           0x1000
  23 #define RB_ART_SIZE             0x10000
  24 #define RB_MAGIC_ERD            0x00455244      /* extended radio data */
  25
  26 static struct rb_info rb_info;
  27
  28 static u32 get_u32(void *buf)
  29 {
  30         u8 *p = buf;
  31
  32         return ((u32) p[3] + ((u32) p[2] << 8) + ((u32) p[1] << 16) +
  33                ((u32) p[0] << 24));
  34 }
  35
  36 static u16 get_u16(void *buf)
  37 {
  38         u8 *p = buf;
  39
  40         return (u16) p[1] + ((u16) p[0] << 8);
  41 }
  42
  43 __init int
  44 routerboot_find_magic(u8 *buf, unsigned int buflen, u32 *offset, bool hard)
  45 {
  46         u32 magic_ref = hard ? RB_MAGIC_HARD : RB_MAGIC_SOFT;
  47         u32 magic;
  48         u32 cur = *offset;
  49
  50         while (cur < buflen) {
  51                 magic = get_u32(buf + cur);
  52                 if (magic == magic_ref) {
  53                         *offset = cur;
  54                         return 0;
  55                 }
  56
  57                 cur += 0x1000;
  58         }
  59
  60         return -ENOENT;
  61 }
  62
  63 __init int
  64 routerboot_find_tag(u8 *buf, unsigned int buflen, u16 tag_id,
  65                     u8 **tag_data, u16 *tag_len)
  66 {
  67         uint32_t magic;
  68         bool align = false;
  69         int ret;
  70
  71         if (buflen < 4)
  72                 return -EINVAL;
  73
  74         magic = get_u32(buf);
  75         switch (magic) {
  76         case RB_MAGIC_ERD:
  77                 align = true;
  78                 /* fall trough */
  79         case RB_MAGIC_HARD:
  80                 /* skip magic value */
  81                 buf += 4;
  82                 buflen -= 4;
  83                 break;
  84
  85         case RB_MAGIC_SOFT:
  86                 if (buflen < 8)
  87                         return -EINVAL;
  88
  89                 /* skip magic and CRC value */
  90                 buf += 8;
  91                 buflen -= 8;
  92
  93                 break;
  94
  95         default:
  96                 return -EINVAL;
  97         }
  98
  99         ret = -ENOENT;
 100         while (buflen > 2) {
 101                 u16 id;
 102                 u16 len;
 103
 104                 len = get_u16(buf);
 105                 buf += 2;
 106                 buflen -= 2;
 107
 108                 if (buflen < 2)
 109                         break;
 110
 111                 id = get_u16(buf);
 112                 buf += 2;
 113                 buflen -= 2;
 114
 115                 if (id == RB_ID_TERMINATOR)
 116                         break;
 117
 118                 if (buflen < len)
 119                         break;
 120
 121                 if (id == tag_id) {
 122                         if (tag_len)
 123                                 *tag_len = len;
 124                         if (tag_data)
 125                                 *tag_data = buf;
 126                         ret = 0;
 127                         break;
 128                 }
 129
 130                 if (align)
 131                         len = (len + 3) / 4;
 132
 133                 buf += len;
 134                 buflen -= len;
 135         }
 136
 137         return ret;
 138 }
 139
 140 static inline int
 141 rb_find_hard_cfg_tag(u16 tag_id, u8 **tag_data, u16 *tag_len)
 142 {
 143         if (!rb_info.hard_cfg_data ||
 144             !rb_info.hard_cfg_size)
 145                 return -ENOENT;
 146
 147         return routerboot_find_tag(rb_info.hard_cfg_data,
 148                                    rb_info.hard_cfg_size,
 149                                    tag_id, tag_data, tag_len);
 150 }
 151
 152 __init const char *
 153 rb_get_board_name(void)
 154 {
 155         u16 tag_len;
 156         u8 *tag;
 157         int err;
 158
 159         err = rb_find_hard_cfg_tag(RB_ID_BOARD_NAME, &tag, &tag_len);
 160         if (err)
 161                 return NULL;
 162
 163         return tag;
 164 }
 165
 166 __init u32
 167 rb_get_hw_options(void)
 168 {
 169         u16 tag_len;
 170         u8 *tag;
 171         int err;
 172
 173         err = rb_find_hard_cfg_tag(RB_ID_HW_OPTIONS, &tag, &tag_len);
 174         if (err)
 175                 return 0;
 176
 177         return get_u32(tag);
 178 }
 179
 180 static void * __init
 181 __rb_get_wlan_data(u16 id)
 182 {
 183         u16 tag_len;
 184         u8 *tag;
 185         void *buf;
 186         int err;
 187         u32 magic;
 188         size_t src_done;
 189         size_t dst_done;
 190
 191         err = rb_find_hard_cfg_tag(RB_ID_WLAN_DATA, &tag, &tag_len);
 192         if (err) {
 193                 pr_err("no calibration data found\n");
 194                 goto err;
 195         }
 196
 197         buf = kmalloc(RB_ART_SIZE, GFP_KERNEL);
 198         if (buf == NULL) {
 199                 pr_err("no memory for calibration data\n");
 200                 goto err;
 201         }
 202
 203         magic = get_u32(tag);
 204         if (magic == RB_MAGIC_ERD) {
 205                 u8 *erd_data;
 206                 u16 erd_len;
 207
 208                 if (id == 0)
 209                         goto err_free;
 210
 211                 err = routerboot_find_tag(tag, tag_len, id,
 212                                           &erd_data, &erd_len);
 213                 if (err) {
 214                         pr_err("no ERD data found for id %u\n", id);
 215                         goto err_free;
 216                 }
 217
 218                 dst_done = RB_ART_SIZE;
 219                 err = lzo1x_decompress_safe(erd_data, erd_len, buf, &dst_done);
 220                 if (err) {
 221                         pr_err("unable to decompress calibration data %d\n",
 222                                err);
 223                         goto err_free;
 224                 }
 225         } else {
 226                 if (id != 0)
 227                         goto err_free;
 228
 229                 err = rle_decode((char *) tag, tag_len, buf, RB_ART_SIZE,
 230                                  &src_done, &dst_done);
 231                 if (err) {
 232                         pr_err("unable to decode calibration data\n");
 233                         goto err_free;
 234                 }
 235         }
 236
 237         return buf;
 238
 239 err_free:
 240         kfree(buf);
 241 err:
 242         return NULL;
 243 }
 244
 245 __init void *
 246 rb_get_wlan_data(void)
 247 {
 248         return __rb_get_wlan_data(0);
 249 }
 250
 251 __init void *
 252 rb_get_ext_wlan_data(u16 id)
 253 {
 254         return __rb_get_wlan_data(id);
 255 }
 256
 257 __init const struct rb_info *
 258 rb_init_info(void *data, unsigned int size)
 259 {
 260         unsigned int offset;
 261
 262         if (size == 0 || (size % RB_BLOCK_SIZE) != 0)
 263                 return NULL;
 264
 265         for (offset = 0; offset < size; offset += RB_BLOCK_SIZE) {
 266                 u32 magic;
 267
 268                 magic = get_u32(data + offset);
 269                 switch (magic) {
 270                 case RB_MAGIC_HARD:
 271                         rb_info.hard_cfg_offs = offset;
 272                         break;
 273
 274                 case RB_MAGIC_SOFT:
 275                         rb_info.soft_cfg_offs = offset;
 276                         break;
 277                 }
 278         }
 279
 280         if (!rb_info.hard_cfg_offs) {
 281                 pr_err("could not find a valid RouterBOOT hard config\n");
 282                 return NULL;
 283         }
 284
 285         if (!rb_info.soft_cfg_offs) {
 286                 pr_err("could not find a valid RouterBOOT soft config\n");
 287                 return NULL;
 288         }
 289
 290         rb_info.hard_cfg_size = RB_BLOCK_SIZE;
 291         rb_info.hard_cfg_data = kmemdup(data + rb_info.hard_cfg_offs,
 292                                         RB_BLOCK_SIZE, GFP_KERNEL);
 293         if (!rb_info.hard_cfg_data)
 294                 return NULL;
 295
 296         rb_info.board_name = rb_get_board_name();
 297         rb_info.hw_options = rb_get_hw_options();
 298
 299         return &rb_info;
 300 }