| /** |
| * @file |
| * Abstract Syntax Notation One (ISO 8824, 8825) encoding |
| * |
| * @todo not optimised (yet), favor correctness over speed, favor speed over size |
| */ |
| |
| /* |
| * Copyright (c) 2006 Axon Digital Design B.V., The Netherlands. |
| * All rights reserved. |
| * |
| * Redistribution and use in source and binary forms, with or without modification, |
| * are permitted provided that the following conditions are met: |
| * |
| * 1. Redistributions of source code must retain the above copyright notice, |
| * this list of conditions and the following disclaimer. |
| * 2. Redistributions in binary form must reproduce the above copyright notice, |
| * this list of conditions and the following disclaimer in the documentation |
| * and/or other materials provided with the distribution. |
| * 3. The name of the author may not be used to endorse or promote products |
| * derived from this software without specific prior written permission. |
| * |
| * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED |
| * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF |
| * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT |
| * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
| * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT |
| * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING |
| * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY |
| * OF SUCH DAMAGE. |
| * |
| * Author: Christiaan Simons <christiaan.simons@axon.tv> |
| */ |
| |
| #include "lwip/opt.h" |
| |
| #if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */ |
| |
| #include "lwip/snmp_asn1.h" |
| |
| /** |
| * Returns octet count for length. |
| * |
| * @param length |
| * @param octets_needed points to the return value |
| */ |
| void |
| snmp_asn1_enc_length_cnt(u16_t length, u8_t *octets_needed) |
| { |
| if (length < 0x80U) |
| { |
| *octets_needed = 1; |
| } |
| else if (length < 0x100U) |
| { |
| *octets_needed = 2; |
| } |
| else |
| { |
| *octets_needed = 3; |
| } |
| } |
| |
| /** |
| * Returns octet count for an u32_t. |
| * |
| * @param value |
| * @param octets_needed points to the return value |
| * |
| * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded |
| * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value |
| * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!! |
| */ |
| void |
| snmp_asn1_enc_u32t_cnt(u32_t value, u16_t *octets_needed) |
| { |
| if (value < 0x80UL) |
| { |
| *octets_needed = 1; |
| } |
| else if (value < 0x8000UL) |
| { |
| *octets_needed = 2; |
| } |
| else if (value < 0x800000UL) |
| { |
| *octets_needed = 3; |
| } |
| else if (value < 0x80000000UL) |
| { |
| *octets_needed = 4; |
| } |
| else |
| { |
| *octets_needed = 5; |
| } |
| } |
| |
| /** |
| * Returns octet count for an s32_t. |
| * |
| * @param value |
| * @param octets_needed points to the return value |
| * |
| * @note ASN coded integers are _always_ signed. |
| */ |
| void |
| snmp_asn1_enc_s32t_cnt(s32_t value, u16_t *octets_needed) |
| { |
| if (value < 0) |
| { |
| value = ~value; |
| } |
| if (value < 0x80L) |
| { |
| *octets_needed = 1; |
| } |
| else if (value < 0x8000L) |
| { |
| *octets_needed = 2; |
| } |
| else if (value < 0x800000L) |
| { |
| *octets_needed = 3; |
| } |
| else |
| { |
| *octets_needed = 4; |
| } |
| } |
| |
| /** |
| * Returns octet count for an object identifier. |
| * |
| * @param ident_len object identifier array length |
| * @param ident points to object identifier array |
| * @param octets_needed points to the return value |
| */ |
| void |
| snmp_asn1_enc_oid_cnt(u8_t ident_len, s32_t *ident, u16_t *octets_needed) |
| { |
| s32_t sub_id; |
| u8_t cnt; |
| |
| cnt = 0; |
| if (ident_len > 1) |
| { |
| /* compressed prefix in one octet */ |
| cnt++; |
| ident_len -= 2; |
| ident += 2; |
| } |
| while(ident_len > 0) |
| { |
| ident_len--; |
| sub_id = *ident; |
| |
| sub_id >>= 7; |
| cnt++; |
| while(sub_id > 0) |
| { |
| sub_id >>= 7; |
| cnt++; |
| } |
| ident++; |
| } |
| *octets_needed = cnt; |
| } |
| |
| /** |
| * Encodes ASN type field into a pbuf chained ASN1 msg. |
| * |
| * @param p points to output pbuf to encode value into |
| * @param ofs points to the offset within the pbuf chain |
| * @param type input ASN1 type |
| * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode |
| */ |
| err_t |
| snmp_asn1_enc_type(struct pbuf *p, u16_t ofs, u8_t type) |
| { |
| u16_t plen, base; |
| u8_t *msg_ptr; |
| |
| plen = 0; |
| while (p != NULL) |
| { |
| base = plen; |
| plen += p->len; |
| if (ofs < plen) |
| { |
| msg_ptr = (u8_t*)p->payload; |
| msg_ptr += ofs - base; |
| *msg_ptr = type; |
| return ERR_OK; |
| } |
| p = p->next; |
| } |
| /* p == NULL, ofs >= plen */ |
| return ERR_ARG; |
| } |
| |
| /** |
| * Encodes host order length field into a pbuf chained ASN1 msg. |
| * |
| * @param p points to output pbuf to encode length into |
| * @param ofs points to the offset within the pbuf chain |
| * @param length is the host order length to be encoded |
| * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode |
| */ |
| err_t |
| snmp_asn1_enc_length(struct pbuf *p, u16_t ofs, u16_t length) |
| { |
| u16_t plen, base; |
| u8_t *msg_ptr; |
| |
| plen = 0; |
| while (p != NULL) |
| { |
| base = plen; |
| plen += p->len; |
| if (ofs < plen) |
| { |
| msg_ptr = (u8_t*)p->payload; |
| msg_ptr += ofs - base; |
| |
| if (length < 0x80) |
| { |
| *msg_ptr = (u8_t)length; |
| return ERR_OK; |
| } |
| else if (length < 0x100) |
| { |
| *msg_ptr = 0x81; |
| ofs += 1; |
| if (ofs >= plen) |
| { |
| /* next octet in next pbuf */ |
| p = p->next; |
| if (p == NULL) { return ERR_ARG; } |
| msg_ptr = (u8_t*)p->payload; |
| } |
| else |
| { |
| /* next octet in same pbuf */ |
| msg_ptr++; |
| } |
| *msg_ptr = (u8_t)length; |
| return ERR_OK; |
| } |
| else |
| { |
| u8_t i; |
| |
| /* length >= 0x100 && length <= 0xFFFF */ |
| *msg_ptr = 0x82; |
| i = 2; |
| while (i > 0) |
| { |
| i--; |
| ofs += 1; |
| if (ofs >= plen) |
| { |
| /* next octet in next pbuf */ |
| p = p->next; |
| if (p == NULL) { return ERR_ARG; } |
| msg_ptr = (u8_t*)p->payload; |
| plen += p->len; |
| } |
| else |
| { |
| /* next octet in same pbuf */ |
| msg_ptr++; |
| } |
| if (i == 0) |
| { |
| /* least significant length octet */ |
| *msg_ptr = (u8_t)length; |
| } |
| else |
| { |
| /* most significant length octet */ |
| *msg_ptr = (u8_t)(length >> 8); |
| } |
| } |
| return ERR_OK; |
| } |
| } |
| p = p->next; |
| } |
| /* p == NULL, ofs >= plen */ |
| return ERR_ARG; |
| } |
| |
| /** |
| * Encodes u32_t (counter, gauge, timeticks) into a pbuf chained ASN1 msg. |
| * |
| * @param p points to output pbuf to encode value into |
| * @param ofs points to the offset within the pbuf chain |
| * @param octets_needed encoding length (from snmp_asn1_enc_u32t_cnt()) |
| * @param value is the host order u32_t value to be encoded |
| * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode |
| * |
| * @see snmp_asn1_enc_u32t_cnt() |
| */ |
| err_t |
| snmp_asn1_enc_u32t(struct pbuf *p, u16_t ofs, u16_t octets_needed, u32_t value) |
| { |
| u16_t plen, base; |
| u8_t *msg_ptr; |
| |
| plen = 0; |
| while (p != NULL) |
| { |
| base = plen; |
| plen += p->len; |
| if (ofs < plen) |
| { |
| msg_ptr = (u8_t*)p->payload; |
| msg_ptr += ofs - base; |
| |
| if (octets_needed == 5) |
| { |
| /* not enough bits in 'value' add leading 0x00 */ |
| octets_needed--; |
| *msg_ptr = 0x00; |
| ofs += 1; |
| if (ofs >= plen) |
| { |
| /* next octet in next pbuf */ |
| p = p->next; |
| if (p == NULL) { return ERR_ARG; } |
| msg_ptr = (u8_t*)p->payload; |
| plen += p->len; |
| } |
| else |
| { |
| /* next octet in same pbuf */ |
| msg_ptr++; |
| } |
| } |
| while (octets_needed > 1) |
| { |
| octets_needed--; |
| *msg_ptr = (u8_t)(value >> (octets_needed << 3)); |
| ofs += 1; |
| if (ofs >= plen) |
| { |
| /* next octet in next pbuf */ |
| p = p->next; |
| if (p == NULL) { return ERR_ARG; } |
| msg_ptr = (u8_t*)p->payload; |
| plen += p->len; |
| } |
| else |
| { |
| /* next octet in same pbuf */ |
| msg_ptr++; |
| } |
| } |
| /* (only) one least significant octet */ |
| *msg_ptr = (u8_t)value; |
| return ERR_OK; |
| } |
| p = p->next; |
| } |
| /* p == NULL, ofs >= plen */ |
| return ERR_ARG; |
| } |
| |
| /** |
| * Encodes s32_t integer into a pbuf chained ASN1 msg. |
| * |
| * @param p points to output pbuf to encode value into |
| * @param ofs points to the offset within the pbuf chain |
| * @param octets_needed encoding length (from snmp_asn1_enc_s32t_cnt()) |
| * @param value is the host order s32_t value to be encoded |
| * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode |
| * |
| * @see snmp_asn1_enc_s32t_cnt() |
| */ |
| err_t |
| snmp_asn1_enc_s32t(struct pbuf *p, u16_t ofs, u16_t octets_needed, s32_t value) |
| { |
| u16_t plen, base; |
| u8_t *msg_ptr; |
| |
| plen = 0; |
| while (p != NULL) |
| { |
| base = plen; |
| plen += p->len; |
| if (ofs < plen) |
| { |
| msg_ptr = (u8_t*)p->payload; |
| msg_ptr += ofs - base; |
| |
| while (octets_needed > 1) |
| { |
| octets_needed--; |
| *msg_ptr = (u8_t)(value >> (octets_needed << 3)); |
| ofs += 1; |
| if (ofs >= plen) |
| { |
| /* next octet in next pbuf */ |
| p = p->next; |
| if (p == NULL) { return ERR_ARG; } |
| msg_ptr = (u8_t*)p->payload; |
| plen += p->len; |
| } |
| else |
| { |
| /* next octet in same pbuf */ |
| msg_ptr++; |
| } |
| } |
| /* (only) one least significant octet */ |
| *msg_ptr = (u8_t)value; |
| return ERR_OK; |
| } |
| p = p->next; |
| } |
| /* p == NULL, ofs >= plen */ |
| return ERR_ARG; |
| } |
| |
| /** |
| * Encodes object identifier into a pbuf chained ASN1 msg. |
| * |
| * @param p points to output pbuf to encode oid into |
| * @param ofs points to the offset within the pbuf chain |
| * @param ident_len object identifier array length |
| * @param ident points to object identifier array |
| * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode |
| */ |
| err_t |
| snmp_asn1_enc_oid(struct pbuf *p, u16_t ofs, u8_t ident_len, s32_t *ident) |
| { |
| u16_t plen, base; |
| u8_t *msg_ptr; |
| |
| plen = 0; |
| while (p != NULL) |
| { |
| base = plen; |
| plen += p->len; |
| if (ofs < plen) |
| { |
| msg_ptr = (u8_t*)p->payload; |
| msg_ptr += ofs - base; |
| |
| if (ident_len > 1) |
| { |
| if ((ident[0] == 1) && (ident[1] == 3)) |
| { |
| /* compressed (most common) prefix .iso.org */ |
| *msg_ptr = 0x2b; |
| } |
| else |
| { |
| /* calculate prefix */ |
| *msg_ptr = (u8_t)((ident[0] * 40) + ident[1]); |
| } |
| ofs += 1; |
| if (ofs >= plen) |
| { |
| /* next octet in next pbuf */ |
| p = p->next; |
| if (p == NULL) { return ERR_ARG; } |
| msg_ptr = (u8_t*)p->payload; |
| plen += p->len; |
| } |
| else |
| { |
| /* next octet in same pbuf */ |
| msg_ptr++; |
| } |
| ident_len -= 2; |
| ident += 2; |
| } |
| else |
| { |
| /* @bug: allow empty varbinds for symmetry (we must decode them for getnext), allow partial compression?? */ |
| /* ident_len <= 1, at least we need zeroDotZero (0.0) (ident_len == 2) */ |
| return ERR_ARG; |
| } |
| while (ident_len > 0) |
| { |
| s32_t sub_id; |
| u8_t shift, tail; |
| |
| ident_len--; |
| sub_id = *ident; |
| tail = 0; |
| shift = 28; |
| while(shift > 0) |
| { |
| u8_t code; |
| |
| code = (u8_t)(sub_id >> shift); |
| if ((code != 0) || (tail != 0)) |
| { |
| tail = 1; |
| *msg_ptr = code | 0x80; |
| ofs += 1; |
| if (ofs >= plen) |
| { |
| /* next octet in next pbuf */ |
| p = p->next; |
| if (p == NULL) { return ERR_ARG; } |
| msg_ptr = (u8_t*)p->payload; |
| plen += p->len; |
| } |
| else |
| { |
| /* next octet in same pbuf */ |
| msg_ptr++; |
| } |
| } |
| shift -= 7; |
| } |
| *msg_ptr = (u8_t)sub_id & 0x7F; |
| if (ident_len > 0) |
| { |
| ofs += 1; |
| if (ofs >= plen) |
| { |
| /* next octet in next pbuf */ |
| p = p->next; |
| if (p == NULL) { return ERR_ARG; } |
| msg_ptr = (u8_t*)p->payload; |
| plen += p->len; |
| } |
| else |
| { |
| /* next octet in same pbuf */ |
| msg_ptr++; |
| } |
| } |
| /* proceed to next sub-identifier */ |
| ident++; |
| } |
| return ERR_OK; |
| } |
| p = p->next; |
| } |
| /* p == NULL, ofs >= plen */ |
| return ERR_ARG; |
| } |
| |
| /** |
| * Encodes raw data (octet string, opaque) into a pbuf chained ASN1 msg. |
| * |
| * @param p points to output pbuf to encode raw data into |
| * @param ofs points to the offset within the pbuf chain |
| * @param raw_len raw data length |
| * @param raw points raw data |
| * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode |
| */ |
| err_t |
| snmp_asn1_enc_raw(struct pbuf *p, u16_t ofs, u16_t raw_len, u8_t *raw) |
| { |
| u16_t plen, base; |
| u8_t *msg_ptr; |
| |
| plen = 0; |
| while (p != NULL) |
| { |
| base = plen; |
| plen += p->len; |
| if (ofs < plen) |
| { |
| msg_ptr = (u8_t*)p->payload; |
| msg_ptr += ofs - base; |
| |
| while (raw_len > 1) |
| { |
| /* copy raw_len - 1 octets */ |
| raw_len--; |
| *msg_ptr = *raw; |
| raw++; |
| ofs += 1; |
| if (ofs >= plen) |
| { |
| /* next octet in next pbuf */ |
| p = p->next; |
| if (p == NULL) { return ERR_ARG; } |
| msg_ptr = (u8_t*)p->payload; |
| plen += p->len; |
| } |
| else |
| { |
| /* next octet in same pbuf */ |
| msg_ptr++; |
| } |
| } |
| if (raw_len > 0) |
| { |
| /* copy last or single octet */ |
| *msg_ptr = *raw; |
| } |
| return ERR_OK; |
| } |
| p = p->next; |
| } |
| /* p == NULL, ofs >= plen */ |
| return ERR_ARG; |
| } |
| |
| #endif /* LWIP_SNMP */ |