Arlindo Montagnoli / Mbed 2 deprecated ProjetoBB

ProjetoBB

Dependencies:   F7_Ethernet WebSocketClient mbed mcp3008

Fork of Nucleo_F746ZG_Ethernet by Dieter Graef

F7_Ethernet/lwip/core/snmp/asn1_dec.c@0:f9b6112278fe, 2016-06-18 (annotated)

Committer:
DieterGraef
Date:
Sat Jun 18 10:49:12 2016 +0000
Revision:
0:f9b6112278fe
Ethernet for the NUCLEO STM32F746 Board Testprogram uses DHCP and NTP to set the clock

Who changed what in which revision?

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