diff --git a/source/6LoWPAN/MAC/mac_ie_lib.c b/source/6LoWPAN/MAC/mac_ie_lib.c
index 0235b905a4b..f7460042bc8 100644
--- a/source/6LoWPAN/MAC/mac_ie_lib.c
+++ b/source/6LoWPAN/MAC/mac_ie_lib.c
@@ -94,3 +94,25 @@ uint8_t mac_ie_header_discover(uint8_t *header_ptr, uint16_t length, mac_header_
     }
     return 0;
 }
+
+uint8_t mac_ie_header_sub_id_discover(uint8_t *header_ptr, uint16_t length, mac_header_IE_t * header_ie, uint8_t sub_id)
+{
+    mac_header_IE_t ie_element;
+    uint8_t *sub_id_ptr;
+    while (length > 2) {
+        mac_ie_header_parse(&ie_element, header_ptr);
+        sub_id_ptr = ie_element.content_ptr;
+        if (ie_element.length && header_ie->id == ie_element.id && *sub_id_ptr == sub_id) {
+            sub_id_ptr++;
+            ie_element.length--;
+            header_ie->content_ptr = sub_id_ptr;
+            header_ie->length = ie_element.length;
+            return ie_element.length;
+        }
+
+        length -= ie_element.length +2;
+
+        header_ptr += ie_element.length + 2;
+    }
+    return 0;
+}
diff --git a/source/6LoWPAN/MAC/mac_ie_lib.h b/source/6LoWPAN/MAC/mac_ie_lib.h
index 65967cffcb9..4c7c8da3724 100644
--- a/source/6LoWPAN/MAC/mac_ie_lib.h
+++ b/source/6LoWPAN/MAC/mac_ie_lib.h
@@ -25,5 +25,6 @@ uint8_t *mac_ie_header_base_write(uint8_t *ptr, uint8_t type, uint16_t length);
 uint8_t *mac_ie_payload_base_write(uint8_t *ptr, uint8_t type, uint16_t length);
 uint16_t mac_ie_payload_discover(uint8_t *payload_ptr, uint16_t length, struct mac_payload_IE_s * payload_ie);
 uint8_t mac_ie_header_discover(uint8_t *header_ptr, uint16_t length, struct mac_header_IE_s * header_ie);
+uint8_t mac_ie_header_sub_id_discover(uint8_t *header_ptr, uint16_t length, mac_header_IE_t * header_ie, uint8_t sub_id);
 
 #endif /* MAC_IE_LIB_H_ */
diff --git a/test/nanostack/unittest/stub/mac_ie_lib_stub.c b/test/nanostack/unittest/stub/mac_ie_lib_stub.c
index 60b67567805..4edab2019e7 100644
--- a/test/nanostack/unittest/stub/mac_ie_lib_stub.c
+++ b/test/nanostack/unittest/stub/mac_ie_lib_stub.c
@@ -45,3 +45,10 @@ uint8_t mac_ie_header_discover(uint8_t *header_ptr, uint16_t length, mac_header_
     header_ie->content_ptr = payload_ptr;
     return mac_ie_lib_stub_def.value_uint8;
 }
+
+
+uint8_t mac_ie_header_sub_id_discover(uint8_t *header_ptr, uint16_t length, mac_header_IE_t * header_ie, uint8_t sub_id)
+{
+    header_ie->content_ptr = payload_ptr;
+    return mac_ie_lib_stub_def.value_uint8;
+}