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
17static 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
25static 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
33routerboot_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}
101