+always_inline int
+vlib_object_within_buffer_data (vlib_main_t * vm, vlib_buffer_t * b,
+ void *obj, size_t len)
+{
+ u8 *o = obj;
+ if (o < b->data ||
+ o + len > b->data + vlib_buffer_get_default_data_size (vm))
+ return 0;
+ return 1;
+}
+
+/* Returns the number of bytes left in buffer from p. */
+static inline u32
+vlib_bytes_left_in_buffer (vlib_buffer_t *b, void *obj)
+{
+ return b->current_length - (((u8 *) obj - b->data) - b->current_data);
+}
+
+always_inline void *
+ip6_ext_next_header_s (ip_protocol_t cur_nh, void *hdr, u32 max_offset,
+ u32 *offset, int *res_nh, bool *last)
+{
+ u16 hdrlen = 0;
+ int new_nh = -1;
+ void *res = 0;
+ if (ip6_ext_hdr (cur_nh))
+ {
+ hdrlen = ip6_ext_header_len (hdr);
+ new_nh = ((ip6_ext_header_t *) hdr)->next_hdr;
+ res = hdr + hdrlen;
+ }
+ else if (cur_nh == IP_PROTOCOL_IPV6_FRAGMENTATION)
+ {
+ ip6_frag_hdr_t *frag_hdr = (ip6_frag_hdr_t *) hdr;
+ if (ip6_frag_hdr_offset (frag_hdr) > 0)
+ *last = true;
+ new_nh = frag_hdr->next_hdr;
+ hdrlen = sizeof (ip6_frag_hdr_t);
+ res = hdr + hdrlen;
+ }
+ else if (cur_nh == IP_PROTOCOL_IPSEC_AH)
+ {
+ new_nh = ((ip6_ext_header_t *) hdr)->next_hdr;
+ hdrlen = ip6_ext_authhdr_len (hdr);
+ res = hdr + hdrlen;
+ }
+ else
+ {
+ ;
+ }
+
+ if (res && (*offset + hdrlen) >= max_offset)
+ {
+ return 0;
+ }
+ *res_nh = new_nh;
+ *offset += hdrlen;
+ return res;
+}
+
+#define IP6_EXT_HDR_MAX (4) /* Maximum number of headers */
+#define IP6_EXT_HDR_MAX_DEPTH (256) /* Maximum header depth */
+typedef struct
+{
+ int length;
+ struct
+ {
+ u16 protocol;
+ u16 offset;
+ } eh[IP6_EXT_HDR_MAX];
+} ip6_ext_hdr_chain_t;
+
+/*
+ * Find ipv6 extension header within ipv6 header within
+ * whichever is smallest of buffer or IP6_EXT_HDR_MAX_DEPTH.
+ * The complete header chain must be in first buffer.
+ *
+ * The complete header chain (up to the terminating header) is
+ * returned in res.
+ * Returns the index of the find_hdr_type if > 0. Otherwise
+ * it returns the index of the last header.
+ */
+always_inline int
+ip6_ext_header_walk (vlib_buffer_t *b, ip6_header_t *ip, int find_hdr_type,
+ ip6_ext_hdr_chain_t *res)
+{
+ int i = 0;
+ int found = -1;
+ void *next_header = ip6_next_header (ip);
+ int next_proto = ip->protocol;
+ res->length = 0;
+ u32 n_bytes_this_buffer =
+ clib_min (vlib_bytes_left_in_buffer (b, ip), IP6_EXT_HDR_MAX_DEPTH);
+ u32 max_offset = clib_min (n_bytes_this_buffer,
+ sizeof (ip6_header_t) +
+ clib_net_to_host_u16 (ip->payload_length));
+ u32 offset = sizeof (ip6_header_t);
+ if ((ip6_ext_header_len_s (ip->protocol, next_header) + offset) > max_offset)
+ {
+ return -1;
+ }
+ bool last = false;
+ while (next_header)
+ {
+ /* Move on to next header */
+ res->eh[i].offset = offset;
+ res->eh[i].protocol = next_proto;
+ if (next_proto == find_hdr_type)
+ found = i;
+ i++;
+ if (last)
+ break;
+ if (i >= IP6_EXT_HDR_MAX)
+ break;
+ next_header = ip6_ext_next_header_s (next_proto, next_header, max_offset,
+ &offset, &next_proto, &last);
+ }
+ res->length = i;
+ if (find_hdr_type < 0)
+ return i - 1;
+ return found != -1 ? found : i - 1;
+}
+
+always_inline void *
+ip6_ext_header_find (vlib_main_t *vm, vlib_buffer_t *b, ip6_header_t *ip,
+ int find_hdr_type, ip6_ext_header_t **prev_ext_header)
+{
+ ip6_ext_hdr_chain_t hdr_chain;
+ int res = ip6_ext_header_walk (b, ip, find_hdr_type, &hdr_chain);
+ if (res < 0)
+ return 0;
+
+ if (prev_ext_header)
+ {
+ if (res > 0)
+ {
+ *prev_ext_header =
+ ip6_ext_next_header_offset (ip, hdr_chain.eh[res - 1].offset);
+ }
+ else
+ {
+ *prev_ext_header = 0;
+ }
+ }
+ if (find_hdr_type == hdr_chain.eh[res].protocol)
+ return ip6_ext_next_header_offset (ip, hdr_chain.eh[res].offset);
+ return 0;
+}