Simon Ford / NetServices

Version of http://mbed.org/cookbook/NetServicesTribute with setting set the same for LPC2368

Dependents:   UDPSocketExample 24LCxx_I2CApp WeatherPlatform_pachube HvZServerLib ... more

lwip/core/snmp/asn1_enc.c@0:350011bf8be7, 2010-11-23 (annotated)

Committer:
simon
Date:
Tue Nov 23 14:15:36 2010 +0000
Revision:
0:350011bf8be7
Experimental version for testing UDP

Who changed what in which revision?

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