Knight KE / Mbed OS Game_Master
Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers mpl.c Source File

mpl.c

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