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 void
00050 snmp_asn1_enc_length_cnt(u16_t length, u8_t *octets_needed)
00051 {
00052 if (length < 0x80U)
00053 {
00054 *octets_needed = 1;
00055 }
00056 else if (length < 0x100U)
00057 {
00058 *octets_needed = 2;
00059 }
00060 else
00061 {
00062 *octets_needed = 3;
00063 }
00064 }
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076 void
00077 snmp_asn1_enc_u32t_cnt(u32_t value, u16_t *octets_needed)
00078 {
00079 if (value < 0x80UL)
00080 {
00081 *octets_needed = 1;
00082 }
00083 else if (value < 0x8000UL)
00084 {
00085 *octets_needed = 2;
00086 }
00087 else if (value < 0x800000UL)
00088 {
00089 *octets_needed = 3;
00090 }
00091 else if (value < 0x80000000UL)
00092 {
00093 *octets_needed = 4;
00094 }
00095 else
00096 {
00097 *octets_needed = 5;
00098 }
00099 }
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109 void
00110 snmp_asn1_enc_s32t_cnt(s32_t value, u16_t *octets_needed)
00111 {
00112 if (value < 0)
00113 {
00114 value = ~value;
00115 }
00116 if (value < 0x80L)
00117 {
00118 *octets_needed = 1;
00119 }
00120 else if (value < 0x8000L)
00121 {
00122 *octets_needed = 2;
00123 }
00124 else if (value < 0x800000L)
00125 {
00126 *octets_needed = 3;
00127 }
00128 else
00129 {
00130 *octets_needed = 4;
00131 }
00132 }
00133
00134
00135
00136
00137
00138
00139
00140
00141 void
00142 snmp_asn1_enc_oid_cnt(u8_t ident_len, s32_t *ident, u16_t *octets_needed)
00143 {
00144 s32_t sub_id;
00145 u8_t cnt;
00146
00147 cnt = 0;
00148 if (ident_len > 1)
00149 {
00150
00151 cnt++;
00152 ident_len -= 2;
00153 ident += 2;
00154 }
00155 while(ident_len > 0)
00156 {
00157 ident_len--;
00158 sub_id = *ident;
00159
00160 sub_id >>= 7;
00161 cnt++;
00162 while(sub_id > 0)
00163 {
00164 sub_id >>= 7;
00165 cnt++;
00166 }
00167 ident++;
00168 }
00169 *octets_needed = cnt;
00170 }
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180 err_t
00181 snmp_asn1_enc_type(struct pbuf *p, u16_t ofs, u8_t type)
00182 {
00183 u16_t plen, base;
00184 u8_t *msg_ptr;
00185
00186 plen = 0;
00187 while (p != NULL)
00188 {
00189 base = plen;
00190 plen += p->len;
00191 if (ofs < plen)
00192 {
00193 msg_ptr = (u8_t*)p->payload;
00194 msg_ptr += ofs - base;
00195 *msg_ptr = type;
00196 return ERR_OK;
00197 }
00198 p = p->next;
00199 }
00200
00201 return ERR_ARG;
00202 }
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212 err_t
00213 snmp_asn1_enc_length(struct pbuf *p, u16_t ofs, u16_t length)
00214 {
00215 u16_t plen, base;
00216 u8_t *msg_ptr;
00217
00218 plen = 0;
00219 while (p != NULL)
00220 {
00221 base = plen;
00222 plen += p->len;
00223 if (ofs < plen)
00224 {
00225 msg_ptr = (u8_t*)p->payload;
00226 msg_ptr += ofs - base;
00227
00228 if (length < 0x80)
00229 {
00230 *msg_ptr = (u8_t)length;
00231 return ERR_OK;
00232 }
00233 else if (length < 0x100)
00234 {
00235 *msg_ptr = 0x81;
00236 ofs += 1;
00237 if (ofs >= plen)
00238 {
00239
00240 p = p->next;
00241 if (p == NULL) { return ERR_ARG; }
00242 msg_ptr = (u8_t*)p->payload;
00243 }
00244 else
00245 {
00246
00247 msg_ptr++;
00248 }
00249 *msg_ptr = (u8_t)length;
00250 return ERR_OK;
00251 }
00252 else
00253 {
00254 u8_t i;
00255
00256
00257 *msg_ptr = 0x82;
00258 i = 2;
00259 while (i > 0)
00260 {
00261 i--;
00262 ofs += 1;
00263 if (ofs >= plen)
00264 {
00265
00266 p = p->next;
00267 if (p == NULL) { return ERR_ARG; }
00268 msg_ptr = (u8_t*)p->payload;
00269 plen += p->len;
00270 }
00271 else
00272 {
00273
00274 msg_ptr++;
00275 }
00276 if (i == 0)
00277 {
00278
00279 *msg_ptr = (u8_t)length;
00280 }
00281 else
00282 {
00283
00284 *msg_ptr = (u8_t)(length >> 8);
00285 }
00286 }
00287 return ERR_OK;
00288 }
00289 }
00290 p = p->next;
00291 }
00292
00293 return ERR_ARG;
00294 }
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307 err_t
00308 snmp_asn1_enc_u32t(struct pbuf *p, u16_t ofs, u16_t octets_needed, u32_t value)
00309 {
00310 u16_t plen, base;
00311 u8_t *msg_ptr;
00312
00313 plen = 0;
00314 while (p != NULL)
00315 {
00316 base = plen;
00317 plen += p->len;
00318 if (ofs < plen)
00319 {
00320 msg_ptr = (u8_t*)p->payload;
00321 msg_ptr += ofs - base;
00322
00323 if (octets_needed == 5)
00324 {
00325
00326 octets_needed--;
00327 *msg_ptr = 0x00;
00328 ofs += 1;
00329 if (ofs >= plen)
00330 {
00331
00332 p = p->next;
00333 if (p == NULL) { return ERR_ARG; }
00334 msg_ptr = (u8_t*)p->payload;
00335 plen += p->len;
00336 }
00337 else
00338 {
00339
00340 msg_ptr++;
00341 }
00342 }
00343 while (octets_needed > 1)
00344 {
00345 octets_needed--;
00346 *msg_ptr = (u8_t)(value >> (octets_needed << 3));
00347 ofs += 1;
00348 if (ofs >= plen)
00349 {
00350
00351 p = p->next;
00352 if (p == NULL) { return ERR_ARG; }
00353 msg_ptr = (u8_t*)p->payload;
00354 plen += p->len;
00355 }
00356 else
00357 {
00358
00359 msg_ptr++;
00360 }
00361 }
00362
00363 *msg_ptr = (u8_t)value;
00364 return ERR_OK;
00365 }
00366 p = p->next;
00367 }
00368
00369 return ERR_ARG;
00370 }
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383 err_t
00384 snmp_asn1_enc_s32t(struct pbuf *p, u16_t ofs, u16_t octets_needed, s32_t value)
00385 {
00386 u16_t plen, base;
00387 u8_t *msg_ptr;
00388
00389 plen = 0;
00390 while (p != NULL)
00391 {
00392 base = plen;
00393 plen += p->len;
00394 if (ofs < plen)
00395 {
00396 msg_ptr = (u8_t*)p->payload;
00397 msg_ptr += ofs - base;
00398
00399 while (octets_needed > 1)
00400 {
00401 octets_needed--;
00402 *msg_ptr = (u8_t)(value >> (octets_needed << 3));
00403 ofs += 1;
00404 if (ofs >= plen)
00405 {
00406
00407 p = p->next;
00408 if (p == NULL) { return ERR_ARG; }
00409 msg_ptr = (u8_t*)p->payload;
00410 plen += p->len;
00411 }
00412 else
00413 {
00414
00415 msg_ptr++;
00416 }
00417 }
00418
00419 *msg_ptr = (u8_t)value;
00420 return ERR_OK;
00421 }
00422 p = p->next;
00423 }
00424
00425 return ERR_ARG;
00426 }
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437 err_t
00438 snmp_asn1_enc_oid(struct pbuf *p, u16_t ofs, u8_t ident_len, s32_t *ident)
00439 {
00440 u16_t plen, base;
00441 u8_t *msg_ptr;
00442
00443 plen = 0;
00444 while (p != NULL)
00445 {
00446 base = plen;
00447 plen += p->len;
00448 if (ofs < plen)
00449 {
00450 msg_ptr = (u8_t*)p->payload;
00451 msg_ptr += ofs - base;
00452
00453 if (ident_len > 1)
00454 {
00455 if ((ident[0] == 1) && (ident[1] == 3))
00456 {
00457
00458 *msg_ptr = 0x2b;
00459 }
00460 else
00461 {
00462
00463 *msg_ptr = (u8_t)((ident[0] * 40) + ident[1]);
00464 }
00465 ofs += 1;
00466 if (ofs >= plen)
00467 {
00468
00469 p = p->next;
00470 if (p == NULL) { return ERR_ARG; }
00471 msg_ptr = (u8_t*)p->payload;
00472 plen += p->len;
00473 }
00474 else
00475 {
00476
00477 msg_ptr++;
00478 }
00479 ident_len -= 2;
00480 ident += 2;
00481 }
00482 else
00483 {
00484
00485
00486 return ERR_ARG;
00487 }
00488 while (ident_len > 0)
00489 {
00490 s32_t sub_id;
00491 u8_t shift, tail;
00492
00493 ident_len--;
00494 sub_id = *ident;
00495 tail = 0;
00496 shift = 28;
00497 while(shift > 0)
00498 {
00499 u8_t code;
00500
00501 code = (u8_t)(sub_id >> shift);
00502 if ((code != 0) || (tail != 0))
00503 {
00504 tail = 1;
00505 *msg_ptr = code | 0x80;
00506 ofs += 1;
00507 if (ofs >= plen)
00508 {
00509
00510 p = p->next;
00511 if (p == NULL) { return ERR_ARG; }
00512 msg_ptr = (u8_t*)p->payload;
00513 plen += p->len;
00514 }
00515 else
00516 {
00517
00518 msg_ptr++;
00519 }
00520 }
00521 shift -= 7;
00522 }
00523 *msg_ptr = (u8_t)sub_id & 0x7F;
00524 if (ident_len > 0)
00525 {
00526 ofs += 1;
00527 if (ofs >= plen)
00528 {
00529
00530 p = p->next;
00531 if (p == NULL) { return ERR_ARG; }
00532 msg_ptr = (u8_t*)p->payload;
00533 plen += p->len;
00534 }
00535 else
00536 {
00537
00538 msg_ptr++;
00539 }
00540 }
00541
00542 ident++;
00543 }
00544 return ERR_OK;
00545 }
00546 p = p->next;
00547 }
00548
00549 return ERR_ARG;
00550 }
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561 err_t
00562 snmp_asn1_enc_raw(struct pbuf *p, u16_t ofs, u16_t raw_len, u8_t *raw)
00563 {
00564 u16_t plen, base;
00565 u8_t *msg_ptr;
00566
00567 plen = 0;
00568 while (p != NULL)
00569 {
00570 base = plen;
00571 plen += p->len;
00572 if (ofs < plen)
00573 {
00574 msg_ptr = (u8_t*)p->payload;
00575 msg_ptr += ofs - base;
00576
00577 while (raw_len > 1)
00578 {
00579
00580 raw_len--;
00581 *msg_ptr = *raw;
00582 raw++;
00583 ofs += 1;
00584 if (ofs >= plen)
00585 {
00586
00587 p = p->next;
00588 if (p == NULL) { return ERR_ARG; }
00589 msg_ptr = (u8_t*)p->payload;
00590 plen += p->len;
00591 }
00592 else
00593 {
00594
00595 msg_ptr++;
00596 }
00597 }
00598 if (raw_len > 0)
00599 {
00600
00601 *msg_ptr = *raw;
00602 }
00603 return ERR_OK;
00604 }
00605 p = p->next;
00606 }
00607
00608 return ERR_ARG;
00609 }
00610
00611 #endif