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/*
16 * hdlc_node.c: hdlc packet processing
17 *
18 * Copyright (c) 2010 Eliot Dresselhaus
19 *
20 * Permission is hereby granted, free of charge, to any person obtaining
21 * a copy of this software and associated documentation files (the
22 * "Software"), to deal in the Software without restriction, including
23 * without limitation the rights to use, copy, modify, merge, publish,
24 * distribute, sublicense, and/or sell copies of the Software, and to
25 * permit persons to whom the Software is furnished to do so, subject to
26 * the following conditions:
27 *
28 * The above copyright notice and this permission notice shall be
29 * included in all copies or substantial portions of the Software.
30 *
31 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
32 *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
33 *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
34 *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
35 *  LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
36 *  OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
37 *  WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
38 */
39
40#include <vlib/vlib.h>
41#include <vnet/pg/pg.h>
42#include <vnet/hdlc/hdlc.h>
43#include <vppinfra/sparse_vec.h>
44
45#define foreach_hdlc_input_next			\
46  _ (PUNT, "error-punt")			\
47  _ (DROP, "error-drop")
48
49typedef enum
50{
51#define _(s,n) HDLC_INPUT_NEXT_##s,
52  foreach_hdlc_input_next
53#undef _
54    HDLC_INPUT_N_NEXT,
55} hdlc_input_next_t;
56
57typedef struct
58{
59  u8 packet_data[32];
60} hdlc_input_trace_t;
61
62static u8 *
63format_hdlc_input_trace (u8 * s, va_list * va)
64{
65  CLIB_UNUSED (vlib_main_t * vm) = va_arg (*va, vlib_main_t *);
66  CLIB_UNUSED (vlib_node_t * node) = va_arg (*va, vlib_node_t *);
67  hdlc_input_trace_t *t = va_arg (*va, hdlc_input_trace_t *);
68
69  s = format (s, "%U", format_hdlc_header, t->packet_data);
70
71  return s;
72}
73
74typedef struct
75{
76  /* Sparse vector mapping hdlc protocol in network byte order
77     to next index. */
78  u16 *next_by_protocol;
79
80  u32 *sparse_index_by_next_index;
81} hdlc_input_runtime_t;
82
83static uword
84hdlc_input (vlib_main_t * vm,
85	    vlib_node_runtime_t * node, vlib_frame_t * from_frame)
86{
87  hdlc_input_runtime_t *rt = (void *) node->runtime_data;
88  u32 n_left_from, next_index, i_next, *from, *to_next;
89
90  from = vlib_frame_vector_args (from_frame);
91  n_left_from = from_frame->n_vectors;
92
93  if (node->flags & VLIB_NODE_FLAG_TRACE)
94    vlib_trace_frame_buffers_only (vm, node,
95				   from,
96				   n_left_from,
97				   sizeof (from[0]),
98				   sizeof (hdlc_input_trace_t));
99
100  next_index = node->cached_next_index;
101  i_next = vec_elt (rt->sparse_index_by_next_index, next_index);
102
103  while (n_left_from > 0)
104    {
105      u32 n_left_to_next;
106
107      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
108
109      while (n_left_from >= 4 && n_left_to_next >= 2)
110	{
111	  u32 bi0, bi1;
112	  vlib_buffer_t *b0, *b1;
113	  hdlc_header_t *h0, *h1;
114	  u32 i0, i1, len0, len1, protocol0, protocol1, enqueue_code;
115
116	  /* Prefetch next iteration. */
117	  {
118	    vlib_buffer_t *b2, *b3;
119
120	    b2 = vlib_get_buffer (vm, from[2]);
121	    b3 = vlib_get_buffer (vm, from[3]);
122
123	    vlib_prefetch_buffer_header (b2, LOAD);
124	    vlib_prefetch_buffer_header (b3, LOAD);
125
126	    CLIB_PREFETCH (b2->data, sizeof (h0[0]), LOAD);
127	    CLIB_PREFETCH (b3->data, sizeof (h1[0]), LOAD);
128	  }
129
130	  bi0 = from[0];
131	  bi1 = from[1];
132	  to_next[0] = bi0;
133	  to_next[1] = bi1;
134	  from += 2;
135	  to_next += 2;
136	  n_left_to_next -= 2;
137	  n_left_from -= 2;
138
139	  b0 = vlib_get_buffer (vm, bi0);
140	  b1 = vlib_get_buffer (vm, bi1);
141
142	  h0 = vlib_buffer_get_current (b0);
143	  h1 = vlib_buffer_get_current (b1);
144
145	  protocol0 = h0->protocol;
146	  protocol1 = h1->protocol;
147
148	  /* Add padding bytes for OSI protocols. */
149	  len0 = sizeof (h0[0]);
150	  len1 = sizeof (h1[0]);
151
152	  len0 += protocol0 == clib_host_to_net_u16 (HDLC_PROTOCOL_osi);
153	  len1 += protocol1 == clib_host_to_net_u16 (HDLC_PROTOCOL_osi);
154
155	  vlib_buffer_advance (b0, len0);
156	  vlib_buffer_advance (b1, len1);
157
158	  /* Index sparse array with network byte order. */
159	  sparse_vec_index2 (rt->next_by_protocol, protocol0, protocol1, &i0,
160			     &i1);
161
162	  b0->error =
163	    node->errors[i0 ==
164			 SPARSE_VEC_INVALID_INDEX ?
165			 HDLC_ERROR_UNKNOWN_PROTOCOL : HDLC_ERROR_NONE];
166	  b1->error =
167	    node->errors[i1 ==
168			 SPARSE_VEC_INVALID_INDEX ?
169			 HDLC_ERROR_UNKNOWN_PROTOCOL : HDLC_ERROR_NONE];
170
171	  enqueue_code = (i0 != i_next) + 2 * (i1 != i_next);
172
173	  if (PREDICT_FALSE (enqueue_code != 0))
174	    {
175	      switch (enqueue_code)
176		{
177		case 1:
178		  /* A B A */
179		  to_next[-2] = bi1;
180		  to_next -= 1;
181		  n_left_to_next += 1;
182		  vlib_set_next_frame_buffer (vm, node,
183					      vec_elt (rt->next_by_protocol,
184						       i0), bi0);
185		  break;
186
187		case 2:
188		  /* A A B */
189		  to_next -= 1;
190		  n_left_to_next += 1;
191		  vlib_set_next_frame_buffer (vm, node,
192					      vec_elt (rt->next_by_protocol,
193						       i1), bi1);
194		  break;
195
196		case 3:
197		  /* A B B or A B C */
198		  to_next -= 2;
199		  n_left_to_next += 2;
200		  vlib_set_next_frame_buffer (vm, node,
201					      vec_elt (rt->next_by_protocol,
202						       i0), bi0);
203		  vlib_set_next_frame_buffer (vm, node,
204					      vec_elt (rt->next_by_protocol,
205						       i1), bi1);
206		  if (i0 == i1)
207		    {
208		      vlib_put_next_frame (vm, node, next_index,
209					   n_left_to_next);
210		      i_next = i1;
211		      next_index = vec_elt (rt->next_by_protocol, i_next);
212		      vlib_get_next_frame (vm, node, next_index, to_next,
213					   n_left_to_next);
214		    }
215		}
216	    }
217	}
218
219      while (n_left_from > 0 && n_left_to_next > 0)
220	{
221	  u32 bi0;
222	  vlib_buffer_t *b0;
223	  hdlc_header_t *h0;
224	  u32 i0, len0, protocol0;
225
226	  bi0 = from[0];
227	  to_next[0] = bi0;
228	  from += 1;
229	  to_next += 1;
230	  n_left_from -= 1;
231	  n_left_to_next -= 1;
232
233	  b0 = vlib_get_buffer (vm, bi0);
234
235	  h0 = vlib_buffer_get_current (b0);
236
237	  protocol0 = h0->protocol;
238
239	  /* Add padding bytes for OSI protocols. */
240	  len0 = sizeof (h0[0]);
241	  len0 += protocol0 == clib_host_to_net_u16 (HDLC_PROTOCOL_osi);
242
243	  vlib_buffer_advance (b0, len0);
244
245	  i0 = sparse_vec_index (rt->next_by_protocol, protocol0);
246
247	  b0->error =
248	    node->errors[i0 ==
249			 SPARSE_VEC_INVALID_INDEX ?
250			 HDLC_ERROR_UNKNOWN_PROTOCOL : HDLC_ERROR_NONE];
251
252	  /* Sent packet to wrong next? */
253	  if (PREDICT_FALSE (i0 != i_next))
254	    {
255	      /* Return old frame; remove incorrectly enqueued packet. */
256	      vlib_put_next_frame (vm, node, next_index, n_left_to_next + 1);
257
258	      /* Send to correct next. */
259	      i_next = i0;
260	      next_index = vec_elt (rt->next_by_protocol, i_next);
261	      vlib_get_next_frame (vm, node, next_index,
262				   to_next, n_left_to_next);
263
264	      to_next[0] = bi0;
265	      to_next += 1;
266	      n_left_to_next -= 1;
267	    }
268	}
269
270      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
271    }
272
273  return from_frame->n_vectors;
274}
275
276static char *hdlc_error_strings[] = {
277#define hdlc_error(n,s) s,
278#include "error.def"
279#undef hdlc_error
280};
281
282/* *INDENT-OFF* */
283VLIB_REGISTER_NODE (hdlc_input_node) = {
284  .function = hdlc_input,
285  .name = "hdlc-input",
286  /* Takes a vector of packets. */
287  .vector_size = sizeof (u32),
288
289  .runtime_data_bytes = sizeof (hdlc_input_runtime_t),
290
291  .n_errors = HDLC_N_ERROR,
292  .error_strings = hdlc_error_strings,
293
294  .n_next_nodes = HDLC_INPUT_N_NEXT,
295  .next_nodes = {
296#define _(s,n) [HDLC_INPUT_NEXT_##s] = n,
297    foreach_hdlc_input_next
298#undef _
299  },
300
301  .format_buffer = format_hdlc_header_with_length,
302  .format_trace = format_hdlc_input_trace,
303  .unformat_buffer = unformat_hdlc_header,
304};
305/* *INDENT-ON* */
306
307static clib_error_t *
308hdlc_input_runtime_init (vlib_main_t * vm)
309{
310  hdlc_input_runtime_t *rt;
311  rt = vlib_node_get_runtime_data (vm, hdlc_input_node.index);
312
313  rt->next_by_protocol = sparse_vec_new
314    ( /* elt bytes */ sizeof (rt->next_by_protocol[0]),
315     /* bits in index */ BITS (((hdlc_header_t *) 0)->protocol));
316
317  vec_validate (rt->sparse_index_by_next_index, HDLC_INPUT_NEXT_DROP);
318  vec_validate (rt->sparse_index_by_next_index, HDLC_INPUT_NEXT_PUNT);
319  rt->sparse_index_by_next_index[HDLC_INPUT_NEXT_DROP]
320    = SPARSE_VEC_INVALID_INDEX;
321  rt->sparse_index_by_next_index[HDLC_INPUT_NEXT_PUNT]
322    = SPARSE_VEC_INVALID_INDEX;
323
324  return 0;
325}
326
327static clib_error_t *
328hdlc_input_init (vlib_main_t * vm)
329{
330
331  {
332    clib_error_t *error = vlib_call_init_function (vm, hdlc_init);
333    if (error)
334      clib_error_report (error);
335  }
336
337  hdlc_setup_node (vm, hdlc_input_node.index);
338  hdlc_input_runtime_init (vm);
339
340  return 0;
341}
342
343VLIB_INIT_FUNCTION (hdlc_input_init);
344VLIB_WORKER_INIT_FUNCTION (hdlc_input_runtime_init);
345
346void
347hdlc_register_input_protocol (vlib_main_t * vm,
348			      hdlc_protocol_t protocol, u32 node_index)
349{
350  hdlc_main_t *em = &hdlc_main;
351  hdlc_protocol_info_t *pi;
352  hdlc_input_runtime_t *rt;
353  u16 *n;
354  u32 i;
355
356  {
357    clib_error_t *error = vlib_call_init_function (vm, hdlc_input_init);
358    if (error)
359      clib_error_report (error);
360  }
361
362  pi = hdlc_get_protocol_info (em, protocol);
363  pi->node_index = node_index;
364  pi->next_index = vlib_node_add_next (vm, hdlc_input_node.index, node_index);
365
366  /* Setup hdlc protocol -> next index sparse vector mapping. */
367  rt = vlib_node_get_runtime_data (vm, hdlc_input_node.index);
368  n =
369    sparse_vec_validate (rt->next_by_protocol,
370			 clib_host_to_net_u16 (protocol));
371  n[0] = pi->next_index;
372
373  /* Rebuild next index -> sparse index inverse mapping when sparse vector
374     is updated. */
375  vec_validate (rt->sparse_index_by_next_index, pi->next_index);
376  for (i = 1; i < vec_len (rt->next_by_protocol); i++)
377    rt->sparse_index_by_next_index[rt->next_by_protocol[i]] = i;
378}
379
380/*
381 * fd.io coding-style-patch-verification: ON
382 *
383 * Local Variables:
384 * eval: (c-set-style "gnu")
385 * End:
386 */
387