ar71xx: om2p-hs: use correct bit mask to disable JTAG functionality
[openwrt/svn-archive/archive.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_tag(u8 *buf, unsigned int buflen, u16 tag_id,
34 u8 **tag_data, u16 *tag_len)
35 {
36 uint32_t magic;
37 int ret;
38
39 if (buflen < 4)
40 return -EINVAL;
41
42 magic = get_u32(buf);
43 switch (magic) {
44 case RB_MAGIC_HARD:
45 /* skip magic value */
46 buf += 4;
47 buflen -= 4;
48 break;
49
50 case RB_MAGIC_SOFT:
51 if (buflen < 8)
52 return -EINVAL;
53
54 /* skip magic and CRC value */
55 buf += 8;
56 buflen -= 8;
57
58 break;
59
60 default:
61 return -EINVAL;
62 }
63
64 ret = -ENOENT;
65 while (buflen > 2) {
66 u16 id;
67 u16 len;
68
69 len = get_u16(buf);
70 buf += 2;
71 buflen -= 2;
72
73 if (buflen < 2)
74 break;
75
76 id = get_u16(buf);
77 buf += 2;
78 buflen -= 2;
79
80 if (id == RB_ID_TERMINATOR)
81 break;
82
83 if (buflen < len)
84 break;
85
86 if (id == tag_id) {
87 if (tag_len)
88 *tag_len = len;
89 if (tag_data)
90 *tag_data = buf;
91 ret = 0;
92 break;
93 }
94
95 buf += len;
96 buflen -= len;
97 }
98
99 return ret;
100 }