ar71xx: add hard_cfg offset detection for rb2011 support - fixes wlan on some newer...
[openwrt.git] / 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 #include <linux/kernel.h>
12 #include <linux/errno.h>
13 #include <linux/routerboot.h>
14
15 #include "routerboot.h"
16
17 static u32 get_u32(void *buf)
18 {
19         u8 *p = buf;
20
21         return ((u32) p[3] + ((u32) p[2] << 8) + ((u32) p[1] << 16) +
22                ((u32) p[0] << 24));
23 }
24
25 static u16 get_u16(void *buf)
26 {
27         u8 *p = buf;
28
29         return (u16) p[1] + ((u16) p[0] << 8);
30 }
31
32 __init int
33 routerboot_find_magic(u8 *buf, unsigned int buflen, u32 *offset, bool hard)
34 {
35         u32 magic_ref = hard ? RB_MAGIC_HARD : RB_MAGIC_SOFT;
36         u32 magic;
37         u32 cur = *offset;
38
39         while (cur < buflen) {
40                 magic = get_u32(buf + cur);
41                 if (magic == magic_ref) {
42                         *offset = cur;
43                         return 0;
44                 }
45
46                 cur += 0x1000;
47         }
48
49         return -ENOENT;
50 }
51
52 __init int
53 routerboot_find_tag(u8 *buf, unsigned int buflen, u16 tag_id,
54                     u8 **tag_data, u16 *tag_len)
55 {
56         uint32_t magic;
57         int ret;
58
59         if (buflen < 4)
60                 return -EINVAL;
61
62         magic = get_u32(buf);
63         switch (magic) {
64         case RB_MAGIC_HARD:
65                 /* skip magic value */
66                 buf += 4;
67                 buflen -= 4;
68                 break;
69
70         case RB_MAGIC_SOFT:
71                 if (buflen < 8)
72                         return -EINVAL;
73
74                 /* skip magic and CRC value */
75                 buf += 8;
76                 buflen -= 8;
77
78                 break;
79
80         default:
81                 return -EINVAL;
82         }
83
84         ret = -ENOENT;
85         while (buflen > 2) {
86                 u16 id;
87                 u16 len;
88
89                 len = get_u16(buf);
90                 buf += 2;
91                 buflen -= 2;
92
93                 if (buflen < 2)
94                         break;
95
96                 id = get_u16(buf);
97                 buf += 2;
98                 buflen -= 2;
99
100                 if (id == RB_ID_TERMINATOR)
101                         break;
102
103                 if (buflen < len)
104                         break;
105
106                 if (id == tag_id) {
107                         if (tag_len)
108                                 *tag_len = len;
109                         if (tag_data)
110                                 *tag_data = buf;
111                         ret = 0;
112                         break;
113                 }
114
115                 buf += len;
116                 buflen -= len;
117         }
118
119         return ret;
120 }