mpl.c

00001 /*
00002  * Copyright (c) 2015-2018, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: Apache-2.0
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *     http://www.apache.org/licenses/LICENSE-2.0
00010  *
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 #include "nsconfig.h"
00018 
00019 #ifdef HAVE_MPL
00020 
00021 #include "ns_types.h"
00022 #include "ns_list.h"
00023 #include "ns_trace.h"
00024 #include "common_functions.h"
00025 #include "nsdynmemLIB.h"
00026 #include "randLIB.h"
00027 #include <string.h>
00028 #include "Core/include/ns_buffer.h"
00029 #include "NWK_INTERFACE/Include/protocol.h"
00030 #include "NWK_INTERFACE/Include/protocol_timer.h"
00031 #include "Common_Protocols/ipv6.h"
00032 #include "Common_Protocols/icmpv6.h"
00033 #include "Service_Libs/Trickle/trickle.h"
00034 #include "6LoWPAN/MAC/mac_helper.h"
00035 #include "6LoWPAN/Thread/thread_common.h"
00036 #include "6LoWPAN/ws/ws_common.h"
00037 #include "MPL/mpl.h"
00038 
00039 #define TRACE_GROUP "mpl"
00040 
00041 #define MPL_OPT_S_MASK      0xC0
00042 #define MPL_OPT_S_SHIFT     6
00043 #define MPL_OPT_M           0x20
00044 #define MPL_OPT_V           0x10
00045 
00046 #define MPL_SEED_IPV6_SRC   0
00047 #define MPL_SEED_16_BIT     1
00048 #define MPL_SEED_64_BIT     2
00049 #define MPL_SEED_128_BIT    3
00050 
00051 #define MAX_BUFFERED_MESSAGES_SIZE 2048
00052 #define MAX_BUFFERED_MESSAGE_LIFETIME 600 // 1/10 s ticks
00053 
00054 static bool mpl_timer_running;
00055 static uint16_t mpl_total_buffered;
00056 
00057 const trickle_params_t rfc7731_default_data_message_trickle_params = {
00058     .Imin = MPL_MS_TO_TICKS(512),   /* RFC 7731 says 10 * expected link latency; ZigBee IP says 512 ms */
00059     .Imax = MPL_MS_TO_TICKS(512),   /* RFC 7731 says equal to Imin; ZigBee IP says 512 ms */
00060     .k = 1,                         /* RFC 7731 says 1; ZigBee IP says infinite */
00061     .TimerExpirations = 3           /* RFC 7731 says 3; ZigBee IP says 2 for routers, 0 for hosts */
00062 };
00063 
00064 const trickle_params_t rfc7731_default_control_message_trickle_params = {
00065     .Imin = MPL_MS_TO_TICKS(512),   /* RFC 7731 says 10 * worst-case link latency */
00066     .Imax = MPL_MS_TO_TICKS(300000),/* 5 minutes, as per RFC 7731 */
00067     .k = 1,
00068     .TimerExpirations = 10
00069 };
00070 
00071 /* Note that we don't use a buffer_t, to save a little RAM. We don't need
00072  * any of the metadata it stores...
00073  */
00074 typedef struct mpl_data_message {
00075     bool running;
00076     bool colour;
00077     uint32_t timestamp;
00078     trickle_t trickle;
00079     ns_list_link_t link;
00080     uint16_t mpl_opt_data_offset;   /* offset to option data of MPL option */
00081     uint8_t message[];
00082 } mpl_buffered_message_t;
00083 
00084 typedef struct mpl_seed {
00085     ns_list_link_t link;
00086     bool colour;
00087     uint16_t lifetime;
00088     uint8_t min_sequence;
00089     uint8_t id_len;
00090     NS_LIST_HEAD (mpl_buffered_message_t, link) messages; /* sequence number order */
00091     uint8_t id[];
00092 } mpl_seed_t;
00093 
00094 /* For simplicity, we assume each MPL domain is on exactly 1 interface */
00095 struct mpl_domain {
00096     protocol_interface_info_entry_t *interface;
00097     uint8_t address[16];
00098     uint8_t sequence;
00099     bool colour;
00100     bool proactive_forwarding;
00101     uint16_t seed_set_entry_lifetime;
00102     NS_LIST_HEAD (mpl_seed_t, link) seeds;
00103     trickle_t trickle;                      // Control timer
00104     trickle_params_t data_trickle_params;
00105     trickle_params_t control_trickle_params;
00106     ns_list_link_t link;
00107     multicast_mpl_seed_id_mode_e seed_id_mode;
00108     uint8_t seed_id[];
00109 };
00110 
00111 static NS_LIST_DEFINE(mpl_domains, mpl_domain_t, link);
00112 
00113 static void mpl_buffer_delete(mpl_seed_t *seed, mpl_buffered_message_t *message);
00114 static void mpl_control_reset_or_start(mpl_domain_t *domain);
00115 static void mpl_schedule_timer(void);
00116 static void mpl_fast_timer(uint16_t ticks);
00117 static buffer_t *mpl_exthdr_provider(buffer_t *buf, ipv6_exthdr_stage_t stage, int16_t *result);
00118 static void mpl_seed_delete(mpl_domain_t *domain, mpl_seed_t *seed);
00119 
00120 static bool mpl_initted;
00121 
00122 static void mpl_init(void)
00123 {
00124     if (mpl_initted) {
00125         return;
00126     }
00127     mpl_initted = true;
00128 
00129     ipv6_set_exthdr_provider(ROUTE_MPL, mpl_exthdr_provider);
00130 }
00131 
00132 static uint8_t mpl_buffer_sequence(const mpl_buffered_message_t *message)
00133 {
00134     return message->message[message->mpl_opt_data_offset + 1];
00135 }
00136 
00137 static uint16_t mpl_buffer_size(const mpl_buffered_message_t *message)
00138 {
00139     return IPV6_HDRLEN + common_read_16_bit(message->message + IPV6_HDROFF_PAYLOAD_LENGTH);
00140 }
00141 
00142 mpl_domain_t *mpl_domain_lookup(protocol_interface_info_entry_t *cur, const uint8_t address[16])
00143 {
00144     ns_list_foreach(mpl_domain_t, domain, &mpl_domains) {
00145         if (domain->interface == cur && addr_ipv6_equal(domain->address, address)) {
00146             return domain;
00147         }
00148     }
00149     return NULL;
00150 }
00151 
00152 mpl_domain_t *mpl_domain_lookup_with_realm_check(protocol_interface_info_entry_t *cur, const uint8_t address[16])
00153 {
00154     if (!addr_is_ipv6_multicast(address)) {
00155         return NULL;
00156     }
00157 
00158     if (addr_ipv6_multicast_scope(address) == IPV6_SCOPE_REALM_LOCAL && cur->mpl_treat_realm_domains_as_one) {
00159         address = ADDR_ALL_MPL_FORWARDERS;
00160     }
00161 
00162     return mpl_domain_lookup(cur, address);
00163 }
00164 
00165 /* Look up domain by address, ignoring the scop field, so ff22::1 matches ff23::1 */
00166 /* We assume all addresses are multicast, so don't bother checking the first byte */
00167 static mpl_domain_t *mpl_domain_lookup_ignoring_scop(protocol_interface_info_entry_t *cur, const uint8_t address[16])
00168 {
00169     ns_list_foreach(mpl_domain_t, domain, &mpl_domains) {
00170         if (domain->interface == cur &&
00171                 memcmp(address + 2, domain->address + 2, 14) == 0 &&
00172                 (address[1] & 0xf0) == (domain->address[1] & 0xf0)) {
00173             return domain;
00174         }
00175     }
00176     return NULL;
00177 }
00178 
00179 static int mpl_domain_count_on_interface(protocol_interface_info_entry_t *cur)
00180 {
00181     int count = 0;
00182     ns_list_foreach(mpl_domain_t, domain, &mpl_domains) {
00183         if (domain->interface == cur) {
00184             count++;
00185         }
00186     }
00187     return count;
00188 }
00189 
00190 mpl_domain_t *mpl_domain_create(protocol_interface_info_entry_t *cur, const uint8_t address[16],
00191                                 const uint8_t *seed_id, multicast_mpl_seed_id_mode_e seed_id_mode,
00192                                 int_fast8_t proactive_forwarding,
00193                                 uint16_t seed_set_entry_lifetime,
00194                                 const trickle_params_t *data_trickle_params,
00195                                 const trickle_params_t *control_trickle_params)
00196 {
00197     if (!addr_is_ipv6_multicast(address) || addr_ipv6_multicast_scope(address) < IPV6_SCOPE_REALM_LOCAL) {
00198         return NULL;
00199     }
00200 
00201     if (addr_ipv6_multicast_scope(address) == IPV6_SCOPE_REALM_LOCAL && cur->mpl_treat_realm_domains_as_one &&
00202             !addr_ipv6_equal(address, ADDR_ALL_MPL_FORWARDERS)) {
00203         return NULL;
00204     }
00205 
00206     mpl_init();
00207 
00208     /* We lock out attempts to join two domains differing only by scop - this
00209      * is because we couldn't distinguish control messages, which are sent
00210      * to the link-local version of the same address. Seems to be a
00211      * specification limitation?
00212      */
00213     if (mpl_domain_lookup_ignoring_scop(cur, address)) {
00214         return NULL;
00215     }
00216 
00217     if (seed_id_mode == MULTICAST_MPL_SEED_ID_DEFAULT) {
00218         seed_id_mode = cur->mpl_seed_id_mode;
00219         seed_id = cur->mpl_seed_id;
00220     }
00221 
00222     uint8_t seed_id_len;
00223     if (seed_id_mode > 0) {
00224         seed_id_len = seed_id_mode;
00225     } else {
00226         seed_id_len = 0;
00227     }
00228 
00229     mpl_domain_t *domain = ns_dyn_mem_alloc(sizeof * domain + seed_id_len);
00230     if (!domain) {
00231         return NULL;
00232     }
00233     memcpy(domain->address, address, 16);
00234     domain->interface = cur;
00235     domain->sequence = randLIB_get_8bit();
00236     domain->colour = false;
00237     ns_list_init(&domain->seeds);
00238     domain->proactive_forwarding = proactive_forwarding >= 0 ? proactive_forwarding
00239                                    : cur->mpl_proactive_forwarding;
00240     domain->seed_set_entry_lifetime = seed_set_entry_lifetime ? seed_set_entry_lifetime
00241                                       : cur->mpl_seed_set_entry_lifetime;
00242     domain->data_trickle_params = data_trickle_params ? *data_trickle_params
00243                                   : cur->mpl_data_trickle_params;
00244     domain->control_trickle_params = control_trickle_params ? *control_trickle_params
00245                                      : cur->mpl_control_trickle_params;
00246     trickle_start(&domain->trickle, &domain->control_trickle_params);
00247     trickle_stop(&domain->trickle);
00248     domain->seed_id_mode = seed_id_mode;
00249     memcpy(domain->seed_id, seed_id, seed_id_len);
00250     ns_list_add_to_end(&mpl_domains, domain);
00251 
00252     //ipv6_route_add_with_info(address, 128, cur->id, NULL, ROUTE_MPL, domain, 0, 0xffffffff, 0);
00253     addr_add_group(cur, address);
00254     if (domain->control_trickle_params.TimerExpirations != 0) {
00255         uint8_t ll_scope[16];
00256         memcpy(ll_scope, address, 16);
00257         ll_scope[1] = (ll_scope[1] & 0xf0) | IPV6_SCOPE_LINK_LOCAL;
00258         addr_add_group(cur, ll_scope);
00259     }
00260 
00261     /* If we just created the first domain on an interface, auto-create the all-forwarders domain (this does nothing if we're already a member) */
00262     if (mpl_domain_count_on_interface(cur) == 1) {
00263         /* Use default interface parameters */
00264         mpl_domain_create(cur, ADDR_ALL_MPL_FORWARDERS, NULL, MULTICAST_MPL_SEED_ID_DEFAULT, -1, 0, NULL, NULL);
00265         cur->mpl_seed = true;
00266     }
00267 
00268     return domain;
00269 }
00270 
00271 bool mpl_domain_delete(protocol_interface_info_entry_t *cur, const uint8_t address[16])
00272 {
00273     mpl_domain_t *domain = mpl_domain_lookup(cur, address);
00274     if (!domain) {
00275         return false;
00276     }
00277     int count = mpl_domain_count_on_interface(cur);
00278 
00279     /* Don't let them delete all-mpl-forwarders unless it's the last */
00280     if (addr_ipv6_equal(address, ADDR_ALL_MPL_FORWARDERS)) {
00281         if (count != 1) {
00282             return true;
00283         }
00284         cur->mpl_seed = false;
00285     }
00286 
00287     ns_list_foreach_safe(mpl_seed_t, seed, &domain->seeds) {
00288         mpl_seed_delete(domain, seed);
00289     }
00290 
00291     //ipv6_route_delete(address, 128, cur->id, NULL, ROUTE_MPL);
00292     addr_delete_group(cur, address);
00293     if (domain->control_trickle_params.TimerExpirations != 0) {
00294         uint8_t ll_scope[16];
00295         memcpy(ll_scope, domain->address, 16);
00296         ll_scope[1] = (ll_scope[1] & 0xf0) | IPV6_SCOPE_LINK_LOCAL;
00297         addr_delete_group(cur, ll_scope);
00298     }
00299     ns_list_remove(&mpl_domains, domain);
00300     ns_dyn_mem_free(domain);
00301     return true;
00302 }
00303 
00304 void mpl_domain_change_timing(mpl_domain_t *domain, const struct trickle_params *data_trickle_params, uint16_t seed_set_entry_lifetime)
00305 {
00306     domain->data_trickle_params = *data_trickle_params;
00307     domain->seed_set_entry_lifetime = seed_set_entry_lifetime;
00308 }
00309 
00310 static void mpl_domain_inconsistent(mpl_domain_t *domain)
00311 {
00312     trickle_inconsistent_heard(&domain->trickle, &domain->control_trickle_params);
00313     mpl_schedule_timer();
00314 }
00315 
00316 static mpl_seed_t *mpl_seed_lookup(const mpl_domain_t *domain, uint8_t id_len, const uint8_t *seed_id)
00317 {
00318     ns_list_foreach(mpl_seed_t, seed, &domain->seeds) {
00319         if (seed->id_len == id_len && memcmp(seed->id, seed_id, id_len) == 0) {
00320             return seed;
00321         }
00322     }
00323 
00324     return NULL;
00325 }
00326 
00327 static mpl_seed_t *mpl_seed_create(mpl_domain_t *domain, uint8_t id_len, const uint8_t *seed_id, uint8_t sequence)
00328 {
00329     mpl_seed_t *seed = ns_dyn_mem_alloc(sizeof(mpl_seed_t) + id_len);
00330     if (!seed) {
00331         return NULL;
00332     }
00333 
00334     seed->min_sequence = sequence;
00335     seed->lifetime = domain->seed_set_entry_lifetime;
00336     seed->id_len = id_len;
00337     seed->colour = domain->colour;
00338     ns_list_init(&seed->messages);
00339     memcpy(seed->id, seed_id, id_len);
00340     ns_list_add_to_end(&domain->seeds, seed);
00341     return seed;
00342 }
00343 
00344 static void mpl_seed_delete(mpl_domain_t *domain, mpl_seed_t *seed)
00345 {
00346     ns_list_foreach_safe(mpl_buffered_message_t, message, &seed->messages) {
00347         mpl_buffer_delete(seed, message);
00348     }
00349     ns_list_remove(&domain->seeds, seed);
00350     ns_dyn_mem_free(seed);
00351 }
00352 
00353 static void mpl_seed_advance_min_sequence(mpl_seed_t *seed, uint8_t min_sequence)
00354 {
00355     seed->min_sequence = min_sequence;
00356     ns_list_foreach_safe(mpl_buffered_message_t, message, &seed->messages) {
00357         if (common_serial_number_greater_8(min_sequence, mpl_buffer_sequence(message))) {
00358             mpl_buffer_delete(seed, message);
00359         }
00360     }
00361 }
00362 
00363 static mpl_buffered_message_t *mpl_buffer_lookup(mpl_seed_t *seed, uint8_t sequence)
00364 {
00365     ns_list_foreach(mpl_buffered_message_t, message, &seed->messages) {
00366         if (mpl_buffer_sequence(message) == sequence) {
00367             return message;
00368         }
00369     }
00370     return NULL;
00371 }
00372 
00373 static void mpl_free_space(void)
00374 {
00375     mpl_seed_t *oldest_seed = NULL;
00376     mpl_buffered_message_t *oldest_message = NULL;
00377 
00378     /* We'll free one message - earliest sequence number from one seed */
00379     /* Choose which seed by looking at the timestamp - oldest one first */
00380     ns_list_foreach(mpl_domain_t, domain, &mpl_domains) {
00381         ns_list_foreach(mpl_seed_t, seed, &domain->seeds) {
00382             mpl_buffered_message_t *message = ns_list_get_first(&seed->messages);
00383             if (!message) {
00384                 continue;
00385             }
00386             if (!oldest_message ||
00387                     protocol_core_monotonic_time - message->timestamp > protocol_core_monotonic_time - oldest_message->timestamp) {
00388                 oldest_message = message;
00389                 oldest_seed = seed;
00390             }
00391         }
00392     }
00393 
00394     if (!oldest_message) {
00395         return;
00396     }
00397 
00398     oldest_seed->min_sequence = mpl_buffer_sequence(oldest_message) + 1;
00399     mpl_buffer_delete(oldest_seed, oldest_message);
00400 }
00401 
00402 
00403 static mpl_buffered_message_t *mpl_buffer_create(buffer_t *buf, mpl_domain_t *domain, mpl_seed_t *seed, uint8_t sequence, uint8_t hop_limit)
00404 {
00405     /* IP layer ensures buffer length == IP length */
00406     uint16_t ip_len = buffer_data_length(buf);
00407 
00408     while (mpl_total_buffered + ip_len > MAX_BUFFERED_MESSAGES_SIZE) {
00409         mpl_free_space();
00410     }
00411 
00412     /* As we came in, message sequence was >= min_sequence, but mpl_free_space
00413      * could end up pushing min_sequence forward. We must take care and
00414      * re-check min_sequence.
00415      *
00416      * For example, let's say min_sequence=1, we're holding 1,3,5, and we receive 2.
00417      * a) If mpl_free_space doesn't touch this seed, we're fine.
00418      * b) If it frees 1, it will advance min_sequence to 2, and we're fine.
00419      * c) If it frees 1 and 3, it will advance min_sequence to 4, and we cannot
00420      *    accept this message. (If we forced min_sequence to 2, we'd end up processing
00421      *    message 3 again).
00422      */
00423     if (common_serial_number_greater_8(seed->min_sequence, sequence)) {
00424         tr_debug("Can no longer accept %"PRIu8" < %"PRIu8, sequence, seed->min_sequence);
00425         return NULL;
00426     }
00427 
00428     mpl_buffered_message_t *message = ns_dyn_mem_alloc(sizeof(mpl_buffered_message_t) + ip_len);
00429     if (!message) {
00430         return NULL;
00431     }
00432     memcpy(message->message, buffer_data_pointer(buf), ip_len);
00433     message->message[IPV6_HDROFF_HOP_LIMIT] = hop_limit;
00434     message->mpl_opt_data_offset = buf->mpl_option_data_offset;
00435     message->colour = seed->colour;
00436     message->timestamp = protocol_core_monotonic_time;
00437     /* Make sure trickle structure is initialised */
00438     trickle_start(&message->trickle, &domain->data_trickle_params);
00439     if (domain->proactive_forwarding) {
00440         mpl_schedule_timer();
00441     } else {
00442         /* Then stop it if not proactive */
00443         trickle_stop(&message->trickle);
00444     }
00445 
00446     /* Messages held ordered - eg for benefit of mpl_seed_bm_len() */
00447     bool inserted = false;
00448     ns_list_foreach_reverse(mpl_buffered_message_t, m, &seed->messages) {
00449         if (common_serial_number_greater_8(sequence, mpl_buffer_sequence(m))) {
00450             ns_list_add_after(&seed->messages, m, message);
00451             inserted = true;
00452             break;
00453         }
00454     }
00455     if (!inserted) {
00456         ns_list_add_to_start(&seed->messages, message);
00457     }
00458     mpl_total_buffered += ip_len;
00459 
00460     /* Does MPL spec intend this distinction between start and reset? */
00461     mpl_control_reset_or_start(domain);
00462 
00463     return message;
00464 }
00465 
00466 static void mpl_buffer_delete(mpl_seed_t *seed, mpl_buffered_message_t *message)
00467 {
00468     mpl_total_buffered -= mpl_buffer_size(message);
00469     ns_list_remove(&seed->messages, message);
00470     ns_dyn_mem_free(message);
00471 }
00472 
00473 static void mpl_buffer_transmit(mpl_domain_t *domain, mpl_buffered_message_t *message, bool newest)
00474 {
00475     uint16_t ip_len = mpl_buffer_size(message);
00476     buffer_t *buf = buffer_get(ip_len);
00477     if (!buf) {
00478         return;
00479     }
00480 
00481     buffer_data_add(buf, message->message, ip_len);
00482 
00483     /* Modify the M flag [Thread says it must be clear] */
00484     uint8_t *flag = buffer_data_pointer(buf) + message->mpl_opt_data_offset;
00485     if (newest && !thread_info(domain->interface)) {
00486         *flag |= MPL_OPT_M;
00487     } else {
00488         *flag &= ~MPL_OPT_M;
00489     }
00490 
00491     // Make sure ip_routed_up is set, even on locally-seeded packets, to
00492     // distinguishes the "forwarded" copies from the original seed.
00493     // Used to suppress extra copies to sleepy children.
00494     buf->ip_routed_up = true;
00495     buf->dst_sa .addr_type  = ADDR_IPV6 ;
00496     buf->src_sa .addr_type  = ADDR_IPV6 ;
00497     memcpy(buf->dst_sa .address , message->message + IPV6_HDROFF_DST_ADDR, 16);
00498     memcpy(buf->src_sa .address , message->message + IPV6_HDROFF_SRC_ADDR, 16);
00499 
00500     ipv6_transmit_multicast_on_interface(buf, domain->interface );
00501 }
00502 
00503 static void mpl_buffer_inconsistent(const mpl_domain_t *domain, mpl_buffered_message_t *message)
00504 {
00505     trickle_inconsistent_heard(&message->trickle, &domain->data_trickle_params);
00506     mpl_schedule_timer();
00507 }
00508 
00509 static uint8_t mpl_seed_bm_len(const mpl_seed_t *seed)
00510 {
00511     mpl_buffered_message_t *last = ns_list_get_last(&seed->messages);
00512     if (last) {
00513         return ((uint8_t)(mpl_buffer_sequence(last) - seed->min_sequence)) / 8 + 1;
00514     } else {
00515         return 0;
00516     }
00517 }
00518 
00519 /* Attempt to optimise by saying ID is source IPv6 */
00520 static uint16_t mpl_seed_info_size(const mpl_seed_t *seed, const uint8_t *src)
00521 {
00522     uint8_t id_len = seed->id_len;
00523     if (id_len == 16 && src && addr_ipv6_equal(src, seed->id)) {
00524         id_len = 0;
00525     }
00526     return 2 + id_len + mpl_seed_bm_len(seed);
00527 }
00528 
00529 static uint8_t *mpl_write_seed_info(uint8_t *ptr, const mpl_seed_t *seed, const uint8_t *src)
00530 {
00531     uint8_t bm_len = mpl_seed_bm_len(seed);
00532     ptr[0] = seed->min_sequence;
00533     ptr[1] = bm_len << 2;
00534     uint8_t id_len = seed->id_len;
00535     if (id_len == 16 && src && addr_ipv6_equal(src, seed->id)) {
00536         id_len = 0;
00537     }
00538     switch (id_len) {
00539         case  0:
00540             ptr[1] |= MPL_SEED_IPV6_SRC;
00541             break;
00542         case  2:
00543             ptr[1] |= MPL_SEED_16_BIT;
00544             break;
00545         case  8:
00546             ptr[1] |= MPL_SEED_64_BIT;
00547             break;
00548         case 16:
00549             ptr[1] |= MPL_SEED_128_BIT;
00550             break;
00551         default:
00552             return ptr;
00553     }
00554     ptr += 2;
00555     memcpy(ptr, seed->id, id_len);
00556     ptr += id_len;
00557     memset(ptr, 0, bm_len);
00558     ns_list_foreach(mpl_buffered_message_t, buffer, &seed->messages) {
00559         uint8_t i = mpl_buffer_sequence(buffer) - seed->min_sequence;
00560         bit_set(ptr, i);
00561     }
00562     ptr += bm_len;
00563     return ptr;
00564 }
00565 
00566 /* Does MPL spec really intend this distinction between start and reset? */
00567 /* (Reset sets interval to Imin, Start puts it somewhere random between Imin and Imax) */
00568 static void mpl_control_reset_or_start(mpl_domain_t *domain)
00569 {
00570     if (trickle_running(&domain->trickle, &domain->control_trickle_params)) {
00571         trickle_inconsistent_heard(&domain->trickle, &domain->control_trickle_params);
00572     } else {
00573         trickle_start(&domain->trickle, &domain->control_trickle_params);
00574     }
00575     mpl_schedule_timer();
00576 }
00577 
00578 static uint8_t mpl_seed_id_len(uint8_t seed_id_type)
00579 {
00580     static const uint8_t len[] = {
00581         [MPL_SEED_IPV6_SRC] = 0,
00582         [MPL_SEED_16_BIT] = 2,
00583         [MPL_SEED_64_BIT] = 8,
00584         [MPL_SEED_128_BIT] = 16
00585     };
00586     return len[seed_id_type];
00587 }
00588 
00589 static uint8_t mpl_seed_id_type(uint8_t seed_id_len)
00590 {
00591     switch (seed_id_len) {
00592         default:
00593             return MPL_SEED_IPV6_SRC;
00594         case 2:
00595             return MPL_SEED_16_BIT;
00596         case 8:
00597             return MPL_SEED_64_BIT;
00598         case 16:
00599             return MPL_SEED_128_BIT;
00600     }
00601 }
00602 
00603 /*
00604  *   0                   1                   2                   3
00605  *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
00606  *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
00607  *  |   min-seqno   |  bm-len   | S |   seed-id (0/2/8/16 octets)   |
00608  *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
00609  *  |                                                               |
00610  *  .            buffered-mpl-messages (variable length)            .
00611  *  .                                                               .
00612  *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
00613  */
00614 static void mpl_send_control(mpl_domain_t *domain)
00615 {
00616     uint16_t size = 0;
00617     const uint8_t *src = NULL;
00618 
00619     ns_list_foreach(mpl_seed_t, seed, &domain->seeds) {
00620         /* If not chosen yet, pick source to match a seed id, to save 16 bytes */
00621         if (!src && seed->id_len == 16 && addr_is_assigned_to_interface(domain->interface, seed->id)) {
00622             src = seed->id;
00623         }
00624         size += mpl_seed_info_size(seed, src);
00625     }
00626     buffer_t *buf = buffer_get(size);
00627     if (!buf) {
00628         return;
00629     }
00630     uint8_t *ptr = buffer_data_pointer(buf);
00631     ns_list_foreach(mpl_seed_t, seed, &domain->seeds) {
00632         ptr = mpl_write_seed_info(ptr, seed, src);
00633     }
00634     buffer_data_end_set(buf, ptr);
00635     buf->options .type  = ICMPV6_TYPE_INFO_MPL_CONTROL;
00636     buf->options .code  = 0;
00637     buf->options .hop_limit  = 255;
00638     memcpy(buf->dst_sa .address , domain->address, 16);
00639     buf->dst_sa .address [1] = (buf->dst_sa .address [1] & 0xf0) | IPV6_SCOPE_LINK_LOCAL;
00640     buf->dst_sa .addr_type  = ADDR_IPV6 ;
00641     if (src) {
00642         buf->src_sa .addr_type  = ADDR_IPV6 ;
00643         memcpy(buf->src_sa .address , src, 16);
00644     }
00645     buf->info  = (buffer_info_t)(B_FROM_ICMP | B_TO_ICMP | B_DIR_DOWN);
00646     buf->interface  = domain->interface;
00647     protocol_push(buf);
00648 }
00649 
00650 /*
00651  * There is an edge case in control handling when the hop limit runs out. This
00652  * is handled as follows:
00653  *
00654  *          Hop Limit 2                Hop Limit 1                [Won't Forward]
00655  *    Seed ---------------> Forwarder -------------> Final Node  -------X------> Neighbour Node
00656  * In Message Set        In Message Set           MinSequence advanced        Not In Message Set
00657  *
00658  * The Final Node does NOT add the message to its buffered message set, and it
00659  * advances MinSequence so that doesn't have to report about the message either
00660  * positively or negatively in control messages.
00661  *
00662  * If it reported "present" in control messages, the Neighbour Node would see a "missing"
00663  * message and reset its control timer. If it reported "absent", the Forwarder would
00664  * notice the inconsistency and resend. So we sidestep the issue by advancing MinSequence.
00665  * This also saves RAM - we'd never retransmit the message anyway, so why buffer it?
00666  *
00667  * This means we drop out-of-order packets at the edge of a hop limit boundary,
00668  * but this isn't a huge deal.
00669  */
00670 buffer_t *mpl_control_handler(buffer_t *buf, protocol_interface_info_entry_t *cur)
00671 {
00672     if (!addr_is_ipv6_multicast(buf->dst_sa .address ) || addr_ipv6_multicast_scope(buf->dst_sa .address ) != IPV6_SCOPE_LINK_LOCAL || buf->options .hop_limit  != 255) {
00673         tr_warn("Invalid control");
00674         return buffer_free(buf);
00675     }
00676 
00677     /* Um, how do we distinguish between multiple domains with different scop?
00678      * Control messages just have the domain address with scop 2. Currently
00679      * deal with that by not letting users join two domains differing only in
00680      * scop.
00681      */
00682 
00683     mpl_domain_t *domain = mpl_domain_lookup_ignoring_scop(cur, buf->dst_sa .address );
00684     if (!domain) {
00685         return buffer_free(buf);
00686     }
00687 
00688     bool they_have_new_data = false;
00689     bool we_have_new_data = false;
00690     const uint8_t *ptr = buffer_data_pointer(buf);
00691     const uint8_t *end = buffer_data_end(buf);
00692 
00693     // All objects will currently have the same colour. The scan
00694     // of the control message will flip the colour of every mentioned seed
00695     // and data message. Then the omission of anything we have will be detected
00696     // by its colour not being flipped.
00697     // This is equivalent to having a "mentioned" flag, except we don't have
00698     // to have a separate "reset" loop.
00699     domain->colour = !domain->colour;
00700     bool new_colour = domain->colour;
00701 
00702     while (ptr < end) {
00703         if (end - ptr < 2) {
00704             tr_err("MPL control error");
00705             break;
00706         }
00707         uint8_t min_seqno = ptr[0];
00708         uint8_t bm_len = ptr[1] >> 2;
00709         uint8_t seed_id_type = ptr[1] & 3;
00710         uint8_t seed_id_len = mpl_seed_id_len(seed_id_type);
00711         ptr += 2;
00712         /* Sequence number is 8-bit, so bitmask should never be bigger than 32 bytes */
00713         if (bm_len > 32 || end - ptr < seed_id_len + bm_len) {
00714             tr_err("MPL control error");
00715             break;
00716         }
00717         const uint8_t *seed_id;
00718         if (seed_id_type == MPL_SEED_IPV6_SRC) {
00719             seed_id = buf->src_sa .address ;
00720             seed_id_len = 16;
00721             /* Thread spec says, or at least implies, that ML16/RLOC address is
00722              * matched against corresponding 16-bit seed id (although
00723              * Thread doesn't use control messages...) */
00724             if (thread_addr_is_mesh_local_16(seed_id, cur)) {
00725                 seed_id += 14;
00726                 seed_id_len = 2;
00727             }
00728         } else {
00729             seed_id = ptr;
00730             ptr += seed_id_len;
00731         }
00732         mpl_seed_t *seed = mpl_seed_lookup(domain, seed_id_len, seed_id);
00733         if (!seed) {
00734             they_have_new_data = true;
00735             ptr += bm_len;
00736             continue;
00737         }
00738 
00739         seed->colour = new_colour;
00740         /* They are assumed to not be interested in messages lower than their min_seqno */
00741         ns_list_foreach(mpl_buffered_message_t, message, &seed->messages) {
00742             if (common_serial_number_greater_8(min_seqno, mpl_buffer_sequence(message))) {
00743                 message->colour = new_colour;
00744             }
00745         }
00746         for (uint8_t i = 0; i / 8 < bm_len; i++) {
00747             if (bit_test(ptr, i)) {
00748                 mpl_buffered_message_t *message = mpl_buffer_lookup(seed, min_seqno + i);
00749 
00750                 if (!message && common_serial_number_greater_8(min_seqno + i, seed->min_sequence)) {
00751                     they_have_new_data = true;
00752                 } else if (message) {
00753                     message->colour = new_colour;
00754                 }
00755             }
00756         }
00757         ptr += bm_len;
00758     }
00759 
00760     /* Search for seeds or messages they haven't mentioned */
00761     ns_list_foreach(mpl_seed_t, seed, &domain->seeds) {
00762         if (seed->colour != new_colour) {
00763             seed->colour = new_colour;
00764             we_have_new_data = true;
00765         }
00766         ns_list_foreach(mpl_buffered_message_t, message, &seed->messages) {
00767             if (message->colour != new_colour) {
00768                 message->colour = new_colour;
00769                 mpl_buffer_inconsistent(domain, message);
00770                 we_have_new_data = true;
00771             }
00772         }
00773     }
00774 
00775     if (they_have_new_data || we_have_new_data) {
00776         if (they_have_new_data) {
00777             tr_info("%s has new MPL data", trace_ipv6(buf->src_sa .address ));
00778         }
00779         if (we_have_new_data) {
00780             tr_info("We have new MPL data for %s", trace_ipv6(buf->src_sa .address ));
00781         }
00782         mpl_domain_inconsistent(domain);
00783     } else {
00784         trickle_consistent_heard(&domain->trickle);
00785     }
00786 
00787 
00788     return buffer_free(buf);
00789 }
00790 
00791 bool mpl_hbh_len_check(const uint8_t *opt_data, uint8_t opt_data_len)
00792 {
00793     if (opt_data_len < 2) {
00794         return false;
00795     }
00796     if (opt_data[0] & MPL_OPT_V) {
00797         return true; /* No length complaint - we let "process" drop */
00798     }
00799 
00800     uint8_t seed_id_type = (opt_data[0] & MPL_OPT_S_MASK) >> MPL_OPT_S_SHIFT;
00801     /* Note that option is allowed to be longer - spec allows for extension
00802      * beyond seed-id.
00803      */
00804     if (opt_data_len < 2 + mpl_seed_id_len(seed_id_type)) {
00805         return false;
00806     }
00807     return true;
00808 }
00809 
00810 /*      0                   1                   2                   3
00811  *      0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
00812  *                                     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
00813  *                                     |  Option Type  |  Opt Data Len |
00814  *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
00815  *     | S |M|V|  rsv  |   sequence    |      seed-id (optional)       |
00816  *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
00817  */
00818 
00819 bool mpl_process_hbh(buffer_t *buf, protocol_interface_info_entry_t *cur, uint8_t *opt_data)
00820 {
00821     if ((buf->options .ip_extflags  & IPEXT_HBH_MPL) || buf->options .ll_security_bypass_rx ) {
00822         tr_warn("Bad MPL");
00823         return false;
00824     }
00825 
00826     /* mpl_hbh_len_check has already returned true, so know length is okay */
00827 
00828     /* V flag indicates incompatible new version - packets MUST be dropped */
00829     if (opt_data[0] & MPL_OPT_V) {
00830         tr_warn("MPL V!");
00831         return false;
00832     }
00833 
00834     mpl_domain_t *domain = mpl_domain_lookup_with_realm_check(cur, buf->dst_sa .address );
00835     if (!domain) {
00836         tr_debug("No MPL domain");
00837         return false;
00838     }
00839 
00840     buf->options .ip_extflags  |= IPEXT_HBH_MPL;
00841     buf->mpl_option_data_offset = opt_data - buffer_data_pointer(buf);
00842 
00843     return true;
00844     // return mpl_forwarder_process_message(buf, domain, opt_data);
00845 }
00846 
00847 /* seeding is true if this is processing an outgoing message */
00848 bool mpl_forwarder_process_message(buffer_t *buf, mpl_domain_t *domain, bool seeding)
00849 {
00850     const uint8_t *opt_data = buffer_data_pointer(buf) + buf->mpl_option_data_offset;
00851     uint8_t sequence = opt_data[1];
00852     uint8_t seed_id_type = (opt_data[0] & MPL_OPT_S_MASK) >> MPL_OPT_S_SHIFT;
00853     const uint8_t *seed_id = opt_data + 2;
00854     uint8_t seed_id_len = mpl_seed_id_len(seed_id_type);
00855 
00856     /* Special handling - just ignore the MPL option if receiving loopback copy.
00857      * (MPL gets to process the outgoing message, and with seeding true - when
00858      * looping back, we want to accept it without MPL getting in the way).
00859      */
00860     if (!seeding && buf->options .multicast_loop ) {
00861         return true;
00862     }
00863 
00864     if (!domain) {
00865         domain = mpl_domain_lookup_with_realm_check(buf->interface , buf->dst_sa .address );
00866         if (!domain) {
00867             tr_debug("No domain %s  %s", tr_ipv6(domain->address), tr_array(seed_id, seed_id_len));
00868             return false;
00869         }
00870     }
00871 
00872     if (seed_id_type == MPL_SEED_IPV6_SRC) {
00873         seed_id = buf->src_sa .address ;
00874         seed_id_len = 16;
00875         /* Thread spec says, or at least implies, that ML16/RLOC address is
00876          * matched against corresponding 16-bit seed id */
00877         if (thread_addr_is_mesh_local_16(seed_id, buf->interface )) {
00878             seed_id += 14;
00879             seed_id_len = 2;
00880         }
00881     }
00882 
00883     tr_debug("seed %s seq %"PRIu8, tr_array(seed_id, seed_id_len), sequence);
00884     mpl_seed_t *seed = mpl_seed_lookup(domain, seed_id_len, seed_id);
00885     if (!seed) {
00886         seed = mpl_seed_create(domain, seed_id_len, seed_id, sequence);
00887         if (!seed) {
00888             tr_debug("No seed %s  %s", tr_ipv6(domain->address), tr_array(seed_id, seed_id_len));
00889             return false;
00890         }
00891     }
00892 
00893     /* If the M flag is set, we report an inconsistency against any messages with higher sequences */
00894     if ((opt_data[0] & MPL_OPT_M) && !thread_info(buf->interface )) {
00895         ns_list_foreach(mpl_buffered_message_t, message, &seed->messages) {
00896             if (common_serial_number_greater_8(mpl_buffer_sequence(message), sequence)) {
00897                 mpl_buffer_inconsistent(domain, message);
00898             }
00899         }
00900     }
00901 
00902     /* Drop old messages (sequence < MinSequence) */
00903     if (common_serial_number_greater_8(seed->min_sequence, sequence)) {
00904         tr_debug("Old MPL message %"PRIu8" < %"PRIu8, sequence, seed->min_sequence);
00905         return false;
00906     }
00907 
00908     mpl_buffered_message_t *message = mpl_buffer_lookup(seed, sequence);
00909     if (message) {
00910         tr_debug("Repeated MPL message %"PRIu8, sequence);
00911         trickle_consistent_heard(&message->trickle);
00912         return false;
00913     }
00914 
00915     seed->lifetime = domain->seed_set_entry_lifetime;
00916 
00917     uint8_t hop_limit = buffer_data_pointer(buf)[IPV6_HDROFF_HOP_LIMIT];
00918     if (!seeding && hop_limit != 0) {
00919         hop_limit--;
00920     }
00921 
00922     if (domain->data_trickle_params.TimerExpirations == 0 || hop_limit == 0 ||
00923             (thread_info(domain->interface) && !thread_i_am_router(domain->interface))) {
00924         /* As a non-forwarder, just accept the packet and advance the
00925          * min_sequence - means we will drop anything arriving out-of-order, but
00926          * old implementation always did this in all cases anyway (even if
00927          * being a forwarder).
00928          *
00929          * We also do this if hop limit is 0, so we are not going to forward.
00930          * This avoids the edge case discussed in the comment above mpl_control_handler.
00931          *
00932          * And finally, also treat Thread non-routers like this, to avoid
00933          * need to dynamically changing TimerExpirations.
00934          */
00935         mpl_seed_advance_min_sequence(seed, sequence + 1);
00936         return true;
00937     }
00938 
00939     message = mpl_buffer_create(buf, domain, seed, sequence, hop_limit);
00940 
00941     return true;
00942 }
00943 
00944 
00945 static void mpl_schedule_timer(void)
00946 {
00947     if (!mpl_timer_running) {
00948         mpl_timer_running = true;
00949         protocol_timer_start(PROTOCOL_TIMER_MULTICAST_TIM, mpl_fast_timer, MPL_TICK_MS);
00950     }
00951 }
00952 
00953 static void mpl_fast_timer(uint16_t ticks)
00954 {
00955     bool need_timer = false;
00956     mpl_timer_running = false;
00957 
00958     ns_list_foreach(mpl_domain_t, domain, &mpl_domains) {
00959         if (trickle_timer(&domain->trickle, &domain->control_trickle_params, ticks)) {
00960             mpl_send_control(domain);
00961         }
00962         ns_list_foreach(mpl_seed_t, seed, &domain->seeds) {
00963             ns_list_foreach(mpl_buffered_message_t, message, &seed->messages) {
00964                 if (trickle_timer(&message->trickle, &domain->data_trickle_params, ticks)) {
00965                     mpl_buffer_transmit(domain, message, ns_list_get_next(&seed->messages, message) == NULL);
00966                 }
00967                 need_timer = need_timer || trickle_running(&message->trickle, &domain->data_trickle_params);
00968             }
00969         }
00970         need_timer = need_timer || trickle_running(&domain->trickle, &domain->control_trickle_params);
00971     }
00972 
00973     if (need_timer) {
00974         mpl_schedule_timer();
00975     }
00976 }
00977 
00978 void mpl_slow_timer(uint16_t seconds)
00979 {
00980     ns_list_foreach(mpl_domain_t, domain, &mpl_domains) {
00981         uint32_t message_age_limit = (domain->seed_set_entry_lifetime * UINT32_C(10)) / 4;
00982         if (message_age_limit > MAX_BUFFERED_MESSAGE_LIFETIME) {
00983             message_age_limit = MAX_BUFFERED_MESSAGE_LIFETIME;
00984         }
00985         ns_list_foreach_safe(mpl_seed_t, seed, &domain->seeds) {
00986             /* Count down seed lifetime, and expire immediately when hit */
00987             if (seed->lifetime > seconds) {
00988                 seed->lifetime -= seconds;
00989             } else {
00990                 mpl_seed_delete(domain, seed);
00991                 continue;
00992             }
00993             /* Once data trickle timer has stopped, we MAY delete a message by
00994              * advancing MinSequence. We use timestamp to control this, so we
00995              * can hold beyond just the initial data transmission, permitting
00996              * it to be restarted by control messages.
00997              */
00998             ns_list_foreach_safe(mpl_buffered_message_t, message, &seed->messages) {
00999                 if (!trickle_running(&message->trickle, &domain->data_trickle_params) &&
01000                         protocol_core_monotonic_time - message->timestamp >= message_age_limit) {
01001                     seed->min_sequence = mpl_buffer_sequence(message) + 1;
01002                     mpl_buffer_delete(seed, message);
01003                 } else {
01004                     break;
01005                 }
01006             }
01007         }
01008     }
01009 }
01010 
01011 void mpl_clear_realm_scope_seeds(protocol_interface_info_entry_t *cur)
01012 {
01013     ns_list_foreach(mpl_domain_t, domain, &mpl_domains) {
01014         if (domain->interface == cur && addr_ipv6_multicast_scope(domain->address) <= IPV6_SCOPE_REALM_LOCAL) {
01015             ns_list_foreach_safe(mpl_seed_t, seed, &domain->seeds) {
01016                 mpl_seed_delete(domain, seed);
01017             }
01018         }
01019     }
01020 }
01021 
01022 static buffer_t *mpl_exthdr_provider(buffer_t *buf, ipv6_exthdr_stage_t stage, int16_t *result)
01023 {
01024     mpl_domain_t *domain = mpl_domain_lookup_with_realm_check(buf->interface , buf->dst_sa .address );
01025 
01026     /* Deal with simpler modify-already-created-header case first. Note that no error returns. */
01027     if (stage == IPV6_EXTHDR_MODIFY) {
01028         if (!domain) {
01029             *result = IPV6_EXTHDR_MODIFY_TUNNEL;
01030             memcpy(buf->dst_sa .address , ADDR_ALL_MPL_FORWARDERS, 16);
01031             buf->src_sa .addr_type  = ADDR_NONE ; // force auto-selection
01032             return buf;
01033         }
01034 
01035         if (buf->options .ip_extflags  & IPEXT_HBH_MPL_UNFILLED) {
01036             /* We assume we created this, therefore our option is in place
01037              * in the expected place. Sequence is set now, AFTER
01038              * fragmentation.
01039              */
01040             uint8_t *iphdr = buffer_data_pointer(buf);
01041             uint8_t *ext = iphdr + IPV6_HDRLEN;
01042             if (iphdr[IPV6_HDROFF_NH] != IPV6_NH_HOP_BY_HOP || ext[2] != IPV6_OPTION_MPL) {
01043                 tr_err("modify");
01044                 return buffer_free(buf);
01045             }
01046             /* We don't bother setting the M flag on these initial packets. Setting to 0 is always acceptable. */
01047             ext[5] = domain->sequence++;
01048             buf->options .ip_extflags  &= ~ IPEXT_HBH_MPL_UNFILLED;
01049             buf->mpl_option_data_offset = IPV6_HDRLEN + 4;
01050             mpl_forwarder_process_message(buf, domain, true);
01051         }
01052         *result = 0;
01053         return buf;
01054     }
01055 
01056     /* Rest of code deals with header insertion */
01057     if (!domain) {
01058         // We will need to tunnel - do nothing on the inner packet
01059         *result = 0;
01060         return buf;
01061     }
01062 
01063     const uint8_t *seed_id;
01064     uint8_t seed_id_len;
01065     uint8_t seed_id_buf[16];
01066     if (domain->seed_id_mode > 0) {
01067         seed_id_len = domain->seed_id_mode;
01068         seed_id = domain->seed_id;
01069     } else switch (domain->seed_id_mode) {
01070             case MULTICAST_MPL_SEED_ID_MAC_SHORT: {
01071                 uint16_t addr = mac_helper_mac16_address_get(buf->interface );
01072                 if (addr < 0xfffe) {
01073                     common_write_16_bit(addr, seed_id_buf);
01074                     seed_id = seed_id_buf;
01075                     seed_id_len = 2;
01076                     break;
01077                 }
01078             // Otherwise fall through to extended
01079                 case MULTICAST_MPL_SEED_ID_MAC:
01080                     seed_id = buf->interface ->mac;
01081                     seed_id_len = 8;
01082                     break;
01083 
01084                 case MULTICAST_MPL_SEED_ID_IID_EUI64:
01085                     seed_id = buf->interface ->iid_eui64;
01086                     seed_id_len = 8;
01087                     break;
01088 
01089                 case MULTICAST_MPL_SEED_ID_IID_SLAAC:
01090                     seed_id = buf->interface ->iid_slaac;
01091                     seed_id_len = 8;
01092                     break;
01093                 }
01094 
01095             default:
01096             case MULTICAST_MPL_SEED_ID_IPV6_SRC_FOR_DOMAIN:
01097                 seed_id = addr_select_source(buf->interface , domain->address, 0);
01098                 seed_id_len = 16;
01099                 break;
01100         }
01101 
01102     if (!seed_id) {
01103         tr_err("No MPL Seed ID");
01104         return buffer_free(buf);
01105     }
01106 
01107     /* "Compress" seed ID if it's the IPv6 source address */
01108     /* (For Thread, also compress if source is the 16-bit address) */
01109     /* (For Wi-sun, not support seed id address compression */
01110     if (!ws_info(buf->interface ) && seed_id_len == 16 && addr_ipv6_equal(seed_id, buf->src_sa .address )) {
01111         seed_id_len = 0;
01112     } else if (seed_id_len == 2 && thread_addr_is_mesh_local_16(buf->src_sa .address , buf->interface ) &&
01113                seed_id[0] == buf->src_sa .address [14] && seed_id[1] == buf->src_sa .address [15]) {
01114         seed_id_len = 0;
01115     }
01116 
01117     switch (stage) {
01118         case IPV6_EXTHDR_SIZE:
01119             *result = 4 + seed_id_len;
01120             return buf;
01121         case IPV6_EXTHDR_INSERT: {
01122             /* Only have 4 possible lengths/padding patterns to consider:
01123               * HbH 2 + Option header 4 + Seed 0 + Padding 2 = 8
01124               * HbH 2 + Option header 4 + Seed 2 + Padding 0 = 8
01125               * HbH 2 + Option header 4 + Seed 8 + Padding 2 = 16
01126               * HbH 2 + Option header 4 + Seed 16 + Padding 2 = 24
01127               */
01128             uint8_t extlen = (6 + seed_id_len + 7) & ~ 7;
01129             buf = buffer_headroom(buf, extlen);
01130             if (!buf) {
01131                 return NULL;
01132             }
01133             uint8_t *ext = buffer_data_reserve_header(buf, extlen);
01134             ext[0] = buf->options .type ;
01135             buf->options .type  = IPV6_NH_HOP_BY_HOP;
01136             ext[1] = (extlen / 8) - 1;
01137             ext[2] = IPV6_OPTION_MPL;
01138             ext[3] = 2 + seed_id_len;
01139             ext[4] = (mpl_seed_id_type(seed_id_len) << MPL_OPT_S_SHIFT);
01140             ext[5] = 0; // sequence placeholder
01141             memcpy(ext + 6, seed_id, seed_id_len);
01142             if (seed_id_len != 2) {
01143                 ext[extlen - 2] = IPV6_OPTION_PADN;
01144                 ext[extlen - 1] = 0;
01145             }
01146 
01147             *result = 0;
01148             buf->options .ip_extflags  |= IPEXT_HBH_MPL | IPEXT_HBH_MPL_UNFILLED;
01149             return buf;
01150         }
01151         default:
01152             return buffer_free(buf);
01153     }
01154 }
01155 
01156 #endif /* HAVE_MPL */
01157