f6e10f08d97ff5e57d7eb54ea238b586bda212a0
[vpp.git] / vnet / vnet / ip / ip6_hop_by_hop.c
1 /*
2  * Copyright (c) 2015 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 #include <vlib/vlib.h>
16 #include <vnet/vnet.h>
17 #include <vnet/pg/pg.h>
18 #include <vppinfra/error.h>
19
20 #include <vnet/ip/ip.h>
21
22 #include <vppinfra/hash.h>
23 #include <vppinfra/error.h>
24 #include <vppinfra/elog.h>
25
26 #include <vnet/ip/ip6_hop_by_hop.h>
27
28 /* Timestamp precision multipliers for seconds, milliseconds, microseconds
29  * and nanoseconds respectively.
30  */
31 static f64 trace_tsp_mul[4] = {1, 1e3, 1e6, 1e9};
32
33 char *ppc_state[] = {"None", "Encap", "Decap"};
34
35 ip6_hop_by_hop_ioam_main_t ip6_hop_by_hop_ioam_main;
36
37 #define foreach_ip6_hbyh_ioam_input_next        \
38   _(IP6_REWRITE, "ip6-rewrite")                 \
39   _(IP6_LOOKUP, "ip6-lookup")                   \
40   _(DROP, "error-drop")                        
41
42 typedef enum {
43 #define _(s,n) IP6_HBYH_IOAM_INPUT_NEXT_##s,
44   foreach_ip6_hbyh_ioam_input_next
45 #undef _
46   IP6_HBYH_IOAM_INPUT_N_NEXT,
47 } ip6_hbyh_ioam_input_next_t;
48
49 typedef union {
50     u64 as_u64;
51     u32 as_u32[2];
52 } time_u64_t;
53
54 static inline u8
55 fetch_trace_data_size(u8 trace_type)
56 {
57   u8 trace_data_size = 0;
58
59   if (trace_type == TRACE_TYPE_IF_TS_APP)   
60       trace_data_size = sizeof(ioam_trace_if_ts_app_t);
61   else if(trace_type == TRACE_TYPE_IF)      
62       trace_data_size = sizeof(ioam_trace_if_t);
63   else if(trace_type == TRACE_TYPE_TS)      
64       trace_data_size = sizeof(ioam_trace_ts_t);
65   else if(trace_type == TRACE_TYPE_APP)     
66       trace_data_size = sizeof(ioam_trace_app_t);
67   else if(trace_type == TRACE_TYPE_TS_APP)  
68       trace_data_size = sizeof(ioam_trace_ts_app_t);
69
70   return trace_data_size;
71 }
72
73 static u8 * format_ioam_data_list_element (u8 * s, va_list * args)
74
75   u32 *elt = va_arg (*args, u32 *);
76   u8  *trace_type_p = va_arg (*args, u8 *);
77   u8  trace_type = *trace_type_p;
78
79
80   if (trace_type & BIT_TTL_NODEID)
81     {
82       u32 ttl_node_id_host_byte_order = clib_net_to_host_u32 (*elt);
83       s = format (s, "ttl 0x%x node id 0x%x ",
84               ttl_node_id_host_byte_order>>24,
85               ttl_node_id_host_byte_order & 0x00FFFFFF);
86
87       elt++;
88     }
89  
90   if (trace_type & BIT_ING_INTERFACE && trace_type & BIT_ING_INTERFACE)
91     {
92         u32 ingress_host_byte_order = clib_net_to_host_u32(*elt);
93         s = format (s, "ingress 0x%x egress 0x%x ", 
94                    ingress_host_byte_order >> 16, 
95                    ingress_host_byte_order &0xFFFF);
96         elt++;
97     }
98  
99   if (trace_type & BIT_TIMESTAMP)
100     {
101         u32 ts_in_host_byte_order = clib_net_to_host_u32 (*elt);
102         s = format (s, "ts 0x%x \n", ts_in_host_byte_order);
103         elt++;
104     }
105  
106   if (trace_type & BIT_APPDATA)
107     {
108         u32 appdata_in_host_byte_order = clib_net_to_host_u32 (*elt);
109         s = format (s, "app 0x%x ", appdata_in_host_byte_order);
110         elt++;
111     }
112  
113   return s;
114 }
115
116 u8 *
117 ip6_hbh_ioam_trace_data_list_trace_handler (u8 *s, ip6_hop_by_hop_option_t *opt)
118 {
119   ioam_trace_option_t *trace;
120   u8 trace_data_size_in_words = 0;
121   u32 *elt;
122   int elt_index = 0;
123
124   trace = (ioam_trace_option_t *)opt;
125 #if 0
126   s = format (s, "  Trace Type 0x%x , %d elts left ts msb(s) 0x%x\n", trace->ioam_trace_type, trace->data_list_elts_left,
127               t->timestamp_msbs);
128 #endif
129   s = format (s, "  Trace Type 0x%x , %d elts left\n", trace->ioam_trace_type, trace->data_list_elts_left);
130   trace_data_size_in_words = fetch_trace_data_size(trace->ioam_trace_type)/4;
131   elt = &trace->elts[0];
132   while ((u8 *) elt < ((u8 *)(&trace->elts[0]) + trace->hdr.length - 2
133                         /* -2 accounts for ioam_trace_type,elts_left */)) {
134     s = format (s, "    [%d] %U\n",elt_index,
135                 format_ioam_data_list_element,
136                 elt, &trace->ioam_trace_type);
137     elt_index++;
138     elt += trace_data_size_in_words;
139   }
140   return (s);
141 }
142
143 int
144 ip6_hbh_ioam_trace_data_list_handler (vlib_buffer_t *b, ip6_header_t *ip, ip6_hop_by_hop_option_t *opt)
145 {
146   ip6_main_t * im = &ip6_main;
147   ip_lookup_main_t * lm = &im->lookup_main;
148   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
149   u8 elt_index = 0;
150   ioam_trace_option_t *trace = (ioam_trace_option_t *)opt;
151   u32 adj_index = vnet_buffer (b)->ip.adj_index[VLIB_TX];
152   ip_adjacency_t *adj = ip_get_adjacency (lm, adj_index);
153   time_u64_t time_u64;
154   u32 *elt;
155   int rv = 0;
156
157   time_u64.as_u64 = 0;
158
159   if (PREDICT_TRUE (trace->data_list_elts_left)) {
160     trace->data_list_elts_left--;
161     /* fetch_trace_data_size returns in bytes. Convert it to 4-bytes
162      * to skip to this node's location.
163      */
164     elt_index = trace->data_list_elts_left * fetch_trace_data_size(trace->ioam_trace_type) / 4;
165     elt = &trace->elts[elt_index];
166     if (trace->ioam_trace_type & BIT_TTL_NODEID) {
167       *elt = clib_host_to_net_u32 ((ip->hop_limit<<24) | hm->node_id);
168       elt++;
169     }
170
171     if (trace->ioam_trace_type & BIT_ING_INTERFACE) {
172       *elt = (vnet_buffer(b)->sw_if_index[VLIB_RX]&0xFFFF) << 16 | (adj->rewrite_header.sw_if_index & 0xFFFF);
173       *elt = clib_host_to_net_u32(*elt);
174       elt++;
175     }
176                  
177     if (trace->ioam_trace_type & BIT_TIMESTAMP) {
178       /* Send least significant 32 bits */
179       f64 time_f64 = (f64)(((f64)hm->unix_time_0) + (vlib_time_now(hm->vlib_main) - hm->vlib_time_0));
180
181       time_u64.as_u64 = time_f64 * trace_tsp_mul[hm->trace_tsp];
182       *elt = clib_host_to_net_u32(time_u64.as_u32[0]);
183       elt++;
184     }
185
186     if (trace->ioam_trace_type & BIT_APPDATA) {
187       /* $$$ set elt0->app_data */
188       *elt = clib_host_to_net_u32(hm->app_data);
189       elt++;
190     }
191   }
192   return (rv);
193 }
194
195 /* The main h-b-h tracer will be invoked, no need to do much here */
196 int
197 ip6_hbh_add_register_option (u8 option,
198                              u8 size,
199                              int rewrite_options(u8 *rewrite_string, u8 rewrite_size))
200 {
201   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
202
203   ASSERT (option < ARRAY_LEN (hm->add_options));
204
205   /* Already registered */
206   if (hm->add_options[option])
207     return (-1);
208
209   hm->add_options[option] = rewrite_options;
210   hm->options_size[option] = size;
211   
212   return (0);
213 }
214
215 int
216 ip6_hbh_add_unregister_option (u8 option)
217 {
218   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
219
220   ASSERT (option < ARRAY_LEN (hm->add_options));
221
222   /* Not registered */
223   if (!hm->add_options[option])
224     return (-1);
225
226   hm->add_options[option] = NULL;
227   hm->options_size[option] = 0;
228   return (0);
229 }
230
231 typedef struct {
232   u32 next_index;
233 } ip6_add_hop_by_hop_trace_t;
234
235 /* packet trace format function */
236 static u8 * format_ip6_add_hop_by_hop_trace (u8 * s, va_list * args)
237 {
238   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
239   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
240   ip6_add_hop_by_hop_trace_t * t = va_arg (*args, 
241                                             ip6_add_hop_by_hop_trace_t *);
242   
243   s = format (s, "IP6_ADD_HOP_BY_HOP: next index %d",
244               t->next_index);
245   return s;
246 }
247
248 vlib_node_registration_t ip6_add_hop_by_hop_node;
249
250 #define foreach_ip6_add_hop_by_hop_error \
251 _(PROCESSED, "Pkts w/ added ip6 hop-by-hop options")
252
253 typedef enum {
254 #define _(sym,str) IP6_ADD_HOP_BY_HOP_ERROR_##sym,
255   foreach_ip6_add_hop_by_hop_error
256 #undef _
257   IP6_ADD_HOP_BY_HOP_N_ERROR,
258 } ip6_add_hop_by_hop_error_t;
259
260 static char * ip6_add_hop_by_hop_error_strings[] = {
261 #define _(sym,string) string,
262   foreach_ip6_add_hop_by_hop_error
263 #undef _
264 };
265
266 static uword
267 ip6_add_hop_by_hop_node_fn (vlib_main_t * vm,
268                   vlib_node_runtime_t * node,
269                   vlib_frame_t * frame)
270 {
271   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
272   u32 n_left_from, * from, * to_next;
273   ip_lookup_next_t next_index;
274   u32 processed = 0;
275   u8 * rewrite = hm->rewrite;
276   u32 rewrite_length = vec_len (rewrite);
277
278   from = vlib_frame_vector_args (frame);
279   n_left_from = frame->n_vectors;
280   next_index = node->cached_next_index;
281
282   while (n_left_from > 0)
283     {
284       u32 n_left_to_next;
285
286       vlib_get_next_frame (vm, node, next_index,
287                            to_next, n_left_to_next);
288
289 #if 0
290       while (n_left_from >= 4 && n_left_to_next >= 2)
291         {
292           u32 next0 = IP6_ADD_HOP_BY_HOP_NEXT_INTERFACE_OUTPUT;
293           u32 next1 = IP6_ADD_HOP_BY_HOP_NEXT_INTERFACE_OUTPUT;
294           u32 sw_if_index0, sw_if_index1;
295           u8 tmp0[6], tmp1[6];
296           ethernet_header_t *en0, *en1;
297           u32 bi0, bi1;
298           vlib_buffer_t * b0, * b1;
299           
300           /* Prefetch next iteration. */
301           {
302             vlib_buffer_t * p2, * p3;
303             
304             p2 = vlib_get_buffer (vm, from[2]);
305             p3 = vlib_get_buffer (vm, from[3]);
306             
307             vlib_prefetch_buffer_header (p2, LOAD);
308             vlib_prefetch_buffer_header (p3, LOAD);
309
310             CLIB_PREFETCH (p2->data, CLIB_CACHE_LINE_BYTES, STORE);
311             CLIB_PREFETCH (p3->data, CLIB_CACHE_LINE_BYTES, STORE);
312           }
313
314           /* speculatively enqueue b0 and b1 to the current next frame */
315           to_next[0] = bi0 = from[0];
316           to_next[1] = bi1 = from[1];
317           from += 2;
318           to_next += 2;
319           n_left_from -= 2;
320           n_left_to_next -= 2;
321
322           b0 = vlib_get_buffer (vm, bi0);
323           b1 = vlib_get_buffer (vm, bi1);
324
325           /* $$$$$ Dual loop: process 2 x packets here $$$$$ */
326           ASSERT (b0->current_data == 0);
327           ASSERT (b1->current_data == 0);
328           
329           ip0 = vlib_buffer_get_current (b0);
330           ip1 = vlib_buffer_get_current (b0);
331
332           sw_if_index0 = vnet_buffer(b0)->sw_if_index[VLIB_RX];
333           sw_if_index1 = vnet_buffer(b1)->sw_if_index[VLIB_RX];
334
335           /* $$$$$ End of processing 2 x packets $$$$$ */
336
337           if (PREDICT_FALSE((node->flags & VLIB_NODE_FLAG_TRACE)))
338             {
339               if (b0->flags & VLIB_BUFFER_IS_TRACED) 
340                 {
341                     ip6_add_hop_by_hop_trace_t *t = 
342                       vlib_add_trace (vm, node, b0, sizeof (*t));
343                     t->sw_if_index = sw_if_index0;
344                     t->next_index = next0;
345                   }
346                 if (b1->flags & VLIB_BUFFER_IS_TRACED) 
347                   {
348                     ip6_add_hop_by_hop_trace_t *t = 
349                       vlib_add_trace (vm, node, b1, sizeof (*t));
350                     t->sw_if_index = sw_if_index1;
351                     t->next_index = next1;
352                   }
353               }
354             
355             /* verify speculative enqueues, maybe switch current next frame */
356             vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
357                                              to_next, n_left_to_next,
358                                              bi0, bi1, next0, next1);
359         }
360 #endif
361
362       while (n_left_from > 0 && n_left_to_next > 0)
363         {
364           u32 bi0;
365           vlib_buffer_t * b0;
366           u32 next0;
367           ip6_header_t * ip0;
368           ip6_hop_by_hop_header_t * hbh0;
369           u64 * copy_src0, * copy_dst0;
370           u16 new_l0;
371           
372           /* speculatively enqueue b0 to the current next frame */
373           bi0 = from[0];
374           to_next[0] = bi0;
375           from += 1;
376           to_next += 1;
377           n_left_from -= 1;
378           n_left_to_next -= 1;
379
380           b0 = vlib_get_buffer (vm, bi0);
381
382           ip0 = vlib_buffer_get_current (b0);
383
384           /* Copy the ip header left by the required amount */
385           copy_dst0 = (u64 *)(((u8 *)ip0) - rewrite_length);
386           copy_src0 = (u64 *) ip0;
387
388           copy_dst0 [0] = copy_src0 [0];
389           copy_dst0 [1] = copy_src0 [1];
390           copy_dst0 [2] = copy_src0 [2];
391           copy_dst0 [3] = copy_src0 [3];
392           copy_dst0 [4] = copy_src0 [4];
393           vlib_buffer_advance (b0, - (word)rewrite_length);
394           ip0 = vlib_buffer_get_current (b0);
395
396           hbh0 = (ip6_hop_by_hop_header_t *)(ip0 + 1);
397           /* $$$ tune, rewrite_length is a multiple of 8 */
398           clib_memcpy (hbh0, rewrite, rewrite_length);
399           /* Patch the protocol chain, insert the h-b-h (type 0) header */
400           hbh0->protocol = ip0->protocol;
401           ip0->protocol = 0;
402           new_l0 = clib_net_to_host_u16 (ip0->payload_length) + rewrite_length;
403           ip0->payload_length = clib_host_to_net_u16 (new_l0);
404           
405           /* Populate the (first) h-b-h list elt */
406           next0 = IP6_HBYH_IOAM_INPUT_NEXT_IP6_LOOKUP;
407
408           if (PREDICT_FALSE((node->flags & VLIB_NODE_FLAG_TRACE) 
409                             && (b0->flags & VLIB_BUFFER_IS_TRACED))) 
410             {
411               ip6_add_hop_by_hop_trace_t *t = 
412                  vlib_add_trace (vm, node, b0, sizeof (*t));
413               t->next_index = next0;
414             }
415             
416           processed++;
417
418           /* verify speculative enqueue, maybe switch current next frame */
419           vlib_validate_buffer_enqueue_x1 (vm, node, next_index,
420                                            to_next, n_left_to_next,
421                                            bi0, next0);
422         }
423
424       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
425     }
426
427   vlib_node_increment_counter (vm, ip6_add_hop_by_hop_node.index, 
428                                IP6_ADD_HOP_BY_HOP_ERROR_PROCESSED, processed);
429   return frame->n_vectors;
430 }
431
432 VLIB_REGISTER_NODE (ip6_add_hop_by_hop_node) = {
433   .function = ip6_add_hop_by_hop_node_fn,
434   .name = "ip6-add-hop-by-hop",
435   .vector_size = sizeof (u32),
436   .format_trace = format_ip6_add_hop_by_hop_trace,
437   .type = VLIB_NODE_TYPE_INTERNAL,
438   
439   .n_errors = ARRAY_LEN(ip6_add_hop_by_hop_error_strings),
440   .error_strings = ip6_add_hop_by_hop_error_strings,
441
442   /* See ip/lookup.h */
443   .n_next_nodes = IP6_HBYH_IOAM_INPUT_N_NEXT,
444   .next_nodes = {
445 #define _(s,n) [IP6_HBYH_IOAM_INPUT_NEXT_##s] = n,
446     foreach_ip6_hbyh_ioam_input_next
447 #undef _
448   },
449 };
450
451 VLIB_NODE_FUNCTION_MULTIARCH (ip6_add_hop_by_hop_node, ip6_add_hop_by_hop_node_fn)
452
453 /* The main h-b-h tracer was already invoked, no need to do much here */
454 typedef struct {
455   u32 next_index;
456 } ip6_pop_hop_by_hop_trace_t;
457
458 /* packet trace format function */
459 static u8 * format_ip6_pop_hop_by_hop_trace (u8 * s, va_list * args)
460 {
461   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
462   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
463   ip6_pop_hop_by_hop_trace_t * t = va_arg (*args, ip6_pop_hop_by_hop_trace_t *);
464   
465   s = format (s, "IP6_POP_HOP_BY_HOP: next index %d",
466               t->next_index);
467   return s;
468 }
469
470 int
471 ip6_hbh_pop_register_option (u8 option,
472                              int options(ip6_header_t *ip, ip6_hop_by_hop_option_t *opt))
473 {
474   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
475
476   ASSERT (option < ARRAY_LEN (hm->pop_options));
477
478   /* Already registered */
479   if (hm->pop_options[option])
480     return (-1);
481
482   hm->pop_options[option] = options;
483
484   return (0);
485 }
486
487 int
488 ip6_hbh_pop_unregister_option (u8 option)
489 {
490   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
491
492   ASSERT (option < ARRAY_LEN (hm->pop_options));
493
494   /* Not registered */
495   if (!hm->pop_options[option])
496     return (-1);
497
498   hm->pop_options[option] = NULL;
499   return (0);
500 }
501
502 vlib_node_registration_t ip6_pop_hop_by_hop_node;
503
504 #define foreach_ip6_pop_hop_by_hop_error                \
505 _(PROCESSED, "Pkts w/ removed ip6 hop-by-hop options")  \
506 _(NO_HOHO, "Pkts w/ no ip6 hop-by-hop options")         \
507 _(OPTION_FAILED, "ip6 pop hop-by-hop failed to process")
508
509 typedef enum {
510 #define _(sym,str) IP6_POP_HOP_BY_HOP_ERROR_##sym,
511   foreach_ip6_pop_hop_by_hop_error
512 #undef _
513   IP6_POP_HOP_BY_HOP_N_ERROR,
514 } ip6_pop_hop_by_hop_error_t;
515
516 static char * ip6_pop_hop_by_hop_error_strings[] = {
517 #define _(sym,string) string,
518   foreach_ip6_pop_hop_by_hop_error
519 #undef _
520 };
521
522 static inline void ioam_pop_hop_by_hop_processing (vlib_main_t * vm,
523                                                 ip6_header_t *ip0,
524                                                 ip6_hop_by_hop_header_t *hbh0)
525 {
526   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
527   ip6_hop_by_hop_option_t *opt0, *limit0;
528   u8 type0;
529
530   if (!hbh0 || !ip0) return;
531
532   opt0 = (ip6_hop_by_hop_option_t *)(hbh0+1);
533   limit0 = (ip6_hop_by_hop_option_t *)
534     ((u8 *)hbh0 + ((hbh0->length+1)<<3));
535
536   /* Scan the set of h-b-h options, process ones that we understand */
537   while (opt0 < limit0)
538     {
539       type0 = opt0->type;
540       switch (type0)
541         {
542         case 0: /* Pad1 */
543           opt0 = (ip6_hop_by_hop_option_t *) ((u8 *)opt0) + 1;
544           continue;
545         case 1: /* PadN */
546           break;
547         default:
548           if (hm->pop_options[type0])
549             {
550               if ((*hm->pop_options[type0])(ip0, opt0) < 0)
551               {
552                 vlib_node_increment_counter (vm, ip6_pop_hop_by_hop_node.index, 
553                                IP6_POP_HOP_BY_HOP_ERROR_OPTION_FAILED, 1);
554               }
555             }
556         }
557         opt0 = (ip6_hop_by_hop_option_t *) (((u8 *)opt0) + opt0->length + sizeof (ip6_hop_by_hop_option_t));
558     }
559 }
560
561 static uword
562 ip6_pop_hop_by_hop_node_fn (vlib_main_t * vm,
563                   vlib_node_runtime_t * node,
564                   vlib_frame_t * frame)
565 {
566   ip6_main_t * im = &ip6_main;
567   ip_lookup_main_t * lm = &im->lookup_main;
568   u32 n_left_from, * from, * to_next;
569   ip_lookup_next_t next_index;
570   u32 processed = 0;
571   u32 no_header = 0;
572   
573   from = vlib_frame_vector_args (frame);
574   n_left_from = frame->n_vectors;
575   next_index = node->cached_next_index;
576   
577   while (n_left_from > 0)
578     {
579       u32 n_left_to_next;
580
581       vlib_get_next_frame (vm, node, next_index,
582                            to_next, n_left_to_next);
583
584 #if 0
585       while (n_left_from >= 4 && n_left_to_next >= 2)
586         {
587           u32 next0 = IP6_POP_HOP_BY_HOP_NEXT_INTERFACE_OUTPUT;
588           u32 next1 = IP6_POP_HOP_BY_HOP_NEXT_INTERFACE_OUTPUT;
589           u32 sw_if_index0, sw_if_index1;
590           u8 tmp0[6], tmp1[6];
591           ethernet_header_t *en0, *en1;
592           u32 bi0, bi1;
593           vlib_buffer_t * b0, * b1;
594           
595           /* Prefetch next iteration. */
596           {
597             vlib_buffer_t * p2, * p3;
598             
599             p2 = vlib_get_buffer (vm, from[2]);
600             p3 = vlib_get_buffer (vm, from[3]);
601             
602             vlib_prefetch_buffer_header (p2, LOAD);
603             vlib_prefetch_buffer_header (p3, LOAD);
604
605             CLIB_PREFETCH (p2->data, CLIB_CACHE_LINE_BYTES, STORE);
606             CLIB_PREFETCH (p3->data, CLIB_CACHE_LINE_BYTES, STORE);
607           }
608
609           /* speculatively enqueue b0 and b1 to the current next frame */
610           to_next[0] = bi0 = from[0];
611           to_next[1] = bi1 = from[1];
612           from += 2;
613           to_next += 2;
614           n_left_from -= 2;
615           n_left_to_next -= 2;
616
617           b0 = vlib_get_buffer (vm, bi0);
618           b1 = vlib_get_buffer (vm, bi1);
619
620           /* $$$$$ Dual loop: process 2 x packets here $$$$$ */
621           ASSERT (b0->current_data == 0);
622           ASSERT (b1->current_data == 0);
623           
624           ip0 = vlib_buffer_get_current (b0);
625           ip1 = vlib_buffer_get_current (b0);
626
627           sw_if_index0 = vnet_buffer(b0)->sw_if_index[VLIB_RX];
628           sw_if_index1 = vnet_buffer(b1)->sw_if_index[VLIB_RX];
629
630           /* $$$$$ End of processing 2 x packets $$$$$ */
631
632           if (PREDICT_FALSE((node->flags & VLIB_NODE_FLAG_TRACE)))
633             {
634               if (b0->flags & VLIB_BUFFER_IS_TRACED) 
635                 {
636                     ip6_pop_hop_by_hop_trace_t *t = 
637                       vlib_add_trace (vm, node, b0, sizeof (*t));
638                     t->sw_if_index = sw_if_index0;
639                     t->next_index = next0;
640                   }
641                 if (b1->flags & VLIB_BUFFER_IS_TRACED) 
642                   {
643                     ip6_pop_hop_by_hop_trace_t *t = 
644                       vlib_add_trace (vm, node, b1, sizeof (*t));
645                     t->sw_if_index = sw_if_index1;
646                     t->next_index = next1;
647                   }
648               }
649             
650             /* verify speculative enqueues, maybe switch current next frame */
651             vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
652                                              to_next, n_left_to_next,
653                                              bi0, bi1, next0, next1);
654         }
655 #endif
656
657       while (n_left_from > 0 && n_left_to_next > 0)
658         {
659           u32 bi0;
660           vlib_buffer_t * b0;
661           u32 next0;
662           u32 adj_index0;
663           ip6_header_t * ip0;
664           ip_adjacency_t * adj0;
665           ip6_hop_by_hop_header_t *hbh0;
666           u64 * copy_dst0, * copy_src0;
667           u16 new_l0;
668           
669           /* speculatively enqueue b0 to the current next frame */
670           bi0 = from[0];
671           to_next[0] = bi0;
672           from += 1;
673           to_next += 1;
674           n_left_from -= 1;
675           n_left_to_next -= 1;
676
677           b0 = vlib_get_buffer (vm, bi0);
678
679           ip0 = vlib_buffer_get_current (b0);
680           adj_index0 = vnet_buffer (b0)->ip.adj_index[VLIB_TX];
681           adj0 = ip_get_adjacency (lm, adj_index0);
682
683           /* Default use the next_index from the adjacency. */
684           next0 = adj0->lookup_next_index;
685
686           /* Perfectly normal to end up here w/ out h-b-h header */
687           hbh0 = (ip6_hop_by_hop_header_t *)(ip0+1);
688           
689           /* TODO:Temporarily doing it here.. do this validation in end_of_path_cb */
690           ioam_pop_hop_by_hop_processing(vm, ip0, hbh0);
691           /* Pop the trace data */
692           vlib_buffer_advance (b0, (hbh0->length+1)<<3);
693           new_l0 = clib_net_to_host_u16 (ip0->payload_length) -
694             ((hbh0->length+1)<<3);
695           ip0->payload_length = clib_host_to_net_u16 (new_l0);
696           ip0->protocol = hbh0->protocol;
697           copy_src0 = (u64 *)ip0;
698           copy_dst0 = copy_src0 + (hbh0->length+1);
699           copy_dst0 [4] = copy_src0[4];
700           copy_dst0 [3] = copy_src0[3];
701           copy_dst0 [2] = copy_src0[2];
702           copy_dst0 [1] = copy_src0[1];
703           copy_dst0 [0] = copy_src0[0];
704           processed++;
705               
706           if (PREDICT_FALSE((node->flags & VLIB_NODE_FLAG_TRACE) 
707                             && (b0->flags & VLIB_BUFFER_IS_TRACED))) 
708             {
709               ip6_pop_hop_by_hop_trace_t *t = 
710                  vlib_add_trace (vm, node, b0, sizeof (*t));
711               t->next_index = next0;
712             }
713
714           /* verify speculative enqueue, maybe switch current next frame */
715           vlib_validate_buffer_enqueue_x1 (vm, node, next_index,
716                                            to_next, n_left_to_next,
717                                            bi0, next0);
718         }
719
720       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
721     }
722
723   vlib_node_increment_counter (vm, ip6_pop_hop_by_hop_node.index, 
724                                IP6_POP_HOP_BY_HOP_ERROR_PROCESSED, processed);
725   vlib_node_increment_counter (vm, ip6_pop_hop_by_hop_node.index, 
726                                IP6_POP_HOP_BY_HOP_ERROR_NO_HOHO, no_header);
727   return frame->n_vectors;
728 }
729
730 VLIB_REGISTER_NODE (ip6_pop_hop_by_hop_node) = {
731   .function = ip6_pop_hop_by_hop_node_fn,
732   .name = "ip6-pop-hop-by-hop",
733   .vector_size = sizeof (u32),
734   .format_trace = format_ip6_pop_hop_by_hop_trace,
735   .type = VLIB_NODE_TYPE_INTERNAL,
736   .sibling_of = "ip6-lookup",
737   .n_errors = ARRAY_LEN(ip6_pop_hop_by_hop_error_strings),
738   .error_strings = ip6_pop_hop_by_hop_error_strings,
739
740   /* See ip/lookup.h */
741   .n_next_nodes = 0,
742 };
743
744 VLIB_NODE_FUNCTION_MULTIARCH (ip6_pop_hop_by_hop_node,
745                               ip6_pop_hop_by_hop_node_fn)
746
747 static clib_error_t *
748 ip6_hop_by_hop_ioam_init (vlib_main_t * vm)
749 {
750   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
751
752   hm->vlib_main = vm;
753   hm->vnet_main = vnet_get_main();
754   hm->unix_time_0 = (u32) time (0); /* Store starting time */
755   hm->vlib_time_0 = vlib_time_now (vm);
756   hm->ioam_flag = IOAM_HBYH_MOD;
757   hm->trace_tsp = TSP_MICROSECONDS; /* Micro seconds */
758   memset(hm->add_options, 0, sizeof(hm->add_options));
759   memset(hm->pop_options, 0, sizeof(hm->pop_options));
760   memset(hm->options_size, 0, sizeof(hm->options_size));
761
762   /*
763    * Register the handlers
764    * XXX: This should be done dynamically based on OAM feature being enabled or not.
765    */
766   if (ip6_hbh_register_option(HBH_OPTION_TYPE_IOAM_TRACE_DATA_LIST, ip6_hbh_ioam_trace_data_list_handler,
767                               ip6_hbh_ioam_trace_data_list_trace_handler) < 0)
768     return (clib_error_create("registration of HBH_OPTION_TYPE_IOAM_TRACE_DATA_LIST failed"));
769
770   return (0);
771 }
772
773 VLIB_INIT_FUNCTION (ip6_hop_by_hop_ioam_init);
774
775 int ip6_ioam_set_rewrite (u8 **rwp, u32 trace_type, u32 trace_option_elts, 
776                           int has_pot_option, int has_ppc_option)
777 {
778   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;  
779   u8 *rewrite = 0;
780   u32 size, rnd_size;
781   ip6_hop_by_hop_header_t *hbh;
782   ioam_trace_option_t * trace_option;
783   u8 *current;
784   u8 trace_data_size = 0;  
785
786   vec_free (*rwp);
787
788   if (trace_option_elts == 0 && has_pot_option == 0)
789     return -1;
790
791   /* Work out how much space we need */
792   size = sizeof (ip6_hop_by_hop_header_t);
793
794   if (trace_option_elts)
795     {
796       size += sizeof (ip6_hop_by_hop_option_t);
797
798       trace_data_size = fetch_trace_data_size(trace_type);
799       if (trace_data_size == 0)
800           return VNET_API_ERROR_INVALID_VALUE;
801
802       if (trace_option_elts * trace_data_size > 254)
803           return VNET_API_ERROR_INVALID_VALUE;
804   
805       size += trace_option_elts * trace_data_size;
806     }
807   if (has_pot_option && hm->add_options[HBH_OPTION_TYPE_IOAM_PROOF_OF_TRANSIT] != 0)
808     {
809       size += sizeof (ip6_hop_by_hop_option_t);
810       size += hm->options_size[HBH_OPTION_TYPE_IOAM_PROOF_OF_TRANSIT];
811     }
812
813   /* Round to a multiple of 8 octets */
814   rnd_size = (size + 7) & ~7;
815
816   /* allocate it, zero-fill / pad by construction */
817   vec_validate (rewrite, rnd_size-1);
818
819   hbh = (ip6_hop_by_hop_header_t *) rewrite;
820   /* Length of header in 8 octet units, not incl first 8 octets */
821   hbh->length = (rnd_size>>3) - 1;
822   current = (u8 *)(hbh+1);
823   
824   if (trace_option_elts)
825     {
826       trace_option = (ioam_trace_option_t *)current;
827       trace_option->hdr.type = HBH_OPTION_TYPE_IOAM_TRACE_DATA_LIST
828         | HBH_OPTION_TYPE_DATA_CHANGE_ENROUTE;
829       trace_option->hdr.length = 
830                2 /*ioam_trace_type,data_list_elts_left */ + 
831               trace_option_elts * trace_data_size;
832       trace_option->ioam_trace_type = trace_type & TRACE_TYPE_MASK;
833       trace_option->data_list_elts_left = trace_option_elts;
834       current += sizeof (ioam_trace_option_t) + 
835         trace_option_elts * trace_data_size;
836     }
837   if (has_pot_option && hm->add_options[HBH_OPTION_TYPE_IOAM_PROOF_OF_TRANSIT] != 0)
838     {
839       if (0 == hm->add_options[HBH_OPTION_TYPE_IOAM_PROOF_OF_TRANSIT](current,
840                                         hm->options_size[HBH_OPTION_TYPE_IOAM_PROOF_OF_TRANSIT]))
841           current += sizeof (hm->options_size[HBH_OPTION_TYPE_IOAM_PROOF_OF_TRANSIT]);
842     }
843   
844   *rwp = rewrite;
845   return 0;
846 }
847
848 clib_error_t *
849 clear_ioam_rewrite_fn(void)
850 {
851   ip6_hop_by_hop_ioam_main_t *hm = &ip6_hop_by_hop_ioam_main;
852
853   vec_free(hm->rewrite);
854   hm->rewrite = 0;
855   hm->node_id = 0;
856   hm->app_data = 0;
857   hm->trace_type = 0;
858   hm->trace_option_elts = 0;
859   hm->has_pot_option = 0;
860   hm->has_ppc_option = 0;
861   hm->trace_tsp = TSP_MICROSECONDS; 
862
863   return 0;
864 }
865
866 clib_error_t * clear_ioam_rewrite_command_fn (vlib_main_t * vm,
867                                  unformat_input_t * input,
868                                  vlib_cli_command_t * cmd)
869 {
870   return(clear_ioam_rewrite_fn());
871 }
872   
873 VLIB_CLI_COMMAND (ip6_clear_ioam_trace_cmd, static) = {
874   .path = "clear ioam rewrite",
875   .short_help = "clear ioam rewrite",
876   .function = clear_ioam_rewrite_command_fn,
877 };
878
879 clib_error_t *
880 ip6_ioam_trace_profile_set(u32 trace_option_elts, u32 trace_type, u32 node_id,
881                            u32 app_data, int has_pot_option, u32 trace_tsp, 
882                            int has_ppc_option)
883 {
884   int rv;
885   ip6_hop_by_hop_ioam_main_t *hm = &ip6_hop_by_hop_ioam_main;
886   rv = ip6_ioam_set_rewrite (&hm->rewrite, trace_type, trace_option_elts,
887                              has_pot_option, has_ppc_option);
888
889   switch (rv)
890     {
891     case 0:
892       hm->node_id = node_id;
893       hm->app_data = app_data;
894       hm->trace_type = trace_type;
895       hm->trace_option_elts = trace_option_elts;
896       hm->has_pot_option = has_pot_option;
897       hm->has_ppc_option = has_ppc_option;
898       hm->trace_tsp = trace_tsp;
899       break;
900
901     default:
902       return clib_error_return_code(0, rv, 0, "ip6_ioam_set_rewrite returned %d", rv);
903     }
904
905   return 0;
906 }
907
908
909 static clib_error_t *
910 ip6_set_ioam_rewrite_command_fn (vlib_main_t * vm,
911                                  unformat_input_t * input,
912                                  vlib_cli_command_t * cmd)
913 {
914   u32 trace_option_elts = 0;
915   u32 trace_type = 0, node_id = 0; 
916   u32 app_data = 0, trace_tsp = TSP_MICROSECONDS;
917   int has_pot_option = 0;
918   int has_ppc_option = 0;
919   clib_error_t * rv = 0;
920   
921   while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
922     {
923       if (unformat (input, "trace-type 0x%x trace-elts %d "
924                            "trace-tsp %d node-id 0x%x app-data 0x%x", 
925                       &trace_type, &trace_option_elts, &trace_tsp,
926                       &node_id, &app_data))
927             ;
928       else if (unformat (input, "pot"))
929         has_pot_option = 1;
930       else if (unformat (input, "ppc encap"))
931         has_ppc_option = PPC_ENCAP;
932       else if (unformat (input, "ppc decap"))
933         has_ppc_option = PPC_DECAP;
934       else if (unformat (input, "ppc none"))
935         has_ppc_option = PPC_NONE;
936       else
937         break;
938     }
939   
940     
941     rv = ip6_ioam_trace_profile_set(trace_option_elts, trace_type, node_id,
942                            app_data, has_pot_option, trace_tsp, has_ppc_option);
943
944     return rv;
945 }
946
947
948 VLIB_CLI_COMMAND (ip6_set_ioam_rewrite_cmd, static) = {
949   .path = "set ioam rewrite",
950   .short_help = "set ioam rewrite trace-type <0x1f|0x3|0x9|0x11|0x19> trace-elts <nn> trace-tsp <0|1|2|3> node-id <node id in hex> app-data <app_data in hex> [pot] [ppc <encap|decap>]",
951   .function = ip6_set_ioam_rewrite_command_fn,
952 };
953   
954 static clib_error_t *
955 ip6_show_ioam_summary_cmd_fn (vlib_main_t * vm,
956                       unformat_input_t * input,
957                       vlib_cli_command_t * cmd)
958 {
959   ip6_hop_by_hop_ioam_main_t *hm = &ip6_hop_by_hop_ioam_main;
960   u8 *s = 0;
961
962
963   if (!is_zero_ip6_address(&hm->adj))
964   {
965   s = format(s, "              REWRITE FLOW CONFIGS - \n");
966   s = format(s, "               Destination Address : %U\n",
967             format_ip6_address, &hm->adj, sizeof(ip6_address_t));
968   s = format(s, "                    Flow operation : %d (%s)\n", hm->ioam_flag,
969            (hm->ioam_flag == IOAM_HBYH_ADD) ? "Add" : 
970           ((hm->ioam_flag == IOAM_HBYH_MOD) ? "Mod" : "Pop"));
971   } 
972   else 
973   {
974   s = format(s, "              REWRITE FLOW CONFIGS - Not configured\n");
975   }
976
977   if (hm->trace_option_elts)
978   {
979   s = format(s, " HOP BY HOP OPTIONS - TRACE CONFIG - \n");
980   s = format(s, "                        Trace Type : 0x%x (%d)\n", 
981           hm->trace_type, hm->trace_type);
982   s = format(s, "         Trace timestamp precision : %d (%s)\n", hm->trace_tsp,
983        (hm->trace_tsp == TSP_SECONDS) ? "Seconds" : 
984       ((hm->trace_tsp == TSP_MILLISECONDS) ? "Milliseconds" : 
985      (((hm->trace_tsp == TSP_MICROSECONDS) ? "Microseconds" : "Nanoseconds"))));
986   s = format(s, "                Num of trace nodes : %d\n", 
987           hm->trace_option_elts);
988   s = format(s, "                           Node-id : 0x%x (%d)\n", 
989           hm->node_id, hm->node_id);
990   s = format(s, "                          App Data : 0x%x (%d)\n", 
991           hm->app_data, hm->app_data);
992   }
993   else
994   {
995   s = format(s, " HOP BY HOP OPTIONS - TRACE CONFIG - Not configured\n");
996   }
997
998   s = format(s, "                        POT OPTION - %d (%s)\n", 
999           hm->has_pot_option, (hm->has_pot_option?"Enabled":"Disabled"));
1000   if (hm->has_pot_option)
1001     s = format(s, "Try 'show ioam pot and show pot profile' for more information\n");
1002
1003   s = format(s, "         EDGE TO EDGE - PPC OPTION - %d (%s)\n", 
1004          hm->has_ppc_option, ppc_state[hm->has_ppc_option]);
1005   if (hm->has_ppc_option)
1006     s = format(s, "Try 'show ioam ppc' for more information\n");
1007
1008   vlib_cli_output(vm, "%v", s);
1009   vec_free(s);
1010   return 0;
1011 }
1012
1013 VLIB_CLI_COMMAND (ip6_show_ioam_run_cmd, static) = {
1014   .path = "show ioam summary",
1015   .short_help = "Summary of IOAM configuration",
1016   .function = ip6_show_ioam_summary_cmd_fn,
1017 };
1018
1019 int ip6_ioam_set_destination (ip6_address_t *addr, u32 mask_width, u32 vrf_id,
1020                               int is_add, int is_pop, int is_none)
1021 {
1022   ip6_main_t * im = &ip6_main;
1023   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
1024   ip_lookup_main_t * lm = &im->lookup_main;
1025   ip_adjacency_t * adj;
1026   u32 fib_index;
1027   u32 len, adj_index;
1028   int i, rv;
1029   uword * p;
1030   BVT(clib_bihash_kv) kv, value;
1031
1032   if ((is_add + is_pop + is_none) != 1)
1033     return VNET_API_ERROR_INVALID_VALUE_2;
1034
1035   /* Go find the adjacency we're supposed to tickle */
1036   p = hash_get (im->fib_index_by_table_id, vrf_id);
1037
1038   if (p == 0)
1039     return VNET_API_ERROR_NO_SUCH_FIB;
1040
1041   fib_index = p[0];
1042
1043   len = vec_len (im->prefix_lengths_in_search_order);
1044   
1045   for (i = 0; i < len; i++)
1046     {
1047       int dst_address_length = im->prefix_lengths_in_search_order[i];
1048       ip6_address_t * mask = &im->fib_masks[dst_address_length];
1049       
1050       if (dst_address_length != mask_width)
1051         continue;
1052
1053       kv.key[0] = addr->as_u64[0] & mask->as_u64[0];
1054       kv.key[1] = addr->as_u64[1] & mask->as_u64[1];
1055       kv.key[2] = ((u64)((fib_index))<<32) | dst_address_length;
1056       
1057       rv = BV(clib_bihash_search_inline_2)(&im->ip6_lookup_table, &kv, &value);
1058       if (rv == 0)
1059         goto found;
1060
1061     }
1062   return VNET_API_ERROR_NO_SUCH_ENTRY;
1063   
1064  found:
1065
1066   /* Got it, modify as directed... */
1067   adj_index = value.value;
1068   adj = ip_get_adjacency (lm, adj_index);
1069
1070   /* Restore original lookup-next action */
1071   if (adj->saved_lookup_next_index)
1072     {
1073       adj->lookup_next_index = adj->saved_lookup_next_index;
1074       adj->saved_lookup_next_index = 0;
1075     }
1076
1077   /* Save current action */
1078   if (is_add || is_pop)
1079     adj->saved_lookup_next_index = adj->lookup_next_index;
1080
1081   if (is_add)
1082     adj->lookup_next_index = IP6_LOOKUP_NEXT_ADD_HOP_BY_HOP;
1083
1084   if (is_pop)
1085     adj->lookup_next_index = IP6_LOOKUP_NEXT_POP_HOP_BY_HOP;
1086
1087   hm->adj = *addr;
1088   hm->ioam_flag = (is_add ? IOAM_HBYH_ADD :
1089                   (is_pop ? IOAM_HBYH_POP : IOAM_HBYH_MOD));
1090   return 0;
1091 }
1092                               
1093 static clib_error_t *
1094 ip6_set_ioam_destination_command_fn (vlib_main_t * vm,
1095                                      unformat_input_t * input,
1096                                      vlib_cli_command_t * cmd)
1097 {
1098   ip6_address_t addr;
1099   u32 mask_width = ~0;
1100   int is_add = 0;
1101   int is_pop = 0;
1102   int is_none = 0;
1103   u32 vrf_id = 0;
1104   int rv;
1105
1106   while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
1107     {
1108       if (unformat (input, "%U/%d", 
1109                     unformat_ip6_address, &addr, &mask_width))
1110         ;
1111       else if (unformat (input, "vrf-id %d", &vrf_id))
1112         ;
1113       else if (unformat (input, "add"))
1114         is_add = 1;
1115       else if (unformat (input, "pop"))
1116         is_pop = 1;
1117       else if (unformat (input, "none"))
1118         is_none = 1;
1119       else
1120         break;
1121     }
1122
1123   if ((is_add + is_pop + is_none) != 1)
1124     return clib_error_return (0, "One of (add, pop, none) required");
1125   if (mask_width == ~0)
1126     return clib_error_return (0, "<address>/<mask-width> required");
1127
1128   rv = ip6_ioam_set_destination (&addr, mask_width, vrf_id, 
1129                                  is_add, is_pop, is_none);
1130
1131   switch (rv)
1132     {
1133     case 0:
1134       break;
1135     default:
1136       return clib_error_return (0, "ip6_ioam_set_destination returned %d", rv);
1137     }
1138   
1139   return 0;
1140 }
1141
1142 VLIB_CLI_COMMAND (ip6_set_ioam_destination_cmd, static) = {
1143   .path = "set ioam destination",
1144   .short_help = "set ioam destination <ip6-address>/<width> add | pop | none",
1145   .function = ip6_set_ioam_destination_command_fn,
1146 };
1147
1148
1149 void vnet_register_ioam_end_of_path_callback (void *cb)
1150 {
1151   ip6_hop_by_hop_ioam_main_t * hm = &ip6_hop_by_hop_ioam_main;
1152
1153   hm->ioam_end_of_path_cb = cb;
1154 }