SiSK / EthernetInterface

fdsf

Dependents:   sisk_proj_stat MQTT Hello_FXOS8700Q WireFSHandControl ... more

lwip/core/snmp/asn1_dec.c@0:d7bd7384a37c, 2016-12-07 (annotated)

Committer:
grzemich
Date:
Wed Dec 07 23:47:50 2016 +0000
Revision:
0:d7bd7384a37c
dgd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
grzemich 0:d7bd7384a37c 1 /**
grzemich 0:d7bd7384a37c 2 * @file
grzemich 0:d7bd7384a37c 3 * Abstract Syntax Notation One (ISO 8824, 8825) decoding
grzemich 0:d7bd7384a37c 4 *
grzemich 0:d7bd7384a37c 5 * @todo not optimised (yet), favor correctness over speed, favor speed over size
grzemich 0:d7bd7384a37c 6 */
grzemich 0:d7bd7384a37c 7
grzemich 0:d7bd7384a37c 8 /*
grzemich 0:d7bd7384a37c 9 * Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
grzemich 0:d7bd7384a37c 10 * All rights reserved.
grzemich 0:d7bd7384a37c 11 *
grzemich 0:d7bd7384a37c 12 * Redistribution and use in source and binary forms, with or without modification,
grzemich 0:d7bd7384a37c 13 * are permitted provided that the following conditions are met:
grzemich 0:d7bd7384a37c 14 *
grzemich 0:d7bd7384a37c 15 * 1. Redistributions of source code must retain the above copyright notice,
grzemich 0:d7bd7384a37c 16 * this list of conditions and the following disclaimer.
grzemich 0:d7bd7384a37c 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
grzemich 0:d7bd7384a37c 18 * this list of conditions and the following disclaimer in the documentation
grzemich 0:d7bd7384a37c 19 * and/or other materials provided with the distribution.
grzemich 0:d7bd7384a37c 20 * 3. The name of the author may not be used to endorse or promote products
grzemich 0:d7bd7384a37c 21 * derived from this software without specific prior written permission.
grzemich 0:d7bd7384a37c 22 *
grzemich 0:d7bd7384a37c 23 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
grzemich 0:d7bd7384a37c 24 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
grzemich 0:d7bd7384a37c 25 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
grzemich 0:d7bd7384a37c 26 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
grzemich 0:d7bd7384a37c 27 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
grzemich 0:d7bd7384a37c 28 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
grzemich 0:d7bd7384a37c 29 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
grzemich 0:d7bd7384a37c 30 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
grzemich 0:d7bd7384a37c 31 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
grzemich 0:d7bd7384a37c 32 * OF SUCH DAMAGE.
grzemich 0:d7bd7384a37c 33 *
grzemich 0:d7bd7384a37c 34 * Author: Christiaan Simons <christiaan.simons@axon.tv>
grzemich 0:d7bd7384a37c 35 */
grzemich 0:d7bd7384a37c 36
grzemich 0:d7bd7384a37c 37 #include "lwip/opt.h"
grzemich 0:d7bd7384a37c 38
grzemich 0:d7bd7384a37c 39 #if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */
grzemich 0:d7bd7384a37c 40
grzemich 0:d7bd7384a37c 41 #include "lwip/snmp_asn1.h"
grzemich 0:d7bd7384a37c 42
grzemich 0:d7bd7384a37c 43 /**
grzemich 0:d7bd7384a37c 44 * Retrieves type field from incoming pbuf chain.
grzemich 0:d7bd7384a37c 45 *
grzemich 0:d7bd7384a37c 46 * @param p points to a pbuf holding an ASN1 coded type field
grzemich 0:d7bd7384a37c 47 * @param ofs points to the offset within the pbuf chain of the ASN1 coded type field
grzemich 0:d7bd7384a37c 48 * @param type return ASN1 type
grzemich 0:d7bd7384a37c 49 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
grzemich 0:d7bd7384a37c 50 */
grzemich 0:d7bd7384a37c 51 err_t
grzemich 0:d7bd7384a37c 52 snmp_asn1_dec_type(struct pbuf *p, u16_t ofs, u8_t *type)
grzemich 0:d7bd7384a37c 53 {
grzemich 0:d7bd7384a37c 54 u16_t plen, base;
grzemich 0:d7bd7384a37c 55 u8_t *msg_ptr;
grzemich 0:d7bd7384a37c 56
grzemich 0:d7bd7384a37c 57 plen = 0;
grzemich 0:d7bd7384a37c 58 while (p != NULL)
grzemich 0:d7bd7384a37c 59 {
grzemich 0:d7bd7384a37c 60 base = plen;
grzemich 0:d7bd7384a37c 61 plen += p->len;
grzemich 0:d7bd7384a37c 62 if (ofs < plen)
grzemich 0:d7bd7384a37c 63 {
grzemich 0:d7bd7384a37c 64 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 65 msg_ptr += ofs - base;
grzemich 0:d7bd7384a37c 66 *type = *msg_ptr;
grzemich 0:d7bd7384a37c 67 return ERR_OK;
grzemich 0:d7bd7384a37c 68 }
grzemich 0:d7bd7384a37c 69 p = p->next;
grzemich 0:d7bd7384a37c 70 }
grzemich 0:d7bd7384a37c 71 /* p == NULL, ofs >= plen */
grzemich 0:d7bd7384a37c 72 return ERR_ARG;
grzemich 0:d7bd7384a37c 73 }
grzemich 0:d7bd7384a37c 74
grzemich 0:d7bd7384a37c 75 /**
grzemich 0:d7bd7384a37c 76 * Decodes length field from incoming pbuf chain into host length.
grzemich 0:d7bd7384a37c 77 *
grzemich 0:d7bd7384a37c 78 * @param p points to a pbuf holding an ASN1 coded length
grzemich 0:d7bd7384a37c 79 * @param ofs points to the offset within the pbuf chain of the ASN1 coded length
grzemich 0:d7bd7384a37c 80 * @param octets_used returns number of octets used by the length code
grzemich 0:d7bd7384a37c 81 * @param length return host order length, upto 64k
grzemich 0:d7bd7384a37c 82 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
grzemich 0:d7bd7384a37c 83 */
grzemich 0:d7bd7384a37c 84 err_t
grzemich 0:d7bd7384a37c 85 snmp_asn1_dec_length(struct pbuf *p, u16_t ofs, u8_t *octets_used, u16_t *length)
grzemich 0:d7bd7384a37c 86 {
grzemich 0:d7bd7384a37c 87 u16_t plen, base;
grzemich 0:d7bd7384a37c 88 u8_t *msg_ptr;
grzemich 0:d7bd7384a37c 89
grzemich 0:d7bd7384a37c 90 plen = 0;
grzemich 0:d7bd7384a37c 91 while (p != NULL)
grzemich 0:d7bd7384a37c 92 {
grzemich 0:d7bd7384a37c 93 base = plen;
grzemich 0:d7bd7384a37c 94 plen += p->len;
grzemich 0:d7bd7384a37c 95 if (ofs < plen)
grzemich 0:d7bd7384a37c 96 {
grzemich 0:d7bd7384a37c 97 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 98 msg_ptr += ofs - base;
grzemich 0:d7bd7384a37c 99
grzemich 0:d7bd7384a37c 100 if (*msg_ptr < 0x80)
grzemich 0:d7bd7384a37c 101 {
grzemich 0:d7bd7384a37c 102 /* primitive definite length format */
grzemich 0:d7bd7384a37c 103 *octets_used = 1;
grzemich 0:d7bd7384a37c 104 *length = *msg_ptr;
grzemich 0:d7bd7384a37c 105 return ERR_OK;
grzemich 0:d7bd7384a37c 106 }
grzemich 0:d7bd7384a37c 107 else if (*msg_ptr == 0x80)
grzemich 0:d7bd7384a37c 108 {
grzemich 0:d7bd7384a37c 109 /* constructed indefinite length format, termination with two zero octets */
grzemich 0:d7bd7384a37c 110 u8_t zeros;
grzemich 0:d7bd7384a37c 111 u8_t i;
grzemich 0:d7bd7384a37c 112
grzemich 0:d7bd7384a37c 113 *length = 0;
grzemich 0:d7bd7384a37c 114 zeros = 0;
grzemich 0:d7bd7384a37c 115 while (zeros != 2)
grzemich 0:d7bd7384a37c 116 {
grzemich 0:d7bd7384a37c 117 i = 2;
grzemich 0:d7bd7384a37c 118 while (i > 0)
grzemich 0:d7bd7384a37c 119 {
grzemich 0:d7bd7384a37c 120 i--;
grzemich 0:d7bd7384a37c 121 (*length) += 1;
grzemich 0:d7bd7384a37c 122 ofs += 1;
grzemich 0:d7bd7384a37c 123 if (ofs >= plen)
grzemich 0:d7bd7384a37c 124 {
grzemich 0:d7bd7384a37c 125 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 126 p = p->next;
grzemich 0:d7bd7384a37c 127 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 128 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 129 plen += p->len;
grzemich 0:d7bd7384a37c 130 }
grzemich 0:d7bd7384a37c 131 else
grzemich 0:d7bd7384a37c 132 {
grzemich 0:d7bd7384a37c 133 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 134 msg_ptr++;
grzemich 0:d7bd7384a37c 135 }
grzemich 0:d7bd7384a37c 136 if (*msg_ptr == 0)
grzemich 0:d7bd7384a37c 137 {
grzemich 0:d7bd7384a37c 138 zeros++;
grzemich 0:d7bd7384a37c 139 if (zeros == 2)
grzemich 0:d7bd7384a37c 140 {
grzemich 0:d7bd7384a37c 141 /* stop while (i > 0) */
grzemich 0:d7bd7384a37c 142 i = 0;
grzemich 0:d7bd7384a37c 143 }
grzemich 0:d7bd7384a37c 144 }
grzemich 0:d7bd7384a37c 145 else
grzemich 0:d7bd7384a37c 146 {
grzemich 0:d7bd7384a37c 147 zeros = 0;
grzemich 0:d7bd7384a37c 148 }
grzemich 0:d7bd7384a37c 149 }
grzemich 0:d7bd7384a37c 150 }
grzemich 0:d7bd7384a37c 151 *octets_used = 1;
grzemich 0:d7bd7384a37c 152 return ERR_OK;
grzemich 0:d7bd7384a37c 153 }
grzemich 0:d7bd7384a37c 154 else if (*msg_ptr == 0x81)
grzemich 0:d7bd7384a37c 155 {
grzemich 0:d7bd7384a37c 156 /* constructed definite length format, one octet */
grzemich 0:d7bd7384a37c 157 ofs += 1;
grzemich 0:d7bd7384a37c 158 if (ofs >= plen)
grzemich 0:d7bd7384a37c 159 {
grzemich 0:d7bd7384a37c 160 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 161 p = p->next;
grzemich 0:d7bd7384a37c 162 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 163 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 164 }
grzemich 0:d7bd7384a37c 165 else
grzemich 0:d7bd7384a37c 166 {
grzemich 0:d7bd7384a37c 167 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 168 msg_ptr++;
grzemich 0:d7bd7384a37c 169 }
grzemich 0:d7bd7384a37c 170 *length = *msg_ptr;
grzemich 0:d7bd7384a37c 171 *octets_used = 2;
grzemich 0:d7bd7384a37c 172 return ERR_OK;
grzemich 0:d7bd7384a37c 173 }
grzemich 0:d7bd7384a37c 174 else if (*msg_ptr == 0x82)
grzemich 0:d7bd7384a37c 175 {
grzemich 0:d7bd7384a37c 176 u8_t i;
grzemich 0:d7bd7384a37c 177
grzemich 0:d7bd7384a37c 178 /* constructed definite length format, two octets */
grzemich 0:d7bd7384a37c 179 i = 2;
grzemich 0:d7bd7384a37c 180 while (i > 0)
grzemich 0:d7bd7384a37c 181 {
grzemich 0:d7bd7384a37c 182 i--;
grzemich 0:d7bd7384a37c 183 ofs += 1;
grzemich 0:d7bd7384a37c 184 if (ofs >= plen)
grzemich 0:d7bd7384a37c 185 {
grzemich 0:d7bd7384a37c 186 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 187 p = p->next;
grzemich 0:d7bd7384a37c 188 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 189 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 190 plen += p->len;
grzemich 0:d7bd7384a37c 191 }
grzemich 0:d7bd7384a37c 192 else
grzemich 0:d7bd7384a37c 193 {
grzemich 0:d7bd7384a37c 194 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 195 msg_ptr++;
grzemich 0:d7bd7384a37c 196 }
grzemich 0:d7bd7384a37c 197 if (i == 0)
grzemich 0:d7bd7384a37c 198 {
grzemich 0:d7bd7384a37c 199 /* least significant length octet */
grzemich 0:d7bd7384a37c 200 *length |= *msg_ptr;
grzemich 0:d7bd7384a37c 201 }
grzemich 0:d7bd7384a37c 202 else
grzemich 0:d7bd7384a37c 203 {
grzemich 0:d7bd7384a37c 204 /* most significant length octet */
grzemich 0:d7bd7384a37c 205 *length = (*msg_ptr) << 8;
grzemich 0:d7bd7384a37c 206 }
grzemich 0:d7bd7384a37c 207 }
grzemich 0:d7bd7384a37c 208 *octets_used = 3;
grzemich 0:d7bd7384a37c 209 return ERR_OK;
grzemich 0:d7bd7384a37c 210 }
grzemich 0:d7bd7384a37c 211 else
grzemich 0:d7bd7384a37c 212 {
grzemich 0:d7bd7384a37c 213 /* constructed definite length format 3..127 octets, this is too big (>64k) */
grzemich 0:d7bd7384a37c 214 /** @todo: do we need to accept inefficient codings with many leading zero's? */
grzemich 0:d7bd7384a37c 215 *octets_used = 1 + ((*msg_ptr) & 0x7f);
grzemich 0:d7bd7384a37c 216 return ERR_ARG;
grzemich 0:d7bd7384a37c 217 }
grzemich 0:d7bd7384a37c 218 }
grzemich 0:d7bd7384a37c 219 p = p->next;
grzemich 0:d7bd7384a37c 220 }
grzemich 0:d7bd7384a37c 221
grzemich 0:d7bd7384a37c 222 /* p == NULL, ofs >= plen */
grzemich 0:d7bd7384a37c 223 return ERR_ARG;
grzemich 0:d7bd7384a37c 224 }
grzemich 0:d7bd7384a37c 225
grzemich 0:d7bd7384a37c 226 /**
grzemich 0:d7bd7384a37c 227 * Decodes positive integer (counter, gauge, timeticks) into u32_t.
grzemich 0:d7bd7384a37c 228 *
grzemich 0:d7bd7384a37c 229 * @param p points to a pbuf holding an ASN1 coded integer
grzemich 0:d7bd7384a37c 230 * @param ofs points to the offset within the pbuf chain of the ASN1 coded integer
grzemich 0:d7bd7384a37c 231 * @param len length of the coded integer field
grzemich 0:d7bd7384a37c 232 * @param value return host order integer
grzemich 0:d7bd7384a37c 233 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
grzemich 0:d7bd7384a37c 234 *
grzemich 0:d7bd7384a37c 235 * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
grzemich 0:d7bd7384a37c 236 * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
grzemich 0:d7bd7384a37c 237 * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
grzemich 0:d7bd7384a37c 238 */
grzemich 0:d7bd7384a37c 239 err_t
grzemich 0:d7bd7384a37c 240 snmp_asn1_dec_u32t(struct pbuf *p, u16_t ofs, u16_t len, u32_t *value)
grzemich 0:d7bd7384a37c 241 {
grzemich 0:d7bd7384a37c 242 u16_t plen, base;
grzemich 0:d7bd7384a37c 243 u8_t *msg_ptr;
grzemich 0:d7bd7384a37c 244
grzemich 0:d7bd7384a37c 245 plen = 0;
grzemich 0:d7bd7384a37c 246 while (p != NULL)
grzemich 0:d7bd7384a37c 247 {
grzemich 0:d7bd7384a37c 248 base = plen;
grzemich 0:d7bd7384a37c 249 plen += p->len;
grzemich 0:d7bd7384a37c 250 if (ofs < plen)
grzemich 0:d7bd7384a37c 251 {
grzemich 0:d7bd7384a37c 252 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 253 msg_ptr += ofs - base;
grzemich 0:d7bd7384a37c 254 if ((len > 0) && (len < 6))
grzemich 0:d7bd7384a37c 255 {
grzemich 0:d7bd7384a37c 256 /* start from zero */
grzemich 0:d7bd7384a37c 257 *value = 0;
grzemich 0:d7bd7384a37c 258 if (*msg_ptr & 0x80)
grzemich 0:d7bd7384a37c 259 {
grzemich 0:d7bd7384a37c 260 /* negative, expecting zero sign bit! */
grzemich 0:d7bd7384a37c 261 return ERR_ARG;
grzemich 0:d7bd7384a37c 262 }
grzemich 0:d7bd7384a37c 263 else
grzemich 0:d7bd7384a37c 264 {
grzemich 0:d7bd7384a37c 265 /* positive */
grzemich 0:d7bd7384a37c 266 if ((len > 1) && (*msg_ptr == 0))
grzemich 0:d7bd7384a37c 267 {
grzemich 0:d7bd7384a37c 268 /* skip leading "sign byte" octet 0x00 */
grzemich 0:d7bd7384a37c 269 len--;
grzemich 0:d7bd7384a37c 270 ofs += 1;
grzemich 0:d7bd7384a37c 271 if (ofs >= plen)
grzemich 0:d7bd7384a37c 272 {
grzemich 0:d7bd7384a37c 273 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 274 p = p->next;
grzemich 0:d7bd7384a37c 275 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 276 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 277 plen += p->len;
grzemich 0:d7bd7384a37c 278 }
grzemich 0:d7bd7384a37c 279 else
grzemich 0:d7bd7384a37c 280 {
grzemich 0:d7bd7384a37c 281 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 282 msg_ptr++;
grzemich 0:d7bd7384a37c 283 }
grzemich 0:d7bd7384a37c 284 }
grzemich 0:d7bd7384a37c 285 }
grzemich 0:d7bd7384a37c 286 /* OR octets with value */
grzemich 0:d7bd7384a37c 287 while (len > 1)
grzemich 0:d7bd7384a37c 288 {
grzemich 0:d7bd7384a37c 289 len--;
grzemich 0:d7bd7384a37c 290 *value |= *msg_ptr;
grzemich 0:d7bd7384a37c 291 *value <<= 8;
grzemich 0:d7bd7384a37c 292 ofs += 1;
grzemich 0:d7bd7384a37c 293 if (ofs >= plen)
grzemich 0:d7bd7384a37c 294 {
grzemich 0:d7bd7384a37c 295 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 296 p = p->next;
grzemich 0:d7bd7384a37c 297 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 298 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 299 plen += p->len;
grzemich 0:d7bd7384a37c 300 }
grzemich 0:d7bd7384a37c 301 else
grzemich 0:d7bd7384a37c 302 {
grzemich 0:d7bd7384a37c 303 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 304 msg_ptr++;
grzemich 0:d7bd7384a37c 305 }
grzemich 0:d7bd7384a37c 306 }
grzemich 0:d7bd7384a37c 307 *value |= *msg_ptr;
grzemich 0:d7bd7384a37c 308 return ERR_OK;
grzemich 0:d7bd7384a37c 309 }
grzemich 0:d7bd7384a37c 310 else
grzemich 0:d7bd7384a37c 311 {
grzemich 0:d7bd7384a37c 312 return ERR_ARG;
grzemich 0:d7bd7384a37c 313 }
grzemich 0:d7bd7384a37c 314 }
grzemich 0:d7bd7384a37c 315 p = p->next;
grzemich 0:d7bd7384a37c 316 }
grzemich 0:d7bd7384a37c 317 /* p == NULL, ofs >= plen */
grzemich 0:d7bd7384a37c 318 return ERR_ARG;
grzemich 0:d7bd7384a37c 319 }
grzemich 0:d7bd7384a37c 320
grzemich 0:d7bd7384a37c 321 /**
grzemich 0:d7bd7384a37c 322 * Decodes integer into s32_t.
grzemich 0:d7bd7384a37c 323 *
grzemich 0:d7bd7384a37c 324 * @param p points to a pbuf holding an ASN1 coded integer
grzemich 0:d7bd7384a37c 325 * @param ofs points to the offset within the pbuf chain of the ASN1 coded integer
grzemich 0:d7bd7384a37c 326 * @param len length of the coded integer field
grzemich 0:d7bd7384a37c 327 * @param value return host order integer
grzemich 0:d7bd7384a37c 328 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
grzemich 0:d7bd7384a37c 329 *
grzemich 0:d7bd7384a37c 330 * @note ASN coded integers are _always_ signed!
grzemich 0:d7bd7384a37c 331 */
grzemich 0:d7bd7384a37c 332 err_t
grzemich 0:d7bd7384a37c 333 snmp_asn1_dec_s32t(struct pbuf *p, u16_t ofs, u16_t len, s32_t *value)
grzemich 0:d7bd7384a37c 334 {
grzemich 0:d7bd7384a37c 335 u16_t plen, base;
grzemich 0:d7bd7384a37c 336 u8_t *msg_ptr;
grzemich 0:d7bd7384a37c 337 #if BYTE_ORDER == LITTLE_ENDIAN
grzemich 0:d7bd7384a37c 338 u8_t *lsb_ptr = (u8_t*)value;
grzemich 0:d7bd7384a37c 339 #endif
grzemich 0:d7bd7384a37c 340 #if BYTE_ORDER == BIG_ENDIAN
grzemich 0:d7bd7384a37c 341 u8_t *lsb_ptr = (u8_t*)value + sizeof(s32_t) - 1;
grzemich 0:d7bd7384a37c 342 #endif
grzemich 0:d7bd7384a37c 343 u8_t sign;
grzemich 0:d7bd7384a37c 344
grzemich 0:d7bd7384a37c 345 plen = 0;
grzemich 0:d7bd7384a37c 346 while (p != NULL)
grzemich 0:d7bd7384a37c 347 {
grzemich 0:d7bd7384a37c 348 base = plen;
grzemich 0:d7bd7384a37c 349 plen += p->len;
grzemich 0:d7bd7384a37c 350 if (ofs < plen)
grzemich 0:d7bd7384a37c 351 {
grzemich 0:d7bd7384a37c 352 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 353 msg_ptr += ofs - base;
grzemich 0:d7bd7384a37c 354 if ((len > 0) && (len < 5))
grzemich 0:d7bd7384a37c 355 {
grzemich 0:d7bd7384a37c 356 if (*msg_ptr & 0x80)
grzemich 0:d7bd7384a37c 357 {
grzemich 0:d7bd7384a37c 358 /* negative, start from -1 */
grzemich 0:d7bd7384a37c 359 *value = -1;
grzemich 0:d7bd7384a37c 360 sign = 1;
grzemich 0:d7bd7384a37c 361 }
grzemich 0:d7bd7384a37c 362 else
grzemich 0:d7bd7384a37c 363 {
grzemich 0:d7bd7384a37c 364 /* positive, start from 0 */
grzemich 0:d7bd7384a37c 365 *value = 0;
grzemich 0:d7bd7384a37c 366 sign = 0;
grzemich 0:d7bd7384a37c 367 }
grzemich 0:d7bd7384a37c 368 /* OR/AND octets with value */
grzemich 0:d7bd7384a37c 369 while (len > 1)
grzemich 0:d7bd7384a37c 370 {
grzemich 0:d7bd7384a37c 371 len--;
grzemich 0:d7bd7384a37c 372 if (sign)
grzemich 0:d7bd7384a37c 373 {
grzemich 0:d7bd7384a37c 374 *lsb_ptr &= *msg_ptr;
grzemich 0:d7bd7384a37c 375 *value <<= 8;
grzemich 0:d7bd7384a37c 376 *lsb_ptr |= 255;
grzemich 0:d7bd7384a37c 377 }
grzemich 0:d7bd7384a37c 378 else
grzemich 0:d7bd7384a37c 379 {
grzemich 0:d7bd7384a37c 380 *lsb_ptr |= *msg_ptr;
grzemich 0:d7bd7384a37c 381 *value <<= 8;
grzemich 0:d7bd7384a37c 382 }
grzemich 0:d7bd7384a37c 383 ofs += 1;
grzemich 0:d7bd7384a37c 384 if (ofs >= plen)
grzemich 0:d7bd7384a37c 385 {
grzemich 0:d7bd7384a37c 386 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 387 p = p->next;
grzemich 0:d7bd7384a37c 388 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 389 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 390 plen += p->len;
grzemich 0:d7bd7384a37c 391 }
grzemich 0:d7bd7384a37c 392 else
grzemich 0:d7bd7384a37c 393 {
grzemich 0:d7bd7384a37c 394 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 395 msg_ptr++;
grzemich 0:d7bd7384a37c 396 }
grzemich 0:d7bd7384a37c 397 }
grzemich 0:d7bd7384a37c 398 if (sign)
grzemich 0:d7bd7384a37c 399 {
grzemich 0:d7bd7384a37c 400 *lsb_ptr &= *msg_ptr;
grzemich 0:d7bd7384a37c 401 }
grzemich 0:d7bd7384a37c 402 else
grzemich 0:d7bd7384a37c 403 {
grzemich 0:d7bd7384a37c 404 *lsb_ptr |= *msg_ptr;
grzemich 0:d7bd7384a37c 405 }
grzemich 0:d7bd7384a37c 406 return ERR_OK;
grzemich 0:d7bd7384a37c 407 }
grzemich 0:d7bd7384a37c 408 else
grzemich 0:d7bd7384a37c 409 {
grzemich 0:d7bd7384a37c 410 return ERR_ARG;
grzemich 0:d7bd7384a37c 411 }
grzemich 0:d7bd7384a37c 412 }
grzemich 0:d7bd7384a37c 413 p = p->next;
grzemich 0:d7bd7384a37c 414 }
grzemich 0:d7bd7384a37c 415 /* p == NULL, ofs >= plen */
grzemich 0:d7bd7384a37c 416 return ERR_ARG;
grzemich 0:d7bd7384a37c 417 }
grzemich 0:d7bd7384a37c 418
grzemich 0:d7bd7384a37c 419 /**
grzemich 0:d7bd7384a37c 420 * Decodes object identifier from incoming message into array of s32_t.
grzemich 0:d7bd7384a37c 421 *
grzemich 0:d7bd7384a37c 422 * @param p points to a pbuf holding an ASN1 coded object identifier
grzemich 0:d7bd7384a37c 423 * @param ofs points to the offset within the pbuf chain of the ASN1 coded object identifier
grzemich 0:d7bd7384a37c 424 * @param len length of the coded object identifier
grzemich 0:d7bd7384a37c 425 * @param oid return object identifier struct
grzemich 0:d7bd7384a37c 426 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
grzemich 0:d7bd7384a37c 427 */
grzemich 0:d7bd7384a37c 428 err_t
grzemich 0:d7bd7384a37c 429 snmp_asn1_dec_oid(struct pbuf *p, u16_t ofs, u16_t len, struct snmp_obj_id *oid)
grzemich 0:d7bd7384a37c 430 {
grzemich 0:d7bd7384a37c 431 u16_t plen, base;
grzemich 0:d7bd7384a37c 432 u8_t *msg_ptr;
grzemich 0:d7bd7384a37c 433 s32_t *oid_ptr;
grzemich 0:d7bd7384a37c 434
grzemich 0:d7bd7384a37c 435 plen = 0;
grzemich 0:d7bd7384a37c 436 while (p != NULL)
grzemich 0:d7bd7384a37c 437 {
grzemich 0:d7bd7384a37c 438 base = plen;
grzemich 0:d7bd7384a37c 439 plen += p->len;
grzemich 0:d7bd7384a37c 440 if (ofs < plen)
grzemich 0:d7bd7384a37c 441 {
grzemich 0:d7bd7384a37c 442 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 443 msg_ptr += ofs - base;
grzemich 0:d7bd7384a37c 444
grzemich 0:d7bd7384a37c 445 oid->len = 0;
grzemich 0:d7bd7384a37c 446 oid_ptr = &oid->id[0];
grzemich 0:d7bd7384a37c 447 if (len > 0)
grzemich 0:d7bd7384a37c 448 {
grzemich 0:d7bd7384a37c 449 /* first compressed octet */
grzemich 0:d7bd7384a37c 450 if (*msg_ptr == 0x2B)
grzemich 0:d7bd7384a37c 451 {
grzemich 0:d7bd7384a37c 452 /* (most) common case 1.3 (iso.org) */
grzemich 0:d7bd7384a37c 453 *oid_ptr = 1;
grzemich 0:d7bd7384a37c 454 oid_ptr++;
grzemich 0:d7bd7384a37c 455 *oid_ptr = 3;
grzemich 0:d7bd7384a37c 456 oid_ptr++;
grzemich 0:d7bd7384a37c 457 }
grzemich 0:d7bd7384a37c 458 else if (*msg_ptr < 40)
grzemich 0:d7bd7384a37c 459 {
grzemich 0:d7bd7384a37c 460 *oid_ptr = 0;
grzemich 0:d7bd7384a37c 461 oid_ptr++;
grzemich 0:d7bd7384a37c 462 *oid_ptr = *msg_ptr;
grzemich 0:d7bd7384a37c 463 oid_ptr++;
grzemich 0:d7bd7384a37c 464 }
grzemich 0:d7bd7384a37c 465 else if (*msg_ptr < 80)
grzemich 0:d7bd7384a37c 466 {
grzemich 0:d7bd7384a37c 467 *oid_ptr = 1;
grzemich 0:d7bd7384a37c 468 oid_ptr++;
grzemich 0:d7bd7384a37c 469 *oid_ptr = (*msg_ptr) - 40;
grzemich 0:d7bd7384a37c 470 oid_ptr++;
grzemich 0:d7bd7384a37c 471 }
grzemich 0:d7bd7384a37c 472 else
grzemich 0:d7bd7384a37c 473 {
grzemich 0:d7bd7384a37c 474 *oid_ptr = 2;
grzemich 0:d7bd7384a37c 475 oid_ptr++;
grzemich 0:d7bd7384a37c 476 *oid_ptr = (*msg_ptr) - 80;
grzemich 0:d7bd7384a37c 477 oid_ptr++;
grzemich 0:d7bd7384a37c 478 }
grzemich 0:d7bd7384a37c 479 oid->len = 2;
grzemich 0:d7bd7384a37c 480 }
grzemich 0:d7bd7384a37c 481 else
grzemich 0:d7bd7384a37c 482 {
grzemich 0:d7bd7384a37c 483 /* accepting zero length identifiers e.g. for
grzemich 0:d7bd7384a37c 484 getnext operation. uncommon but valid */
grzemich 0:d7bd7384a37c 485 return ERR_OK;
grzemich 0:d7bd7384a37c 486 }
grzemich 0:d7bd7384a37c 487 len--;
grzemich 0:d7bd7384a37c 488 if (len > 0)
grzemich 0:d7bd7384a37c 489 {
grzemich 0:d7bd7384a37c 490 ofs += 1;
grzemich 0:d7bd7384a37c 491 if (ofs >= plen)
grzemich 0:d7bd7384a37c 492 {
grzemich 0:d7bd7384a37c 493 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 494 p = p->next;
grzemich 0:d7bd7384a37c 495 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 496 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 497 plen += p->len;
grzemich 0:d7bd7384a37c 498 }
grzemich 0:d7bd7384a37c 499 else
grzemich 0:d7bd7384a37c 500 {
grzemich 0:d7bd7384a37c 501 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 502 msg_ptr++;
grzemich 0:d7bd7384a37c 503 }
grzemich 0:d7bd7384a37c 504 }
grzemich 0:d7bd7384a37c 505 while ((len > 0) && (oid->len < LWIP_SNMP_OBJ_ID_LEN))
grzemich 0:d7bd7384a37c 506 {
grzemich 0:d7bd7384a37c 507 /* sub-identifier uses multiple octets */
grzemich 0:d7bd7384a37c 508 if (*msg_ptr & 0x80)
grzemich 0:d7bd7384a37c 509 {
grzemich 0:d7bd7384a37c 510 s32_t sub_id = 0;
grzemich 0:d7bd7384a37c 511
grzemich 0:d7bd7384a37c 512 while ((*msg_ptr & 0x80) && (len > 1))
grzemich 0:d7bd7384a37c 513 {
grzemich 0:d7bd7384a37c 514 len--;
grzemich 0:d7bd7384a37c 515 sub_id = (sub_id << 7) + (*msg_ptr & ~0x80);
grzemich 0:d7bd7384a37c 516 ofs += 1;
grzemich 0:d7bd7384a37c 517 if (ofs >= plen)
grzemich 0:d7bd7384a37c 518 {
grzemich 0:d7bd7384a37c 519 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 520 p = p->next;
grzemich 0:d7bd7384a37c 521 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 522 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 523 plen += p->len;
grzemich 0:d7bd7384a37c 524 }
grzemich 0:d7bd7384a37c 525 else
grzemich 0:d7bd7384a37c 526 {
grzemich 0:d7bd7384a37c 527 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 528 msg_ptr++;
grzemich 0:d7bd7384a37c 529 }
grzemich 0:d7bd7384a37c 530 }
grzemich 0:d7bd7384a37c 531 if (!(*msg_ptr & 0x80) && (len > 0))
grzemich 0:d7bd7384a37c 532 {
grzemich 0:d7bd7384a37c 533 /* last octet sub-identifier */
grzemich 0:d7bd7384a37c 534 len--;
grzemich 0:d7bd7384a37c 535 sub_id = (sub_id << 7) + *msg_ptr;
grzemich 0:d7bd7384a37c 536 *oid_ptr = sub_id;
grzemich 0:d7bd7384a37c 537 }
grzemich 0:d7bd7384a37c 538 }
grzemich 0:d7bd7384a37c 539 else
grzemich 0:d7bd7384a37c 540 {
grzemich 0:d7bd7384a37c 541 /* !(*msg_ptr & 0x80) sub-identifier uses single octet */
grzemich 0:d7bd7384a37c 542 len--;
grzemich 0:d7bd7384a37c 543 *oid_ptr = *msg_ptr;
grzemich 0:d7bd7384a37c 544 }
grzemich 0:d7bd7384a37c 545 if (len > 0)
grzemich 0:d7bd7384a37c 546 {
grzemich 0:d7bd7384a37c 547 /* remaining oid bytes available ... */
grzemich 0:d7bd7384a37c 548 ofs += 1;
grzemich 0:d7bd7384a37c 549 if (ofs >= plen)
grzemich 0:d7bd7384a37c 550 {
grzemich 0:d7bd7384a37c 551 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 552 p = p->next;
grzemich 0:d7bd7384a37c 553 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 554 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 555 plen += p->len;
grzemich 0:d7bd7384a37c 556 }
grzemich 0:d7bd7384a37c 557 else
grzemich 0:d7bd7384a37c 558 {
grzemich 0:d7bd7384a37c 559 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 560 msg_ptr++;
grzemich 0:d7bd7384a37c 561 }
grzemich 0:d7bd7384a37c 562 }
grzemich 0:d7bd7384a37c 563 oid_ptr++;
grzemich 0:d7bd7384a37c 564 oid->len++;
grzemich 0:d7bd7384a37c 565 }
grzemich 0:d7bd7384a37c 566 if (len == 0)
grzemich 0:d7bd7384a37c 567 {
grzemich 0:d7bd7384a37c 568 /* len == 0, end of oid */
grzemich 0:d7bd7384a37c 569 return ERR_OK;
grzemich 0:d7bd7384a37c 570 }
grzemich 0:d7bd7384a37c 571 else
grzemich 0:d7bd7384a37c 572 {
grzemich 0:d7bd7384a37c 573 /* len > 0, oid->len == LWIP_SNMP_OBJ_ID_LEN or malformed encoding */
grzemich 0:d7bd7384a37c 574 return ERR_ARG;
grzemich 0:d7bd7384a37c 575 }
grzemich 0:d7bd7384a37c 576
grzemich 0:d7bd7384a37c 577 }
grzemich 0:d7bd7384a37c 578 p = p->next;
grzemich 0:d7bd7384a37c 579 }
grzemich 0:d7bd7384a37c 580 /* p == NULL, ofs >= plen */
grzemich 0:d7bd7384a37c 581 return ERR_ARG;
grzemich 0:d7bd7384a37c 582 }
grzemich 0:d7bd7384a37c 583
grzemich 0:d7bd7384a37c 584 /**
grzemich 0:d7bd7384a37c 585 * Decodes (copies) raw data (ip-addresses, octet strings, opaque encoding)
grzemich 0:d7bd7384a37c 586 * from incoming message into array.
grzemich 0:d7bd7384a37c 587 *
grzemich 0:d7bd7384a37c 588 * @param p points to a pbuf holding an ASN1 coded raw data
grzemich 0:d7bd7384a37c 589 * @param ofs points to the offset within the pbuf chain of the ASN1 coded raw data
grzemich 0:d7bd7384a37c 590 * @param len length of the coded raw data (zero is valid, e.g. empty string!)
grzemich 0:d7bd7384a37c 591 * @param raw_len length of the raw return value
grzemich 0:d7bd7384a37c 592 * @param raw return raw bytes
grzemich 0:d7bd7384a37c 593 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
grzemich 0:d7bd7384a37c 594 */
grzemich 0:d7bd7384a37c 595 err_t
grzemich 0:d7bd7384a37c 596 snmp_asn1_dec_raw(struct pbuf *p, u16_t ofs, u16_t len, u16_t raw_len, u8_t *raw)
grzemich 0:d7bd7384a37c 597 {
grzemich 0:d7bd7384a37c 598 u16_t plen, base;
grzemich 0:d7bd7384a37c 599 u8_t *msg_ptr;
grzemich 0:d7bd7384a37c 600
grzemich 0:d7bd7384a37c 601 if (len > 0)
grzemich 0:d7bd7384a37c 602 {
grzemich 0:d7bd7384a37c 603 plen = 0;
grzemich 0:d7bd7384a37c 604 while (p != NULL)
grzemich 0:d7bd7384a37c 605 {
grzemich 0:d7bd7384a37c 606 base = plen;
grzemich 0:d7bd7384a37c 607 plen += p->len;
grzemich 0:d7bd7384a37c 608 if (ofs < plen)
grzemich 0:d7bd7384a37c 609 {
grzemich 0:d7bd7384a37c 610 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 611 msg_ptr += ofs - base;
grzemich 0:d7bd7384a37c 612 if (raw_len >= len)
grzemich 0:d7bd7384a37c 613 {
grzemich 0:d7bd7384a37c 614 while (len > 1)
grzemich 0:d7bd7384a37c 615 {
grzemich 0:d7bd7384a37c 616 /* copy len - 1 octets */
grzemich 0:d7bd7384a37c 617 len--;
grzemich 0:d7bd7384a37c 618 *raw = *msg_ptr;
grzemich 0:d7bd7384a37c 619 raw++;
grzemich 0:d7bd7384a37c 620 ofs += 1;
grzemich 0:d7bd7384a37c 621 if (ofs >= plen)
grzemich 0:d7bd7384a37c 622 {
grzemich 0:d7bd7384a37c 623 /* next octet in next pbuf */
grzemich 0:d7bd7384a37c 624 p = p->next;
grzemich 0:d7bd7384a37c 625 if (p == NULL) { return ERR_ARG; }
grzemich 0:d7bd7384a37c 626 msg_ptr = (u8_t*)p->payload;
grzemich 0:d7bd7384a37c 627 plen += p->len;
grzemich 0:d7bd7384a37c 628 }
grzemich 0:d7bd7384a37c 629 else
grzemich 0:d7bd7384a37c 630 {
grzemich 0:d7bd7384a37c 631 /* next octet in same pbuf */
grzemich 0:d7bd7384a37c 632 msg_ptr++;
grzemich 0:d7bd7384a37c 633 }
grzemich 0:d7bd7384a37c 634 }
grzemich 0:d7bd7384a37c 635 /* copy last octet */
grzemich 0:d7bd7384a37c 636 *raw = *msg_ptr;
grzemich 0:d7bd7384a37c 637 return ERR_OK;
grzemich 0:d7bd7384a37c 638 }
grzemich 0:d7bd7384a37c 639 else
grzemich 0:d7bd7384a37c 640 {
grzemich 0:d7bd7384a37c 641 /* raw_len < len, not enough dst space */
grzemich 0:d7bd7384a37c 642 return ERR_ARG;
grzemich 0:d7bd7384a37c 643 }
grzemich 0:d7bd7384a37c 644 }
grzemich 0:d7bd7384a37c 645 p = p->next;
grzemich 0:d7bd7384a37c 646 }
grzemich 0:d7bd7384a37c 647 /* p == NULL, ofs >= plen */
grzemich 0:d7bd7384a37c 648 return ERR_ARG;
grzemich 0:d7bd7384a37c 649 }
grzemich 0:d7bd7384a37c 650 else
grzemich 0:d7bd7384a37c 651 {
grzemich 0:d7bd7384a37c 652 /* len == 0, empty string */
grzemich 0:d7bd7384a37c 653 return ERR_OK;
grzemich 0:d7bd7384a37c 654 }
grzemich 0:d7bd7384a37c 655 }
grzemich 0:d7bd7384a37c 656
grzemich 0:d7bd7384a37c 657 #endif /* LWIP_SNMP */