+#define SRMPLS_TE_OFFSET 50
+
+/**
+ * @brief function to sort the colors in descending order
+ */
+int
+sort_color_descent (const u32 * x, u32 * y)
+{
+ return *y - *x;
+}
+
+/********************* Internal (NH, C) labels *******************************/
+/**
+ * @brief find the corresponding label for (endpoint, color) and lock it
+ * endpoint might be NULL or ANY
+ * NULL = 0, ANY=~0
+ */
+u32
+find_or_create_internal_label (ip46_address_t endpoint, u32 color)
+{
+ mpls_sr_main_t *sm = &sr_mpls_main;
+ uword *color_table, *result_label;
+
+ if (!sm->sr_policies_c2e2eclabel_hash.hash)
+ mhash_init (&sm->sr_policies_c2e2eclabel_hash, sizeof (mhash_t),
+ sizeof (u32));
+
+ color_table = mhash_get (&sm->sr_policies_c2e2eclabel_hash, &color);
+ if (!color_table)
+ {
+ mhash_t color_t;
+ memset (&color_t, 0, sizeof (mhash_t));
+ mhash_init (&color_t, sizeof (u32), sizeof (ip46_address_t));
+ mhash_set_mem (&sm->sr_policies_c2e2eclabel_hash, &color,
+ (uword *) & color_t, NULL);
+ color_table = mhash_get (&sm->sr_policies_c2e2eclabel_hash, &color);
+ }
+
+ result_label = mhash_get ((mhash_t *) color_table, &endpoint);
+
+ if (result_label)
+ return (u32) * result_label;
+
+ /* Create and set a new internal label */
+ u32 *new_internal_label = 0;
+ pool_get (sm->ec_labels, new_internal_label);
+ *new_internal_label = 0;
+ mhash_set ((mhash_t *) color_table, &endpoint,
+ (new_internal_label - sm->ec_labels) + SRMPLS_TE_OFFSET, NULL);
+
+ return (new_internal_label - sm->ec_labels) + SRMPLS_TE_OFFSET;
+}
+
+always_inline void
+internal_label_lock_co (ip46_address_t endpoint, u32 color, char co_bits)
+{
+ ip46_address_t zero, any;
+ ip46_address_reset (&zero);
+ any.as_u64[0] = any.as_u64[1] = (u64) ~ 0;
+ switch (co_bits)
+ {
+ case SR_TE_CO_BITS_10:
+ internal_label_lock (endpoint, color);
+ internal_label_lock (zero, color);
+ internal_label_lock (any, color);
+ break;
+ case SR_TE_CO_BITS_01:
+ internal_label_lock (endpoint, color);
+ internal_label_lock (zero, color);
+ break;
+ case SR_TE_CO_BITS_00:
+ case SR_TE_CO_BITS_11:
+ internal_label_lock (endpoint, color);
+ break;
+ }
+}
+
+/**
+ * @brief lock the label for (NH, C)
+ * endpoint might be NULL or ANY
+ * NULL = 0, ANY=~0
+ */
+void
+internal_label_lock (ip46_address_t endpoint, u32 color)
+{
+ mpls_sr_main_t *sm = &sr_mpls_main;
+ uword *color_table, *result_label;
+
+ if (!sm->sr_policies_c2e2eclabel_hash.hash)
+ return;
+
+ color_table = mhash_get (&sm->sr_policies_c2e2eclabel_hash, &color);
+ if (!color_table)
+ return;
+
+ result_label = mhash_get ((mhash_t *) color_table, &endpoint);
+
+ if (!result_label)
+ return;
+
+ /* Lock it */
+ u32 *label_lock =
+ pool_elt_at_index (sm->ec_labels, *result_label - SRMPLS_TE_OFFSET);
+ (*label_lock)++;
+}
+
+
+always_inline void
+internal_label_unlock_co (ip46_address_t endpoint, u32 color, char co_bits)
+{
+ ip46_address_t zero, any;
+ ip46_address_reset (&zero);
+ any.as_u64[0] = any.as_u64[1] = (u64) ~ 0;
+ switch (co_bits)
+ {
+ case SR_TE_CO_BITS_10:
+ internal_label_unlock (endpoint, color);
+ internal_label_unlock (zero, color);
+ internal_label_unlock (any, color);
+ break;
+ case SR_TE_CO_BITS_01:
+ internal_label_unlock (endpoint, color);
+ internal_label_unlock (zero, color);
+ break;
+ case SR_TE_CO_BITS_00:
+ case SR_TE_CO_BITS_11:
+ internal_label_unlock (endpoint, color);
+ break;
+ }
+}
+
+/**
+ * @brief Release lock on label for (endpoint, color)
+ * endpoint might be NULL or ANY
+ * NULL = 0, ANY=~0
+ */
+void
+internal_label_unlock (ip46_address_t endpoint, u32 color)
+{
+ mpls_sr_main_t *sm = &sr_mpls_main;
+ uword *color_table, *result_label;
+
+ if (!sm->sr_policies_c2e2eclabel_hash.hash)
+ return;
+
+ color_table = mhash_get (&sm->sr_policies_c2e2eclabel_hash, &color);
+ if (!color_table)
+ return;
+
+ result_label = mhash_get ((mhash_t *) color_table, &endpoint);
+
+ if (!result_label)
+ return;
+
+ u32 *label_lock =
+ pool_elt_at_index (sm->ec_labels, *result_label - SRMPLS_TE_OFFSET);
+ (*label_lock)--;
+
+ if (*label_lock == 0)
+ {
+ pool_put (sm->ec_labels, label_lock);
+ mhash_unset ((mhash_t *) color_table, &endpoint, NULL);
+ if (mhash_elts ((mhash_t *) color_table) == 0)
+ {
+ mhash_free ((mhash_t *) color_table);
+ mhash_unset (&sm->sr_policies_c2e2eclabel_hash, &color, NULL);
+ if (mhash_elts (&sm->sr_policies_c2e2eclabel_hash) == 0)
+ {
+ mhash_free (&sm->sr_policies_c2e2eclabel_hash);
+ sm->sr_policies_c2e2eclabel_hash.hash = NULL;
+ fib_table_unlock (sm->fib_table_EC, FIB_PROTOCOL_MPLS,
+ FIB_SOURCE_SR);
+ sm->fib_table_EC = (u32) ~ 0;
+ }
+ }
+ }
+}
+
+/********************* steering computation *********************************/
+/**
+ * @brief function to update the FIB
+ */
+void
+compute_sr_te_automated_steering_fib_entry (mpls_sr_steering_policy_t *
+ steer_pl)
+{
+ mpls_sr_main_t *sm = &sr_mpls_main;
+ fib_prefix_t pfx = { 0 };
+
+ u32 *internal_labels = 0;
+ ip46_address_t zero, any;
+ ip46_address_reset (&zero);
+ any.as_u64[0] = any.as_u64[1] = (u64) ~ 0;
+
+ u32 *color_i = NULL;
+ vec_foreach (color_i, steer_pl->color)
+ {
+ switch (steer_pl->co_bits)
+ {
+ case SR_TE_CO_BITS_10:
+ vec_add1 (internal_labels,
+ find_or_create_internal_label (steer_pl->next_hop,
+ *color_i));
+ vec_add1 (internal_labels,
+ find_or_create_internal_label (zero, *color_i));
+ vec_add1 (internal_labels,
+ find_or_create_internal_label (any, *color_i));
+ break;
+ case SR_TE_CO_BITS_01:
+ vec_add1 (internal_labels,
+ find_or_create_internal_label (steer_pl->next_hop,
+ *color_i));
+ vec_add1 (internal_labels,
+ find_or_create_internal_label (zero, *color_i));
+ break;
+ case SR_TE_CO_BITS_00:
+ case SR_TE_CO_BITS_11:
+ vec_add1 (internal_labels,
+ find_or_create_internal_label (steer_pl->next_hop,
+ *color_i));
+ break;
+ }
+ }
+
+ /* Does hidden FIB already exist? */
+ if (sm->fib_table_EC == (u32) ~ 0)
+ {
+ sm->fib_table_EC = fib_table_create_and_lock (FIB_PROTOCOL_MPLS,
+ FIB_SOURCE_SR,
+ "SR-MPLS Traffic Engineering (NextHop,Color)");
+
+ fib_table_flush (sm->fib_table_EC, FIB_PROTOCOL_MPLS,
+ FIB_SOURCE_SPECIAL);
+ }
+
+ /* Add the corresponding FIB entries */
+ fib_route_path_t path = {
+ .frp_proto = DPO_PROTO_MPLS,
+ .frp_eos = MPLS_EOS,
+ .frp_sw_if_index = ~0,
+ .frp_fib_index = sm->fib_table_EC,
+ .frp_weight = 1,
+ .frp_flags = FIB_ROUTE_PATH_FLAG_NONE,
+ .frp_label_stack = 0
+ };
+ fib_route_path_t *paths = NULL;
+
+ if (steer_pl->classify.traffic_type == SR_STEER_IPV6)
+ {
+ pfx.fp_proto = FIB_PROTOCOL_IP6;
+ pfx.fp_len = steer_pl->classify.mask_width;
+ pfx.fp_addr.ip6 = steer_pl->classify.prefix.ip6;
+ }
+ else if (steer_pl->classify.traffic_type == SR_STEER_IPV4)
+ {
+ pfx.fp_proto = FIB_PROTOCOL_IP4;
+ pfx.fp_len = steer_pl->classify.mask_width;
+ pfx.fp_addr.ip4 = steer_pl->classify.prefix.ip4;
+ }
+
+ if (steer_pl->vpn_label != (u32) ~ 0)
+ {
+ vec_add1 (path.frp_label_stack, steer_pl->vpn_label);
+ path.frp_eos = MPLS_NON_EOS;
+ }
+
+ u32 label_i;
+ vec_foreach_index (label_i, internal_labels)
+ {
+ path.frp_local_label = internal_labels[label_i];
+ path.frp_preference = label_i;
+ vec_add1 (paths, path);
+ }
+
+ /* Finally we must add to FIB IGP to N */
+ clib_memcpy (&path.frp_addr, &steer_pl->next_hop,
+ sizeof (steer_pl->next_hop));
+ path.frp_preference = vec_len (internal_labels);
+ path.frp_label_stack = NULL;
+
+ if (steer_pl->nh_type == SR_STEER_IPV6)
+ {
+ path.frp_proto = DPO_PROTO_IP6;
+ path.frp_fib_index =
+ fib_table_find (FIB_PROTOCOL_IP6,
+ (steer_pl->classify.fib_table !=
+ (u32) ~ 0 ? steer_pl->classify.fib_table : 0));
+ }
+ else if (steer_pl->nh_type == SR_STEER_IPV4)
+ {
+ path.frp_proto = DPO_PROTO_IP4;
+ path.frp_fib_index =
+ fib_table_find (FIB_PROTOCOL_IP4,
+ (steer_pl->classify.fib_table !=
+ (u32) ~ 0 ? steer_pl->classify.fib_table : 0));
+ }
+
+ vec_add1 (paths, path);
+ if (steer_pl->classify.traffic_type == SR_STEER_IPV6)
+ fib_table_entry_update (fib_table_find
+ (FIB_PROTOCOL_IP6,
+ (steer_pl->classify.fib_table !=
+ (u32) ~ 0 ? steer_pl->classify.fib_table : 0)),
+ &pfx, FIB_SOURCE_SR,
+ FIB_ENTRY_FLAG_LOOSE_URPF_EXEMPT, paths);
+ else if (steer_pl->classify.traffic_type == SR_STEER_IPV4)
+ fib_table_entry_update (fib_table_find
+ (FIB_PROTOCOL_IP4,
+ (steer_pl->classify.fib_table !=
+ (u32) ~ 0 ? steer_pl->classify.fib_table : 0)),
+ &pfx, FIB_SOURCE_SR,
+ FIB_ENTRY_FLAG_LOOSE_URPF_EXEMPT, paths);
+
+ vec_free (paths);
+ paths = NULL;
+}
+