Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "lwip/opt.h"
00038
00039 #if LWIP_SNMP
00040
00041 #include "lwip/snmp_asn1.h"
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 err_t
00052 snmp_asn1_dec_type(struct pbuf *p, u16_t ofs, u8_t *type)
00053 {
00054 u16_t plen, base;
00055 u8_t *msg_ptr;
00056
00057 plen = 0;
00058 while (p != NULL)
00059 {
00060 base = plen;
00061 plen += p->len;
00062 if (ofs < plen)
00063 {
00064 msg_ptr = (u8_t*)p->payload;
00065 msg_ptr += ofs - base;
00066 *type = *msg_ptr;
00067 return ERR_OK;
00068 }
00069 p = p->next;
00070 }
00071
00072 return ERR_ARG;
00073 }
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084 err_t
00085 snmp_asn1_dec_length(struct pbuf *p, u16_t ofs, u8_t *octets_used, u16_t *length)
00086 {
00087 u16_t plen, base;
00088 u8_t *msg_ptr;
00089
00090 plen = 0;
00091 while (p != NULL)
00092 {
00093 base = plen;
00094 plen += p->len;
00095 if (ofs < plen)
00096 {
00097 msg_ptr = (u8_t*)p->payload;
00098 msg_ptr += ofs - base;
00099
00100 if (*msg_ptr < 0x80)
00101 {
00102
00103 *octets_used = 1;
00104 *length = *msg_ptr;
00105 return ERR_OK;
00106 }
00107 else if (*msg_ptr == 0x80)
00108 {
00109
00110 u8_t zeros;
00111 u8_t i;
00112
00113 *length = 0;
00114 zeros = 0;
00115 while (zeros != 2)
00116 {
00117 i = 2;
00118 while (i > 0)
00119 {
00120 i--;
00121 (*length) += 1;
00122 ofs += 1;
00123 if (ofs >= plen)
00124 {
00125
00126 p = p->next;
00127 if (p == NULL) { return ERR_ARG; }
00128 msg_ptr = (u8_t*)p->payload;
00129 plen += p->len;
00130 }
00131 else
00132 {
00133
00134 msg_ptr++;
00135 }
00136 if (*msg_ptr == 0)
00137 {
00138 zeros++;
00139 if (zeros == 2)
00140 {
00141
00142 i = 0;
00143 }
00144 }
00145 else
00146 {
00147 zeros = 0;
00148 }
00149 }
00150 }
00151 *octets_used = 1;
00152 return ERR_OK;
00153 }
00154 else if (*msg_ptr == 0x81)
00155 {
00156
00157 ofs += 1;
00158 if (ofs >= plen)
00159 {
00160
00161 p = p->next;
00162 if (p == NULL) { return ERR_ARG; }
00163 msg_ptr = (u8_t*)p->payload;
00164 }
00165 else
00166 {
00167
00168 msg_ptr++;
00169 }
00170 *length = *msg_ptr;
00171 *octets_used = 2;
00172 return ERR_OK;
00173 }
00174 else if (*msg_ptr == 0x82)
00175 {
00176 u8_t i;
00177
00178
00179 i = 2;
00180 while (i > 0)
00181 {
00182 i--;
00183 ofs += 1;
00184 if (ofs >= plen)
00185 {
00186
00187 p = p->next;
00188 if (p == NULL) { return ERR_ARG; }
00189 msg_ptr = (u8_t*)p->payload;
00190 plen += p->len;
00191 }
00192 else
00193 {
00194
00195 msg_ptr++;
00196 }
00197 if (i == 0)
00198 {
00199
00200 *length |= *msg_ptr;
00201 }
00202 else
00203 {
00204
00205 *length = (*msg_ptr) << 8;
00206 }
00207 }
00208 *octets_used = 3;
00209 return ERR_OK;
00210 }
00211 else
00212 {
00213
00214
00215 *octets_used = 1 + ((*msg_ptr) & 0x7f);
00216 return ERR_ARG;
00217 }
00218 }
00219 p = p->next;
00220 }
00221
00222
00223 return ERR_ARG;
00224 }
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239 err_t
00240 snmp_asn1_dec_u32t(struct pbuf *p, u16_t ofs, u16_t len, u32_t *value)
00241 {
00242 u16_t plen, base;
00243 u8_t *msg_ptr;
00244
00245 plen = 0;
00246 while (p != NULL)
00247 {
00248 base = plen;
00249 plen += p->len;
00250 if (ofs < plen)
00251 {
00252 msg_ptr = (u8_t*)p->payload;
00253 msg_ptr += ofs - base;
00254 if ((len > 0) && (len < 6))
00255 {
00256
00257 *value = 0;
00258 if (*msg_ptr & 0x80)
00259 {
00260
00261 return ERR_ARG;
00262 }
00263 else
00264 {
00265
00266 if ((len > 1) && (*msg_ptr == 0))
00267 {
00268
00269 len--;
00270 ofs += 1;
00271 if (ofs >= plen)
00272 {
00273
00274 p = p->next;
00275 if (p == NULL) { return ERR_ARG; }
00276 msg_ptr = (u8_t*)p->payload;
00277 plen += p->len;
00278 }
00279 else
00280 {
00281
00282 msg_ptr++;
00283 }
00284 }
00285 }
00286
00287 while (len > 1)
00288 {
00289 len--;
00290 *value |= *msg_ptr;
00291 *value <<= 8;
00292 ofs += 1;
00293 if (ofs >= plen)
00294 {
00295
00296 p = p->next;
00297 if (p == NULL) { return ERR_ARG; }
00298 msg_ptr = (u8_t*)p->payload;
00299 plen += p->len;
00300 }
00301 else
00302 {
00303
00304 msg_ptr++;
00305 }
00306 }
00307 *value |= *msg_ptr;
00308 return ERR_OK;
00309 }
00310 else
00311 {
00312 return ERR_ARG;
00313 }
00314 }
00315 p = p->next;
00316 }
00317
00318 return ERR_ARG;
00319 }
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332 err_t
00333 snmp_asn1_dec_s32t(struct pbuf *p, u16_t ofs, u16_t len, s32_t *value)
00334 {
00335 u16_t plen, base;
00336 u8_t *msg_ptr;
00337 #if BYTE_ORDER == LITTLE_ENDIAN
00338 u8_t *lsb_ptr = (u8_t*)value;
00339 #endif
00340 #if BYTE_ORDER == BIG_ENDIAN
00341 u8_t *lsb_ptr = (u8_t*)value + sizeof(s32_t) - 1;
00342 #endif
00343 u8_t sign;
00344
00345 plen = 0;
00346 while (p != NULL)
00347 {
00348 base = plen;
00349 plen += p->len;
00350 if (ofs < plen)
00351 {
00352 msg_ptr = (u8_t*)p->payload;
00353 msg_ptr += ofs - base;
00354 if ((len > 0) && (len < 5))
00355 {
00356 if (*msg_ptr & 0x80)
00357 {
00358
00359 *value = -1;
00360 sign = 1;
00361 }
00362 else
00363 {
00364
00365 *value = 0;
00366 sign = 0;
00367 }
00368
00369 while (len > 1)
00370 {
00371 len--;
00372 if (sign)
00373 {
00374 *lsb_ptr &= *msg_ptr;
00375 *value <<= 8;
00376 *lsb_ptr |= 255;
00377 }
00378 else
00379 {
00380 *lsb_ptr |= *msg_ptr;
00381 *value <<= 8;
00382 }
00383 ofs += 1;
00384 if (ofs >= plen)
00385 {
00386
00387 p = p->next;
00388 if (p == NULL) { return ERR_ARG; }
00389 msg_ptr = (u8_t*)p->payload;
00390 plen += p->len;
00391 }
00392 else
00393 {
00394
00395 msg_ptr++;
00396 }
00397 }
00398 if (sign)
00399 {
00400 *lsb_ptr &= *msg_ptr;
00401 }
00402 else
00403 {
00404 *lsb_ptr |= *msg_ptr;
00405 }
00406 return ERR_OK;
00407 }
00408 else
00409 {
00410 return ERR_ARG;
00411 }
00412 }
00413 p = p->next;
00414 }
00415
00416 return ERR_ARG;
00417 }
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428 err_t
00429 snmp_asn1_dec_oid(struct pbuf *p, u16_t ofs, u16_t len, struct snmp_obj_id *oid)
00430 {
00431 u16_t plen, base;
00432 u8_t *msg_ptr;
00433 s32_t *oid_ptr;
00434
00435 plen = 0;
00436 while (p != NULL)
00437 {
00438 base = plen;
00439 plen += p->len;
00440 if (ofs < plen)
00441 {
00442 msg_ptr = (u8_t*)p->payload;
00443 msg_ptr += ofs - base;
00444
00445 oid->len = 0;
00446 oid_ptr = &oid->id[0];
00447 if (len > 0)
00448 {
00449
00450 if (*msg_ptr == 0x2B)
00451 {
00452
00453 *oid_ptr = 1;
00454 oid_ptr++;
00455 *oid_ptr = 3;
00456 oid_ptr++;
00457 }
00458 else if (*msg_ptr < 40)
00459 {
00460 *oid_ptr = 0;
00461 oid_ptr++;
00462 *oid_ptr = *msg_ptr;
00463 oid_ptr++;
00464 }
00465 else if (*msg_ptr < 80)
00466 {
00467 *oid_ptr = 1;
00468 oid_ptr++;
00469 *oid_ptr = (*msg_ptr) - 40;
00470 oid_ptr++;
00471 }
00472 else
00473 {
00474 *oid_ptr = 2;
00475 oid_ptr++;
00476 *oid_ptr = (*msg_ptr) - 80;
00477 oid_ptr++;
00478 }
00479 oid->len = 2;
00480 }
00481 else
00482 {
00483
00484
00485 return ERR_OK;
00486 }
00487 len--;
00488 if (len > 0)
00489 {
00490 ofs += 1;
00491 if (ofs >= plen)
00492 {
00493
00494 p = p->next;
00495 if (p == NULL) { return ERR_ARG; }
00496 msg_ptr = (u8_t*)p->payload;
00497 plen += p->len;
00498 }
00499 else
00500 {
00501
00502 msg_ptr++;
00503 }
00504 }
00505 while ((len > 0) && (oid->len < LWIP_SNMP_OBJ_ID_LEN))
00506 {
00507
00508 if (*msg_ptr & 0x80)
00509 {
00510 s32_t sub_id = 0;
00511
00512 while ((*msg_ptr & 0x80) && (len > 1))
00513 {
00514 len--;
00515 sub_id = (sub_id << 7) + (*msg_ptr & ~0x80);
00516 ofs += 1;
00517 if (ofs >= plen)
00518 {
00519
00520 p = p->next;
00521 if (p == NULL) { return ERR_ARG; }
00522 msg_ptr = (u8_t*)p->payload;
00523 plen += p->len;
00524 }
00525 else
00526 {
00527
00528 msg_ptr++;
00529 }
00530 }
00531 if (!(*msg_ptr & 0x80) && (len > 0))
00532 {
00533
00534 len--;
00535 sub_id = (sub_id << 7) + *msg_ptr;
00536 *oid_ptr = sub_id;
00537 }
00538 }
00539 else
00540 {
00541
00542 len--;
00543 *oid_ptr = *msg_ptr;
00544 }
00545 if (len > 0)
00546 {
00547
00548 ofs += 1;
00549 if (ofs >= plen)
00550 {
00551
00552 p = p->next;
00553 if (p == NULL) { return ERR_ARG; }
00554 msg_ptr = (u8_t*)p->payload;
00555 plen += p->len;
00556 }
00557 else
00558 {
00559
00560 msg_ptr++;
00561 }
00562 }
00563 oid_ptr++;
00564 oid->len++;
00565 }
00566 if (len == 0)
00567 {
00568
00569 return ERR_OK;
00570 }
00571 else
00572 {
00573
00574 return ERR_ARG;
00575 }
00576
00577 }
00578 p = p->next;
00579 }
00580
00581 return ERR_ARG;
00582 }
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595 err_t
00596 snmp_asn1_dec_raw(struct pbuf *p, u16_t ofs, u16_t len, u16_t raw_len, u8_t *raw)
00597 {
00598 u16_t plen, base;
00599 u8_t *msg_ptr;
00600
00601 if (len > 0)
00602 {
00603 plen = 0;
00604 while (p != NULL)
00605 {
00606 base = plen;
00607 plen += p->len;
00608 if (ofs < plen)
00609 {
00610 msg_ptr = (u8_t*)p->payload;
00611 msg_ptr += ofs - base;
00612 if (raw_len >= len)
00613 {
00614 while (len > 1)
00615 {
00616
00617 len--;
00618 *raw = *msg_ptr;
00619 raw++;
00620 ofs += 1;
00621 if (ofs >= plen)
00622 {
00623
00624 p = p->next;
00625 if (p == NULL) { return ERR_ARG; }
00626 msg_ptr = (u8_t*)p->payload;
00627 plen += p->len;
00628 }
00629 else
00630 {
00631
00632 msg_ptr++;
00633 }
00634 }
00635
00636 *raw = *msg_ptr;
00637 return ERR_OK;
00638 }
00639 else
00640 {
00641
00642 return ERR_ARG;
00643 }
00644 }
00645 p = p->next;
00646 }
00647
00648 return ERR_ARG;
00649 }
00650 else
00651 {
00652
00653 return ERR_OK;
00654 }
00655 }
00656
00657 #endif