38a274e3e3c8db2cb4a0664351d600a05966dbf6
[vpp.git] / vnet / vnet / lisp-gpe / ip_forward.c
1 /*
2  * Copyright (c) 2016 Cisco and/or its affiliates.
3  * Licensed under the Apache License, Version 2.0 (the "License");
4  * you may not use this file except in compliance with the License.
5  * You may obtain a copy of the License at:
6  *
7  *     http://www.apache.org/licenses/LICENSE-2.0
8  *
9  * Unless required by applicable law or agreed to in writing, software
10  * distributed under the License is distributed on an "AS IS" BASIS,
11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12  * See the License for the specific language governing permissions and
13  * limitations under the License.
14  */
15
16 #include <vnet/lisp-gpe/lisp_gpe.h>
17
18 /* avoids calling route callbacks for src fib */
19 static void
20 ip4_sd_fib_set_adj_index (lisp_gpe_main_t * lgm, ip4_fib_t * fib, u32 flags,
21                            u32 dst_address_u32, u32 dst_address_length,
22                            u32 adj_index)
23 {
24   ip_lookup_main_t * lm = lgm->lm4;
25   uword * hash;
26
27   if (vec_bytes(fib->old_hash_values))
28     memset (fib->old_hash_values, ~0, vec_bytes (fib->old_hash_values));
29   if (vec_bytes(fib->new_hash_values))
30     memset (fib->new_hash_values, ~0, vec_bytes (fib->new_hash_values));
31   fib->new_hash_values[0] = adj_index;
32
33   /* Make sure adj index is valid. */
34   if (CLIB_DEBUG > 0)
35     (void) ip_get_adjacency (lm, adj_index);
36
37   hash = fib->adj_index_by_dst_address[dst_address_length];
38
39   hash = _hash_set3 (hash, dst_address_u32,
40                      fib->new_hash_values,
41                      fib->old_hash_values);
42
43   fib->adj_index_by_dst_address[dst_address_length] = hash;
44 }
45
46 /* copied from ip4_forward since it's static */
47 static void
48 ip4_fib_init_adj_index_by_dst_address (ip_lookup_main_t * lm,
49                                        ip4_fib_t * fib,
50                                        u32 address_length)
51 {
52   hash_t * h;
53   uword max_index;
54
55   ASSERT (lm->fib_result_n_bytes >= sizeof (uword));
56   lm->fib_result_n_words = round_pow2 (lm->fib_result_n_bytes, sizeof(uword))
57       / sizeof(uword);
58
59   fib->adj_index_by_dst_address[address_length] =
60     hash_create (32 /* elts */, lm->fib_result_n_words * sizeof (uword));
61
62   hash_set_flags (fib->adj_index_by_dst_address[address_length],
63                   HASH_FLAG_NO_AUTO_SHRINK);
64
65   h = hash_header (fib->adj_index_by_dst_address[address_length]);
66   max_index = (hash_value_bytes (h) / sizeof (fib->new_hash_values[0])) - 1;
67
68   /* Initialize new/old hash value vectors. */
69   vec_validate_init_empty (fib->new_hash_values, max_index, ~0);
70   vec_validate_init_empty (fib->old_hash_values, max_index, ~0);
71 }
72
73 static void
74 ip4_sd_fib_add_del_src_route (lisp_gpe_main_t * lgm,
75                               ip4_add_del_route_args_t * a)
76 {
77   ip_lookup_main_t * lm = lgm->lm4;
78   ip4_fib_t * fib;
79   u32 dst_address, dst_address_length, adj_index, old_adj_index;
80   uword * hash, is_del;
81
82   /* Either create new adjacency or use given one depending on arguments. */
83   if (a->n_add_adj > 0)
84       ip_add_adjacency (lm, a->add_adj, a->n_add_adj, &adj_index);
85   else
86     adj_index = a->adj_index;
87
88   dst_address = a->dst_address.data_u32;
89   dst_address_length = a->dst_address_length;
90
91   fib = pool_elt_at_index(lgm->ip4_src_fibs, a->table_index_or_table_id);
92
93   if (! fib->adj_index_by_dst_address[dst_address_length])
94     ip4_fib_init_adj_index_by_dst_address (lm, fib, dst_address_length);
95
96   hash = fib->adj_index_by_dst_address[dst_address_length];
97
98   is_del = (a->flags & IP4_ROUTE_FLAG_DEL) != 0;
99
100   if (is_del)
101     {
102       fib->old_hash_values[0] = ~0;
103       hash = _hash_unset (hash, dst_address, fib->old_hash_values);
104       fib->adj_index_by_dst_address[dst_address_length] = hash;
105     }
106   else
107     ip4_sd_fib_set_adj_index (lgm, fib, a->flags, dst_address,
108                               dst_address_length, adj_index);
109
110   old_adj_index = fib->old_hash_values[0];
111
112   /* Avoid spurious reference count increments */
113   if (old_adj_index == adj_index
114       && adj_index != ~0
115       && !(a->flags & IP4_ROUTE_FLAG_KEEP_OLD_ADJACENCY))
116     {
117       ip_adjacency_t * adj = ip_get_adjacency (lm, adj_index);
118       if (adj->share_count > 0)
119         adj->share_count --;
120     }
121
122   ip4_fib_mtrie_add_del_route (fib, a->dst_address, dst_address_length,
123                                is_del ? old_adj_index : adj_index,
124                                is_del);
125
126   /* Delete old adjacency index if present and changed. */
127   if (! (a->flags & IP4_ROUTE_FLAG_KEEP_OLD_ADJACENCY)
128       && old_adj_index != ~0
129       && old_adj_index != adj_index)
130     ip_del_adjacency (lm, old_adj_index);
131 }
132
133 static void *
134 ip4_sd_get_src_route (lisp_gpe_main_t * lgm, u32 src_fib_index,
135                       ip4_address_t * src, u32 address_length)
136 {
137   ip4_fib_t * fib = pool_elt_at_index (lgm->ip4_src_fibs, src_fib_index);
138   uword * hash, * p;
139
140   hash = fib->adj_index_by_dst_address[address_length];
141   p = hash_get (hash, src->as_u32);
142   return (void *) p;
143 }
144
145 typedef CLIB_PACKED (struct ip4_route {
146   ip4_address_t address;
147   u32 address_length : 6;
148   u32 index : 26;
149 }) ip4_route_t;
150
151 void
152 ip4_sd_fib_clear_src_fib (lisp_gpe_main_t * lgm, ip4_fib_t * fib)
153 {
154   ip4_route_t * routes = 0, * r;
155   u32 i;
156
157   vec_reset_length (routes);
158
159   for (i = 0; i < ARRAY_LEN (fib->adj_index_by_dst_address); i++) {
160       uword * hash = fib->adj_index_by_dst_address[i];
161       hash_pair_t * p;
162       ip4_route_t x;
163
164       x.address_length = i;
165
166       hash_foreach_pair (p, hash,
167       ({
168           x.address.data_u32 = p->key;
169           vec_add1 (routes, x);
170       }));
171   }
172
173   vec_foreach (r, routes) {
174       ip4_add_del_route_args_t a;
175
176       memset (&a, 0, sizeof (a));
177       a.flags = IP4_ROUTE_FLAG_FIB_INDEX | IP4_ROUTE_FLAG_DEL;
178       a.table_index_or_table_id = fib - lgm->ip4_src_fibs;
179       a.dst_address = r->address;
180       a.dst_address_length = r->address_length;
181       a.adj_index = ~0;
182
183       ip4_sd_fib_add_del_src_route (lgm, &a);
184   }
185 }
186
187 static u8
188 ip4_fib_is_empty (ip4_fib_t * fib)
189 {
190   u8 fib_is_empty;
191   int i;
192
193   fib_is_empty = 1;
194   for (i = ARRAY_LEN (fib->adj_index_by_dst_address) - 1; i >= 0; i--)
195     {
196       uword * hash = fib->adj_index_by_dst_address[i];
197       uword n_elts = hash_elts (hash);
198       if (n_elts)
199         {
200           fib_is_empty = 0;
201           break;
202         }
203     }
204   return fib_is_empty;
205 }
206
207 static int
208 ip4_sd_fib_add_del_route (lisp_gpe_main_t * lgm, ip_prefix_t * dst_prefix,
209                           ip_prefix_t * src_prefix, u32 table_id,
210                           ip_adjacency_t * add_adj, u8 is_add)
211 {
212   uword * p;
213   ip4_add_del_route_args_t a;
214   ip_adjacency_t * dst_adjp, dst_adj;
215   ip4_address_t dst = ip_prefix_v4(dst_prefix), src;
216   u32 dst_address_length = ip_prefix_len(dst_prefix), src_address_length = 0;
217   ip4_fib_t * src_fib;
218
219   if (src_prefix)
220     {
221       src = ip_prefix_v4(src_prefix);
222       src_address_length = ip_prefix_len(src_prefix);
223     }
224   else
225     memset(&src, 0, sizeof(src));
226
227   /* lookup dst adj */
228   p = ip4_get_route (lgm->im4, table_id, 0, dst.as_u8, dst_address_length);
229
230   if (is_add)
231     {
232       /* insert dst prefix to ip4 fib, if it's not in yet */
233       if (p == 0)
234         {
235           /* allocate and init src ip4 fib */
236           pool_get(lgm->ip4_src_fibs, src_fib);
237           ip4_mtrie_init (&src_fib->mtrie);
238
239           /* configure adjacency */
240           memset(&dst_adj, 0, sizeof(dst_adj));
241
242           /* reuse rewrite header to store pointer to src fib */
243           dst_adj.rewrite_header.sw_if_index = src_fib - lgm->ip4_src_fibs;
244
245           /* dst adj should point to lisp gpe lookup */
246           dst_adj.lookup_next_index = lgm->ip4_lookup_next_lgpe_ip4_lookup;
247
248           memset(&a, 0, sizeof(a));
249           a.flags = IP4_ROUTE_FLAG_TABLE_ID;
250           a.table_index_or_table_id = table_id; /* vrf */
251           a.adj_index = ~0;
252           a.dst_address_length = dst_address_length;
253           a.dst_address = dst;
254           a.flags |= IP4_ROUTE_FLAG_ADD;
255           a.add_adj = &dst_adj;
256           a.n_add_adj = 1;
257
258           ip4_add_del_route (lgm->im4, &a);
259
260           /* lookup dst adj to obtain the adj index */
261           p = ip4_get_route (lgm->im4, table_id, 0, dst.as_u8,
262                              dst_address_length);
263
264           ASSERT(p != 0);
265         }
266     }
267   else
268     {
269       if (p == 0)
270         {
271           clib_warning("Trying to delete inexistent dst route for %U. Aborting",
272                        format_ip4_address_and_length, dst.as_u8,
273                        dst_address_length);
274           return -1;
275         }
276     }
277
278   dst_adjp = ip_get_adjacency (lgm->lm4, p[0]);
279
280   /* add/del src prefix to src fib */
281   memset(&a, 0, sizeof(a));
282   a.flags = IP4_ROUTE_FLAG_TABLE_ID;
283   a.table_index_or_table_id = dst_adjp->rewrite_header.sw_if_index;
284   a.adj_index = ~0;
285   a.flags |= is_add ? IP4_ROUTE_FLAG_ADD : IP4_ROUTE_FLAG_DEL;
286   a.add_adj = add_adj;
287   a.n_add_adj = is_add ? 1 : 0;
288   /* if src prefix is null, add 0/0 */
289   a.dst_address_length = src_address_length;
290   a.dst_address = src;
291   ip4_sd_fib_add_del_src_route (lgm, &a);
292
293   /* if a delete, check if there are elements left in the src fib */
294   if (!is_add)
295     {
296       src_fib = pool_elt_at_index(lgm->ip4_src_fibs,
297                                   dst_adjp->rewrite_header.sw_if_index);
298       if (!src_fib)
299         return 0;
300
301       /* if there's nothing left */
302       if (ip4_fib_is_empty(src_fib))
303         {
304           /* remove the src fib ..  */
305           pool_put(lgm->ip4_src_fibs, src_fib);
306
307           /* .. and remove dst route */
308           memset(&a, 0, sizeof(a));
309           a.flags = IP4_ROUTE_FLAG_TABLE_ID;
310           a.table_index_or_table_id = table_id; /* vrf */
311           a.adj_index = ~0;
312           a.dst_address_length = dst_address_length;
313           a.dst_address = dst;
314           a.flags |= IP4_ROUTE_FLAG_DEL;
315
316           ip4_add_del_route (lgm->im4, &a);
317         }
318     }
319
320   return 0;
321 }
322
323 static void *
324 ip4_sd_fib_get_route (lisp_gpe_main_t * lgm, ip_prefix_t * dst_prefix,
325                       ip_prefix_t * src_prefix, u32 table_id)
326 {
327   uword * p;
328   ip4_address_t dst = ip_prefix_v4(dst_prefix), src;
329   u32 dst_address_length = ip_prefix_len(dst_prefix), src_address_length = 0;
330   ip_adjacency_t * dst_adj;
331
332   if (src_prefix)
333     {
334       src = ip_prefix_v4(src_prefix);
335       src_address_length = ip_prefix_len(src_prefix);
336     }
337   else
338     memset(&src, 0, sizeof(src));
339
340   /* lookup dst adj */
341   p = ip4_get_route (lgm->im4, table_id, 0, dst.as_u8, dst_address_length);
342   if (p == 0)
343       return p;
344
345   dst_adj = ip_get_adjacency (lgm->lm4, p[0]);
346   return ip4_sd_get_src_route (lgm, dst_adj->rewrite_header.sw_if_index, &src,
347                                src_address_length);
348 }
349
350 static u32
351 ip6_sd_get_src_route (lisp_gpe_main_t * lgm, u32 src_fib_index,
352                       ip6_address_t * src, u32 address_length)
353 {
354   int i, len;
355   int rv;
356   BVT(clib_bihash_kv) kv, value;
357   ip6_src_fib_t * fib = pool_elt_at_index (lgm->ip6_src_fibs, src_fib_index);
358
359   len = vec_len (fib->prefix_lengths_in_search_order);
360
361   for (i = 0; i < len; i++)
362     {
363       int dst_address_length = fib->prefix_lengths_in_search_order[i];
364       ip6_address_t * mask;
365
366       ASSERT(dst_address_length >= 0 && dst_address_length <= 128);
367
368       mask = &fib->fib_masks[dst_address_length];
369
370       kv.key[0] = src->as_u64[0] & mask->as_u64[0];
371       kv.key[1] = src->as_u64[1] & mask->as_u64[1];
372       kv.key[2] = dst_address_length;
373
374       rv = BV(clib_bihash_search_inline_2)(&fib->ip6_lookup_table, &kv, &value);
375       if (rv == 0)
376         return value.value;
377     }
378
379   return 0;
380 }
381
382 static void
383 compute_prefix_lengths_in_search_order (ip6_src_fib_t * fib)
384 {
385   int i;
386   vec_reset_length(fib->prefix_lengths_in_search_order);
387   /* Note: bitmap reversed so this is in fact a longest prefix match */
388   clib_bitmap_foreach(i, fib->non_empty_dst_address_length_bitmap, ({
389     int dst_address_length = 128 - i;
390     vec_add1 (fib->prefix_lengths_in_search_order, dst_address_length);
391   }));
392 }
393
394 /* Rewrite of ip6_add_del_route() because it uses im6 to find the fib */
395 static void
396 ip6_sd_fib_add_del_src_route (lisp_gpe_main_t * lgm,
397                               ip6_add_del_route_args_t * a)
398 {
399   ip_lookup_main_t * lm = lgm->lm6;
400   ip6_src_fib_t * fib;
401   ip6_address_t dst_address;
402   u32 dst_address_length, adj_index;
403   uword is_del;
404   u32 old_adj_index = ~0;
405   BVT(clib_bihash_kv) kv, value;
406
407   vlib_smp_unsafe_warning();
408
409   is_del = (a->flags & IP6_ROUTE_FLAG_DEL) != 0;
410
411   /* Either create new adjacency or use given one depending on arguments. */
412   if (a->n_add_adj > 0)
413     {
414       ip_add_adjacency (lm, a->add_adj, a->n_add_adj, &adj_index);
415     }
416   else
417     adj_index = a->adj_index;
418
419   dst_address = a->dst_address;
420   dst_address_length = a->dst_address_length;
421   fib = pool_elt_at_index(lgm->ip6_src_fibs, a->table_index_or_table_id);
422
423   ASSERT (dst_address_length < ARRAY_LEN (fib->fib_masks));
424   ip6_address_mask (&dst_address, &fib->fib_masks[dst_address_length]);
425
426   /* refcount accounting */
427   if (is_del)
428     {
429       ASSERT(fib->dst_address_length_refcounts[dst_address_length] > 0);
430       if (--fib->dst_address_length_refcounts[dst_address_length] == 0)
431         {
432           fib->non_empty_dst_address_length_bitmap = clib_bitmap_set (
433               fib->non_empty_dst_address_length_bitmap,
434               128 - dst_address_length, 0);
435           compute_prefix_lengths_in_search_order (fib);
436         }
437     }
438   else
439     {
440       fib->dst_address_length_refcounts[dst_address_length]++;
441
442       fib->non_empty_dst_address_length_bitmap =
443         clib_bitmap_set (fib->non_empty_dst_address_length_bitmap,
444                              128 - dst_address_length, 1);
445       compute_prefix_lengths_in_search_order (fib);
446     }
447
448   kv.key[0] = dst_address.as_u64[0];
449   kv.key[1] = dst_address.as_u64[1];
450   kv.key[2] = dst_address_length;
451
452   if (BV(clib_bihash_search)(&fib->ip6_lookup_table, &kv, &value) == 0)
453     old_adj_index = value.value;
454
455   if (is_del)
456     BV(clib_bihash_add_del) (&fib->ip6_lookup_table, &kv, 0 /* is_add */);
457   else
458     {
459       /* Make sure adj index is valid. */
460       if (CLIB_DEBUG > 0)
461         (void) ip_get_adjacency (lm, adj_index);
462
463       kv.value = adj_index;
464
465       BV(clib_bihash_add_del) (&fib->ip6_lookup_table, &kv, 1 /* is_add */);
466     }
467
468   /* Avoid spurious reference count increments */
469   if (old_adj_index == adj_index
470       && !(a->flags & IP6_ROUTE_FLAG_KEEP_OLD_ADJACENCY))
471     {
472       ip_adjacency_t * adj = ip_get_adjacency (lm, adj_index);
473       if (adj->share_count > 0)
474         adj->share_count --;
475     }
476
477   /* Delete old adjacency index if present and changed. */
478   {
479     if (! (a->flags & IP6_ROUTE_FLAG_KEEP_OLD_ADJACENCY)
480         && old_adj_index != ~0
481         && old_adj_index != adj_index)
482       ip_del_adjacency (lm, old_adj_index);
483   }
484 }
485
486 static void
487 ip6_src_fib_init (ip6_src_fib_t * fib)
488 {
489   uword i;
490
491   for (i = 0; i < ARRAY_LEN (fib->fib_masks); i++)
492     {
493       u32 j, i0, i1;
494
495       i0 = i / 32;
496       i1 = i % 32;
497
498       for (j = 0; j < i0; j++)
499         fib->fib_masks[i].as_u32[j] = ~0;
500
501       if (i1)
502         fib->fib_masks[i].as_u32[i0] = clib_host_to_net_u32 (
503             pow2_mask (i1) << (32 - i1));
504     }
505
506   if (fib->lookup_table_nbuckets == 0)
507     fib->lookup_table_nbuckets = IP6_FIB_DEFAULT_HASH_NUM_BUCKETS;
508
509   fib->lookup_table_nbuckets = 1 << max_log2 (fib->lookup_table_nbuckets);
510
511   if (fib->lookup_table_size == 0)
512     fib->lookup_table_size = IP6_FIB_DEFAULT_HASH_MEMORY_SIZE;
513
514   BV(clib_bihash_init) (&fib->ip6_lookup_table, "ip6 lookup table",
515                         fib->lookup_table_nbuckets,
516                         fib->lookup_table_size);
517
518 }
519
520 static int
521 ip6_sd_fib_add_del_route (lisp_gpe_main_t * lgm, ip_prefix_t * dst_prefix,
522                           ip_prefix_t * src_prefix, u32 table_id,
523                           ip_adjacency_t * add_adj, u8 is_add)
524 {
525   u32 adj_index;
526   ip6_add_del_route_args_t a;
527   ip_adjacency_t * dst_adjp, dst_adj;
528   ip6_address_t dst = ip_prefix_v6(dst_prefix), src;
529   u32 dst_address_length = ip_prefix_len(dst_prefix), src_address_length = 0;
530   ip6_src_fib_t * src_fib;
531
532   if (src_prefix)
533     {
534       src = ip_prefix_v6(src_prefix);
535       src_address_length = ip_prefix_len(src_prefix);
536     }
537   else
538     memset(&src, 0, sizeof(src));
539
540   /* lookup dst adj and create it if it doesn't exist */
541   adj_index = ip6_get_route (lgm->im6, table_id, 0, &dst, dst_address_length);
542
543   if (is_add)
544     {
545       /* insert dst prefix to ip6 fib, if it's not in yet */
546       if (adj_index == 0)
547         {
548
549           /* allocate and init src ip6 fib */
550           pool_get(lgm->ip6_src_fibs, src_fib);
551           memset(src_fib, 0, sizeof(src_fib[0]));
552           ip6_src_fib_init (src_fib);
553
554           memset(&dst_adj, 0, sizeof(dst_adj));
555
556           /* reuse rewrite header to store pointer to src fib */
557           dst_adj.rewrite_header.sw_if_index = src_fib - lgm->ip6_src_fibs;
558
559           /* dst adj should point to lisp gpe ip lookup */
560           dst_adj.lookup_next_index = lgm->ip6_lookup_next_lgpe_ip6_lookup;
561
562           memset(&a, 0, sizeof(a));
563           a.flags = IP6_ROUTE_FLAG_TABLE_ID;
564           a.table_index_or_table_id = table_id; /* vrf */
565           a.adj_index = ~0;
566           a.dst_address_length = dst_address_length;
567           a.dst_address = dst;
568           a.flags |= IP6_ROUTE_FLAG_ADD;
569           a.add_adj = &dst_adj;
570           a.n_add_adj = 1;
571
572           ip6_add_del_route (lgm->im6, &a);
573
574           /* lookup dst adj to obtain the adj index */
575           adj_index = ip6_get_route (lgm->im6, table_id, 0, &dst,
576                                      dst_address_length);
577
578           ASSERT(adj_index != 0);
579         }
580     }
581   else
582     {
583       if (adj_index == 0)
584         {
585           clib_warning("Trying to delete inexistent dst route for %U. Aborting",
586                        format_ip6_address_and_length, dst.as_u8,
587                        dst_address_length);
588           return -1;
589         }
590     }
591
592   dst_adjp = ip_get_adjacency (lgm->lm6, adj_index);
593
594   /* add/del src prefix to src fib */
595   memset(&a, 0, sizeof(a));
596   a.flags = IP6_ROUTE_FLAG_TABLE_ID;
597   a.table_index_or_table_id = dst_adjp->rewrite_header.sw_if_index;
598   a.adj_index = ~0;
599   a.flags |= is_add ? IP6_ROUTE_FLAG_ADD : IP6_ROUTE_FLAG_DEL;
600   a.add_adj = add_adj;
601   a.n_add_adj = is_add ? 1 : 0;
602   /* if src prefix is null, add ::0 */
603   a.dst_address_length = src_address_length;
604   a.dst_address = src;
605   ip6_sd_fib_add_del_src_route (lgm, &a);
606
607   /* if a delete, check if there are elements left in the src fib */
608   if (!is_add)
609     {
610       src_fib = pool_elt_at_index(lgm->ip6_src_fibs,
611                                   dst_adjp->rewrite_header.sw_if_index);
612       if (!src_fib)
613         return 0;
614
615       /* if there's nothing left */
616       if (clib_bitmap_count_set_bits (
617           src_fib->non_empty_dst_address_length_bitmap) == 0)
618         {
619           /* remove src fib .. */
620           pool_put(lgm->ip6_src_fibs, src_fib);
621
622           /* .. and remove dst route */
623           memset(&a, 0, sizeof(a));
624           a.flags = IP6_ROUTE_FLAG_TABLE_ID;
625           a.table_index_or_table_id = table_id; /* vrf */
626           a.adj_index = ~0;
627           a.dst_address_length = dst_address_length;
628           a.dst_address = dst;
629           a.flags |= IP6_ROUTE_FLAG_DEL;
630
631           ip6_add_del_route (lgm->im6, &a);
632         }
633     }
634
635   return 0;
636 }
637
638 static u32
639 ip6_sd_fib_get_route (lisp_gpe_main_t * lgm, ip_prefix_t * dst_prefix,
640                       ip_prefix_t * src_prefix, u32 table_id)
641 {
642   u32 adj_index;
643   ip6_address_t dst = ip_prefix_v6(dst_prefix), src;
644   u32 dst_address_length = ip_prefix_len(dst_prefix), src_address_length = 0;
645   ip_adjacency_t * dst_adj;
646
647   if (src_prefix)
648     {
649       src = ip_prefix_v6(src_prefix);
650       src_address_length = ip_prefix_len(src_prefix);
651     }
652   else
653     memset(&src, 0, sizeof(src));
654
655   /* lookup dst adj */
656   adj_index = ip6_get_route (lgm->im6, table_id, 0, &dst, dst_address_length);
657   if (adj_index == 0)
658       return adj_index;
659
660   dst_adj = ip_get_adjacency (lgm->lm6, adj_index);
661   return ip6_sd_get_src_route (lgm, dst_adj->rewrite_header.sw_if_index, &src,
662                                src_address_length);
663 }
664
665 int
666 ip_sd_fib_add_del_route (lisp_gpe_main_t * lgm, ip_prefix_t * dst_prefix,
667                          ip_prefix_t * src_prefix, u32 table_id,
668                          ip_adjacency_t * add_adj, u8 is_add)
669 {
670   return (
671   ip_prefix_version(dst_prefix) == IP4 ?
672       ip4_sd_fib_add_del_route : ip6_sd_fib_add_del_route) (lgm, dst_prefix,
673                                                             src_prefix,
674                                                             table_id, add_adj,
675                                                             is_add);
676 }
677
678 u32
679 ip_sd_fib_get_route (lisp_gpe_main_t * lgm, ip_prefix_t * dst_prefix,
680                      ip_prefix_t * src_prefix, u32 table_id)
681 {
682   if (ip_prefix_version(dst_prefix) == IP4)
683     {
684       u32 * adj_index = ip4_sd_fib_get_route (lgm, dst_prefix, src_prefix,
685                                               table_id);
686       return (adj_index == 0) ? 0 : adj_index[0];
687     }
688   else
689     return ip6_sd_fib_get_route (lgm, dst_prefix, src_prefix, table_id);
690 }
691
692 always_inline void
693 ip4_src_fib_lookup_one (lisp_gpe_main_t * lgm, u32 src_fib_index0,
694                         ip4_address_t * addr0, u32 * src_adj_index0)
695 {
696   ip4_fib_mtrie_leaf_t leaf0, leaf1;
697   ip4_fib_mtrie_t * mtrie0;
698
699   mtrie0 = &vec_elt_at_index(lgm->ip4_src_fibs, src_fib_index0)->mtrie;
700
701   leaf0 = leaf1 = IP4_FIB_MTRIE_LEAF_ROOT;
702   leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 0);
703   leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 1);
704   leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 2);
705   leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 3);
706
707   /* Handle default route. */
708   leaf0 = (leaf0 == IP4_FIB_MTRIE_LEAF_EMPTY ? mtrie0->default_leaf : leaf0);
709   src_adj_index0[0] = ip4_fib_mtrie_leaf_get_adj_index (leaf0);
710 }
711
712 always_inline void
713 ip4_src_fib_lookup_two (lisp_gpe_main_t * lgm, u32 src_fib_index0,
714                         u32 src_fib_index1, ip4_address_t * addr0,
715                         ip4_address_t * addr1, u32 * src_adj_index0,
716                         u32 * src_adj_index1)
717 {
718   ip4_fib_mtrie_leaf_t leaf0, leaf1;
719   ip4_fib_mtrie_t * mtrie0, * mtrie1;
720
721   mtrie0 = &vec_elt_at_index(lgm->ip4_src_fibs, src_fib_index0)->mtrie;
722   mtrie1 = &vec_elt_at_index(lgm->ip4_src_fibs, src_fib_index1)->mtrie;
723
724   leaf0 = leaf1 = IP4_FIB_MTRIE_LEAF_ROOT;
725
726   leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 0);
727   leaf1 = ip4_fib_mtrie_lookup_step (mtrie1, leaf1, addr1, 0);
728
729   leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 1);
730   leaf1 = ip4_fib_mtrie_lookup_step (mtrie1, leaf1, addr1, 1);
731
732   leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 2);
733   leaf1 = ip4_fib_mtrie_lookup_step (mtrie1, leaf1, addr1, 2);
734
735   leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 3);
736   leaf1 = ip4_fib_mtrie_lookup_step (mtrie1, leaf1, addr1, 3);
737
738   /* Handle default route. */
739   leaf0 = (leaf0 == IP4_FIB_MTRIE_LEAF_EMPTY ? mtrie0->default_leaf : leaf0);
740   leaf1 = (leaf1 == IP4_FIB_MTRIE_LEAF_EMPTY ? mtrie1->default_leaf : leaf1);
741   src_adj_index0[0] = ip4_fib_mtrie_leaf_get_adj_index (leaf0);
742   src_adj_index1[0] = ip4_fib_mtrie_leaf_get_adj_index (leaf1);
743 }
744
745 always_inline uword
746 lgpe_ip4_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
747                  vlib_frame_t * from_frame)
748 {
749   u32 n_left_from, next_index, * from, * to_next;
750   lisp_gpe_main_t * lgm = &lisp_gpe_main;
751
752   from = vlib_frame_vector_args (from_frame);
753   n_left_from = from_frame->n_vectors;
754
755   next_index = node->cached_next_index;
756
757   while (n_left_from > 0)
758     {
759       u32 n_left_to_next;
760
761       vlib_get_next_frame(vm, node, next_index, to_next, n_left_to_next);
762
763       while (n_left_from >= 4 && n_left_to_next >= 2)
764         {
765           u32 bi0, bi1;
766           vlib_buffer_t * b0, * b1;
767           ip4_header_t * ip0, * ip1;
768           u32 dst_adj_index0, src_adj_index0, src_fib_index0, dst_adj_index1,
769               src_adj_index1, src_fib_index1;
770           ip_adjacency_t * dst_adj0, * src_adj0, * dst_adj1, * src_adj1;
771           u32 next0, next1;
772
773           next0 = next1 = LGPE_IP4_LOOKUP_NEXT_LISP_CP_LOOKUP;
774
775           /* Prefetch next iteration. */
776           {
777             vlib_buffer_t * p2, * p3;
778
779             p2 = vlib_get_buffer (vm, from[2]);
780             p3 = vlib_get_buffer (vm, from[3]);
781
782             vlib_prefetch_buffer_header (p2, LOAD);
783             vlib_prefetch_buffer_header (p3, LOAD);
784
785             CLIB_PREFETCH (p2->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
786             CLIB_PREFETCH (p3->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
787           }
788
789           bi0 = from[0];
790           bi1 = from[1];
791           to_next[0] = bi0;
792           to_next[1] = bi1;
793           from += 2;
794           to_next += 2;
795           n_left_to_next -= 2;
796           n_left_from -= 2;
797
798           b0 = vlib_get_buffer (vm, bi0);
799           b1 = vlib_get_buffer (vm, bi1);
800
801           ip0 = vlib_buffer_get_current (b0);
802           ip1 = vlib_buffer_get_current (b1);
803
804           /* dst lookup was done by ip4 lookup */
805           dst_adj_index0 = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
806           dst_adj_index1 = vnet_buffer(b1)->ip.adj_index[VLIB_TX];
807
808           dst_adj0 = ip_get_adjacency (lgm->lm4, dst_adj_index0);
809           dst_adj1 = ip_get_adjacency (lgm->lm4, dst_adj_index1);
810
811           src_fib_index0 = dst_adj0->rewrite_header.sw_if_index;
812           src_fib_index1 = dst_adj1->rewrite_header.sw_if_index;
813
814           /* if default route not hit in ip4 lookup */
815           if (PREDICT_TRUE(src_fib_index0 != (u32) ~0
816                            && src_fib_index1 != (u32) ~0))
817             {
818               ip4_src_fib_lookup_two (lgm, src_fib_index0, src_fib_index1,
819                                       &ip0->src_address, &ip1->src_address,
820                                       &src_adj_index0, &src_adj_index1);
821
822               vnet_buffer(b0)->ip.adj_index[VLIB_TX] = src_adj_index0;
823               vnet_buffer(b1)->ip.adj_index[VLIB_TX] = src_adj_index1;
824
825               src_adj0 = ip_get_adjacency (lgm->lm4, src_adj_index0);
826               src_adj1 = ip_get_adjacency (lgm->lm4, src_adj_index1);
827
828               next0 = src_adj0->lookup_next_index;
829               next1 = src_adj1->lookup_next_index;
830
831               /* prepare buffer for lisp-gpe output node */
832               vnet_buffer (b0)->sw_if_index[VLIB_TX] =
833                   src_adj0->rewrite_header.sw_if_index;
834               vnet_buffer (b1)->sw_if_index[VLIB_TX] =
835                   src_adj1->rewrite_header.sw_if_index;
836             }
837           else
838             {
839               if (src_fib_index0 != (u32) ~0)
840                 {
841                   ip4_src_fib_lookup_one (lgm, src_fib_index0,
842                                           &ip0->src_address, &src_adj_index0);
843                   vnet_buffer(b0)->ip.adj_index[VLIB_TX] = src_adj_index0;
844                   src_adj0 = ip_get_adjacency (lgm->lm4, src_adj_index0);
845                   next0 = src_adj0->lookup_next_index;
846                   vnet_buffer (b0)->sw_if_index[VLIB_TX] =
847                       src_adj0->rewrite_header.sw_if_index;
848                 }
849               if (src_fib_index1 != (u32) ~0)
850                 {
851                   ip4_src_fib_lookup_one (lgm, src_fib_index1,
852                                           &ip1->src_address, &src_adj_index1);
853                   vnet_buffer(b1)->ip.adj_index[VLIB_TX] = src_adj_index1;
854                   src_adj1 = ip_get_adjacency (lgm->lm4, src_adj_index1);
855                   next1 = src_adj1->lookup_next_index;
856                   vnet_buffer (b1)->sw_if_index[VLIB_TX] =
857                       src_adj1->rewrite_header.sw_if_index;
858                 }
859             }
860
861           vlib_validate_buffer_enqueue_x2(vm, node, next_index, to_next,
862                                           n_left_to_next, bi0, bi1, next0,
863                                           next1);
864         }
865
866       while (n_left_from > 0 && n_left_to_next > 0)
867         {
868           vlib_buffer_t * b0;
869           ip4_header_t * ip0;
870           u32 bi0, dst_adj_index0, src_adj_index0, src_fib_index0;
871           u32 next0 = LGPE_IP4_LOOKUP_NEXT_LISP_CP_LOOKUP;
872           ip_adjacency_t * dst_adj0, * src_adj0;
873
874           bi0 = from[0];
875           to_next[0] = bi0;
876           from += 1;
877           to_next += 1;
878           n_left_from -= 1;
879           n_left_to_next -= 1;
880
881           b0 = vlib_get_buffer (vm, bi0);
882           ip0 = vlib_buffer_get_current (b0);
883
884           /* dst lookup was done by ip4 lookup */
885           dst_adj_index0 = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
886           dst_adj0 = ip_get_adjacency (lgm->lm4, dst_adj_index0);
887           src_fib_index0 = dst_adj0->rewrite_header.sw_if_index;
888
889           /* if default route not hit in ip4 lookup */
890           if (PREDICT_TRUE(src_fib_index0 != (u32 ) ~0))
891             {
892               /* do src lookup */
893               ip4_src_fib_lookup_one (lgm, src_fib_index0, &ip0->src_address,
894                                       &src_adj_index0);
895               vnet_buffer(b0)->ip.adj_index[VLIB_TX] = src_adj_index0;
896               src_adj0 = ip_get_adjacency (lgm->lm4, src_adj_index0);
897               next0 = src_adj0->lookup_next_index;
898
899               /* prepare packet for lisp-gpe output node */
900               vnet_buffer (b0)->sw_if_index[VLIB_TX] =
901                   src_adj0->rewrite_header.sw_if_index;
902             }
903
904           vlib_validate_buffer_enqueue_x1(vm, node, next_index, to_next,
905                                           n_left_to_next, bi0, next0);
906         }
907       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
908     }
909   return from_frame->n_vectors;
910 }
911
912 VLIB_REGISTER_NODE (lgpe_ip4_lookup_node) = {
913   .function = lgpe_ip4_lookup,
914   .name = "lgpe-ip4-lookup",
915   .vector_size = sizeof (u32),
916
917   .type = VLIB_NODE_TYPE_INTERNAL,
918
919   .n_next_nodes = LGPE_IP4_LOOKUP_N_NEXT,
920   .next_nodes = {
921 #define _(sym,str) [LGPE_IP4_LOOKUP_NEXT_##sym] = str,
922       foreach_lgpe_ip4_lookup_next
923 #undef _
924   },
925 };
926
927 static u32
928 ip6_src_fib_lookup (lisp_gpe_main_t * lgm, u32 src_fib_index,
929                     ip6_address_t * src)
930 {
931   int i, len;
932   int rv;
933   BVT(clib_bihash_kv) kv, value;
934   ip6_src_fib_t * fib = pool_elt_at_index (lgm->ip6_src_fibs, src_fib_index);
935
936   len = vec_len (fib->prefix_lengths_in_search_order);
937
938   for (i = 0; i < len; i++)
939     {
940       int dst_address_length = fib->prefix_lengths_in_search_order[i];
941       ip6_address_t * mask;
942
943       ASSERT(dst_address_length >= 0 && dst_address_length <= 128);
944
945       mask = &fib->fib_masks[dst_address_length];
946
947       kv.key[0] = src->as_u64[0] & mask->as_u64[0];
948       kv.key[1] = src->as_u64[1] & mask->as_u64[1];
949       kv.key[2] = dst_address_length;
950
951       rv = BV(clib_bihash_search_inline_2)(&fib->ip6_lookup_table, &kv, &value);
952       if (rv == 0)
953         return value.value;
954     }
955
956   return 0;
957 }
958
959 always_inline uword
960 lgpe_ip6_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
961                  vlib_frame_t * from_frame)
962 {
963   u32 n_left_from, next_index, * from, * to_next;
964   lisp_gpe_main_t * lgm = &lisp_gpe_main;
965
966   from = vlib_frame_vector_args (from_frame);
967   n_left_from = from_frame->n_vectors;
968
969   next_index = node->cached_next_index;
970
971   while (n_left_from > 0)
972     {
973       u32 n_left_to_next;
974
975       vlib_get_next_frame(vm, node, next_index, to_next, n_left_to_next);
976
977       while (n_left_from >= 4 && n_left_to_next >= 2)
978         {
979           u32 bi0, bi1;
980           vlib_buffer_t * b0, * b1;
981           ip6_header_t * ip0, * ip1;
982           u32 dst_adj_index0, src_adj_index0, src_fib_index0, dst_adj_index1,
983               src_adj_index1, src_fib_index1;
984           ip_adjacency_t * dst_adj0, * src_adj0, * dst_adj1, * src_adj1;
985           u32 next0, next1;
986
987           next0 = next1 = LGPE_IP6_LOOKUP_NEXT_LISP_CP_LOOKUP;
988
989           /* Prefetch next iteration. */
990           {
991             vlib_buffer_t * p2, * p3;
992
993             p2 = vlib_get_buffer (vm, from[2]);
994             p3 = vlib_get_buffer (vm, from[3]);
995
996             vlib_prefetch_buffer_header (p2, LOAD);
997             vlib_prefetch_buffer_header (p3, LOAD);
998
999             CLIB_PREFETCH (p2->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
1000             CLIB_PREFETCH (p3->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
1001           }
1002
1003           bi0 = from[0];
1004           bi1 = from[1];
1005           to_next[0] = bi0;
1006           to_next[1] = bi1;
1007           from += 2;
1008           to_next += 2;
1009           n_left_to_next -= 2;
1010           n_left_from -= 2;
1011
1012           b0 = vlib_get_buffer (vm, bi0);
1013           b1 = vlib_get_buffer (vm, bi1);
1014
1015           ip0 = vlib_buffer_get_current (b0);
1016           ip1 = vlib_buffer_get_current (b1);
1017
1018           /* dst lookup was done by ip6 lookup */
1019           dst_adj_index0 = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
1020           dst_adj_index1 = vnet_buffer(b1)->ip.adj_index[VLIB_TX];
1021
1022           dst_adj0 = ip_get_adjacency (lgm->lm6, dst_adj_index0);
1023           dst_adj1 = ip_get_adjacency (lgm->lm6, dst_adj_index1);
1024
1025           src_fib_index0 = dst_adj0->rewrite_header.sw_if_index;
1026           src_fib_index1 = dst_adj1->rewrite_header.sw_if_index;
1027
1028           /* if default route not hit in ip6 lookup */
1029           if (PREDICT_TRUE(src_fib_index0 != (u32) ~0
1030                            && src_fib_index1 != (u32) ~0))
1031             {
1032               /* do src lookup */
1033               src_adj_index0 = ip6_src_fib_lookup (lgm, src_fib_index0,
1034                                                    &ip0->src_address);
1035               src_adj_index1 = ip6_src_fib_lookup (lgm, src_fib_index1,
1036                                                    &ip1->src_address);
1037
1038               vnet_buffer(b0)->ip.adj_index[VLIB_TX] = src_adj_index0;
1039               vnet_buffer(b1)->ip.adj_index[VLIB_TX] = src_adj_index1;
1040
1041               src_adj0 = ip_get_adjacency (lgm->lm6, src_adj_index0);
1042               src_adj1 = ip_get_adjacency (lgm->lm6, src_adj_index1);
1043
1044               next0 = src_adj0->lookup_next_index;
1045               next1 = src_adj1->lookup_next_index;
1046
1047               /* prepare buffer for lisp-gpe output node */
1048               vnet_buffer (b0)->sw_if_index[VLIB_TX] =
1049                   src_adj0->rewrite_header.sw_if_index;
1050               vnet_buffer (b1)->sw_if_index[VLIB_TX] =
1051                   src_adj1->rewrite_header.sw_if_index;
1052             }
1053           else
1054             {
1055               if (src_fib_index0 != (u32) ~0)
1056                 {
1057                   src_adj_index0 = ip6_src_fib_lookup (lgm, src_fib_index0,
1058                                                        &ip0->src_address);
1059                   vnet_buffer(b0)->ip.adj_index[VLIB_TX] = src_adj_index0;
1060                   src_adj0 = ip_get_adjacency (lgm->lm6, src_adj_index0);
1061                   next0 = src_adj0->lookup_next_index;
1062                   vnet_buffer (b0)->sw_if_index[VLIB_TX] =
1063                       src_adj0->rewrite_header.sw_if_index;
1064                 }
1065               if (src_fib_index1 != (u32) ~0)
1066                 {
1067                   src_adj_index1 = ip6_src_fib_lookup (lgm, src_fib_index1,
1068                                                        &ip1->src_address);
1069                   vnet_buffer(b1)->ip.adj_index[VLIB_TX] = src_adj_index1;
1070                   src_adj1 = ip_get_adjacency (lgm->lm6, src_adj_index1);
1071                   next1 = src_adj1->lookup_next_index;
1072                   vnet_buffer (b1)->sw_if_index[VLIB_TX] =
1073                       src_adj1->rewrite_header.sw_if_index;
1074                 }
1075             }
1076
1077           vlib_validate_buffer_enqueue_x2(vm, node, next_index, to_next,
1078                                           n_left_to_next, bi0, bi1, next0,
1079                                           next1);
1080         }
1081
1082       while (n_left_from > 0 && n_left_to_next > 0)
1083         {
1084           vlib_buffer_t * b0;
1085           ip6_header_t * ip0;
1086           u32 bi0, dst_adj_index0, src_adj_index0, src_fib_index0;
1087           u32 next0 = LGPE_IP6_LOOKUP_NEXT_LISP_CP_LOOKUP;
1088           ip_adjacency_t * dst_adj0, * src_adj0;
1089
1090           bi0 = from[0];
1091           to_next[0] = bi0;
1092           from += 1;
1093           to_next += 1;
1094           n_left_from -= 1;
1095           n_left_to_next -= 1;
1096
1097           b0 = vlib_get_buffer (vm, bi0);
1098           ip0 = vlib_buffer_get_current (b0);
1099
1100           /* dst lookup was done by ip6 lookup */
1101           dst_adj_index0 = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
1102           dst_adj0 = ip_get_adjacency (lgm->lm6, dst_adj_index0);
1103           src_fib_index0 = dst_adj0->rewrite_header.sw_if_index;
1104
1105           /* if default route not hit in ip6 lookup */
1106           if (PREDICT_TRUE(src_fib_index0 != (u32 ) ~0))
1107             {
1108               /* do src lookup */
1109               src_adj_index0 = ip6_src_fib_lookup (lgm, src_fib_index0,
1110                                                    &ip0->src_address);
1111
1112               vnet_buffer(b0)->ip.adj_index[VLIB_TX] = src_adj_index0;
1113               src_adj0 = ip_get_adjacency (lgm->lm6, src_adj_index0);
1114               next0 = src_adj0->lookup_next_index;
1115
1116               /* prepare packet for lisp-gpe output node */
1117               vnet_buffer (b0)->sw_if_index[VLIB_TX] =
1118                   src_adj0->rewrite_header.sw_if_index;
1119             }
1120
1121           vlib_validate_buffer_enqueue_x1(vm, node, next_index, to_next,
1122                                           n_left_to_next, bi0, next0);
1123         }
1124       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1125     }
1126   return from_frame->n_vectors;
1127 }
1128
1129 VLIB_REGISTER_NODE (lgpe_ip6_lookup_node) = {
1130   .function = lgpe_ip6_lookup,
1131   .name = "lgpe-ip6-lookup",
1132   .vector_size = sizeof (u32),
1133
1134   .type = VLIB_NODE_TYPE_INTERNAL,
1135
1136   .n_next_nodes = LGPE_IP6_LOOKUP_N_NEXT,
1137   .next_nodes = {
1138 #define _(sym,str) [LGPE_IP6_LOOKUP_NEXT_##sym] = str,
1139       foreach_lgpe_ip6_lookup_next
1140 #undef _
1141   },
1142 };