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