1/*
2 *------------------------------------------------------------------
3 * Copyright (c) 2019 Cisco and/or its affiliates.
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at:
7 *
8 *     http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *------------------------------------------------------------------
16 */
17
18#include <stdlib.h>
19#include <stdio.h>
20#include <string.h>
21#include <getopt.h>
22#include <errno.h>
23#include <inttypes.h>
24#include <pthread.h>
25#include <stdbool.h>
26#include <unistd.h>
27
28#include <sys/epoll.h>
29#include <sys/eventfd.h>
30
31#include <libmemif.h>
32#include <icmp_proto.h>
33
34
35#define APP_NAME "ICMP_Responder_mt_v3.1"
36#define IF_NAME  "memif_connection"
37
38#ifdef ICMP_DBG
39#define DBG(...) do {                                               \
40                    printf (APP_NAME":%s:%d: ", __func__, __LINE__);         \
41                    printf (__VA_ARGS__);                           \
42                    printf ("\n");                                  \
43                } while (0)
44#else
45#define DBG(...)
46#endif
47
48#define ICMPR_BUFFER_LENGTH		32
49#define ICMPR_SOCKET_FILENAME_LEN	256
50#define ICMPR_MEMIF_BUFFER_NUM		256
51
52static struct option options[] = {
53  {"threads", required_argument, 0, 't'},
54  {"if_num", required_argument, 0, 'i'}
55};
56
57struct memif_connection
58{
59  uint16_t id;			/* unique interface id */
60  bool connected;		/* is connected */
61  struct per_thread_data *ptd;	/* per thread data */
62  memif_conn_handle_t handle;	/* memif connection handle */
63  uint8_t ip_addr[4];		/* ip4 address */
64};
65
66struct per_thread_data
67{
68  bool running;			/* is thread main loop running */
69  uint8_t index;		/* thread index */
70  int epfd;			/* epoll file descriptor */
71  int pcfd;			/* poll cancel file descriptor */
72  uint16_t if_num;		/* number of interfaces on this thread */
73  struct memif_connection *conns;	/* memif connections pool */
74  memif_per_thread_main_handle_t pt_main;	/* memif per thread main handle */
75  memif_socket_handle_t socket_handle;		/* memif socket handle */
76};
77
78struct icmpr_main
79{
80  uint8_t threads;		/* number of threads */
81  uint16_t per_thread_if_num;	/* number of interfaces per thread */
82  struct per_thread_data *ptd;	/* per thread data pool */
83  pthread_t *pthread;		/* thread pool */
84};
85
86struct icmpr_main icmpr_main;
87
88int
89add_epoll_fd (int epfd, int fd, uint32_t events)
90{
91  if (fd < 0)
92    {
93      DBG ("invalid fd %d", fd);
94      return -1;
95    }
96  struct epoll_event evt;
97  memset (&evt, 0, sizeof (evt));
98  evt.events = events;
99  evt.data.fd = fd;
100  if (epoll_ctl (epfd, EPOLL_CTL_ADD, fd, &evt) < 0)
101    {
102      DBG ("epoll_ctl: %s fd %d", strerror (errno), fd);
103      return -1;
104    }
105  DBG ("fd %d added to epoll", fd);
106  return 0;
107}
108
109int
110mod_epoll_fd (int epfd, int fd, uint32_t events)
111{
112  if (fd < 0)
113    {
114      DBG ("invalid fd %d", fd);
115      return -1;
116    }
117  struct epoll_event evt;
118  memset (&evt, 0, sizeof (evt));
119  evt.events = events;
120  evt.data.fd = fd;
121  if (epoll_ctl (epfd, EPOLL_CTL_MOD, fd, &evt) < 0)
122    {
123      DBG ("epoll_ctl: %s fd %d", strerror (errno), fd);
124      return -1;
125    }
126  DBG ("fd %d moddified on epoll", fd);
127  return 0;
128}
129
130int
131del_epoll_fd (int epfd, int fd)
132{
133  if (fd < 0)
134    {
135      DBG ("invalid fd %d", fd);
136      return -1;
137    }
138  struct epoll_event evt;
139  memset (&evt, 0, sizeof (evt));
140  if (epoll_ctl (epfd, EPOLL_CTL_DEL, fd, &evt) < 0)
141    {
142      DBG ("epoll_ctl: %s fd %d", strerror (errno), fd);
143      return -1;
144    }
145  DBG ("fd %d removed from epoll", fd);
146  return 0;
147}
148
149/* Called when libmemif requests an update on any of its file descriptors */
150static int
151control_fd_update (int fd, uint8_t events, void *private_ctx)
152{
153  struct per_thread_data *ptd = (struct per_thread_data *) private_ctx;
154  uint32_t evt = 0;
155
156  if (ptd == NULL)
157    return -1;
158
159  /* convert memif event definitions to epoll events */
160  if (events & MEMIF_FD_EVENT_DEL)
161    return del_epoll_fd (ptd->epfd, fd);
162
163  if (events & MEMIF_FD_EVENT_READ)
164    evt |= EPOLLIN;
165  if (events & MEMIF_FD_EVENT_WRITE)
166    evt |= EPOLLOUT;
167
168  if (events & MEMIF_FD_EVENT_MOD)
169    return mod_epoll_fd (ptd->epfd, fd, evt);
170
171  return add_epoll_fd (ptd->epfd, fd, evt);
172}
173
174static int
175on_connect (memif_conn_handle_t conn, void *private_ctx)
176{
177  struct per_thread_data *ptd = (struct per_thread_data *) private_ctx;
178  struct memif_connection *c;
179  int i = 0;
180
181  while (i < ptd->if_num && ptd->conns[i].handle != conn)
182    i++;
183  c = &ptd->conns[i];
184
185  c->connected = true;
186  DBG ("Connected: %u", c->id);
187
188  memif_refill_queue (conn, 0, -1, 0);
189
190  return 0;
191}
192
193static int
194on_disconnect (memif_conn_handle_t conn, void *private_ctx)
195{
196  struct per_thread_data *ptd = (struct per_thread_data *) private_ctx;
197  struct memif_connection *c;
198  int i = 0;
199
200  while (i < ptd->if_num && ptd->conns[i].handle != conn)
201    i++;
202  c = &ptd->conns[i];
203
204  c->connected = false;
205  DBG ("Disconnected: %u", c->id);
206
207  return 0;
208}
209
210static int
211on_interrupt (memif_conn_handle_t conn, void *private_ctx, uint16_t qid)
212{
213  struct per_thread_data *ptd = (struct per_thread_data *) private_ctx;
214  struct memif_connection *c;
215  memif_buffer_t mbufs[ICMPR_MEMIF_BUFFER_NUM];
216  uint16_t rx = 0;
217  uint16_t tx = 0;
218  uint16_t ret;
219  memif_err_t err;
220  int i = 0;
221
222  memset (mbufs, 0, sizeof (memif_buffer_t) * ICMPR_MEMIF_BUFFER_NUM);
223
224  while (i < ptd->if_num && ptd->conns[i].handle != conn)
225    i++;
226  c = &ptd->conns[i];
227
228  /* receive data from shared memory buffers */
229  err = memif_rx_burst (conn, qid, mbufs, ICMPR_MEMIF_BUFFER_NUM, &rx);
230  if (err != MEMIF_ERR_SUCCESS)
231  {
232    printf ("memif_rx_burst: %s\n", memif_strerror (err));
233    goto error;
234  }
235
236  /* resolve packet in place (zer-copy slave) */
237  for (i = 0; i < rx; i++)
238    resolve_packet2 (mbufs[i].data, &mbufs[i].len, c->ip_addr);
239
240  /* enqueue received buffers */
241  err = memif_buffer_enq_tx (conn, qid, mbufs, i, &tx);
242  if (err != MEMIF_ERR_SUCCESS)
243  {
244    printf ("memif_rx_burst: %s\n", memif_strerror (err));
245    goto error;
246  }
247
248  /* mark shared memory buffers as free */
249  err = memif_refill_queue (conn, qid, rx, 0);
250  if (err != MEMIF_ERR_SUCCESS)
251  {
252    printf ("memif_rx_burst: %s\n", memif_strerror (err));
253    goto error;
254  }
255
256  err = memif_tx_burst (conn, qid, mbufs, tx, &ret);
257  if (err != MEMIF_ERR_SUCCESS)
258  {
259    printf ("memif_rx_burst: %s\n", memif_strerror (err));
260    goto error;
261  }
262
263  return 0;
264
265error:
266  memif_refill_queue (conn, qid, -1, 0);
267  return -1;
268}
269
270int
271poll_event (memif_per_thread_main_handle_t pt_main, int pcfd, int epfd,
272	    int timeout)
273{
274  struct epoll_event evt;
275  int en = 0;
276  uint8_t events = 0;
277  memset (&evt, 0, sizeof (evt));
278  evt.events = EPOLLIN | EPOLLOUT;
279
280  en = epoll_pwait (epfd, &evt, 1, timeout, NULL);
281  if (en < 0)
282    {
283      printf ("epoll_pwait: %s\n", strerror (errno));
284      return -1;
285    }
286
287  if (en > 0)
288    {
289      /* Cancel event polling */
290      if (evt.data.fd == pcfd)
291	return 1;
292
293      if (evt.events & EPOLLIN)
294	events |= MEMIF_FD_EVENT_READ;
295      if (evt.events & EPOLLOUT)
296	events |= MEMIF_FD_EVENT_WRITE;
297      if (evt.events & EPOLLERR)
298	events |= MEMIF_FD_EVENT_ERROR;
299
300      /* No need to use locks, as the database is separated */
301      memif_per_thread_control_fd_handler (pt_main, evt.data.fd, events);
302    }
303
304  return 0;
305}
306
307static void *
308icmpr_thread_fn (void *data)
309{
310  struct per_thread_data *ptd = (struct per_thread_data *) data;
311  int rv;
312  uint16_t i;
313  char socket_filename[ICMPR_SOCKET_FILENAME_LEN] = "/run/vpp/memif";
314  memif_conn_args_t args;
315
316  ptd->epfd = epoll_create (1);
317
318  ptd->conns = malloc (sizeof (struct memif_connection) * ptd->if_num);
319  if (ptd->conns == NULL)
320    {
321      printf ("%s\n", strerror (errno));
322      return NULL;
323    }
324
325  memset (ptd->conns, 0, sizeof (struct memif_connection) * ptd->if_num);
326
327  /* Initialize memif database (per thread). */
328  rv =
329    memif_per_thread_init (&ptd->pt_main, ptd, control_fd_update, APP_NAME,
330			   NULL, NULL, NULL);
331  if (rv != MEMIF_ERR_SUCCESS)
332    {
333      printf ("memif_per_thread_init: %s\n", memif_strerror (rv));
334      return NULL;
335    }
336
337  /*  Create unique socket. Each thread requires uniqueue socket. Interfaces created
338   *  on the same thread can share one socket.
339   */
340  socket_filename[strlen (socket_filename)] = '0' + ptd->index;
341  strncpy (socket_filename + strlen (socket_filename), ".sock", 5);
342  DBG ("socket_filename: %s", socket_filename);
343
344  rv = memif_per_thread_create_socket (ptd->pt_main, &ptd->socket_handle,
345				       socket_filename, ptd);
346  if (rv != MEMIF_ERR_SUCCESS)
347    {
348      printf ("memif_per_thread_create_socket: %s\n", memif_strerror (rv));
349      return NULL;
350    }
351
352  /* Create interfaces on this thread */
353  for (i = 0; i < ptd->if_num; i++)
354    {
355      ptd->conns[i].ip_addr[0] = 192;
356      ptd->conns[i].ip_addr[1] = 168;
357      ptd->conns[i].ip_addr[2] = ptd->index + 1;
358      ptd->conns[i].ip_addr[3] = i * 2 + 2;
359
360      memset (&args, 0, sizeof (args));
361
362      args.socket = ptd->socket_handle;
363      ptd->conns[i].id = i;
364      args.interface_id = i;
365
366      rv = memif_create (&ptd->conns[i].handle, &args, on_connect,
367			 on_disconnect, on_interrupt, ptd);
368      if (rv < 0)
369	{
370	  printf ("%s\n", memif_strerror (rv));
371	  return NULL;
372	}
373    }
374
375  /* Poll cancel file descriptor. When an event is received on this fd, exit thread
376   * loop in respective thread.
377   */
378  ptd->pcfd = eventfd (0, EFD_NONBLOCK);
379  if (ptd->pcfd < 0)
380    {
381      printf ("eventfd: %s\n", strerror (errno));
382      return NULL;
383    }
384  if (add_epoll_fd (ptd->epfd, ptd->pcfd, EPOLLIN) < 0)
385    {
386      printf ("Failed to add poll cancel fd to epfd.");
387      return NULL;
388    }
389
390  /* Thread loop */
391  ptd->running = true;
392  while (ptd->running)
393    {
394      rv = poll_event (ptd->pt_main, ptd->pcfd, ptd->epfd, -1);
395      if (rv != 0)
396	ptd->running = false;
397    }
398
399  /* Clean up */
400  for (i = 0; i < ptd->if_num; i++)
401    memif_delete (&ptd->conns[i].handle);
402
403  memif_delete_socket (&ptd->socket_handle);
404
405  memif_per_thread_cleanup (&ptd->pt_main);
406
407  free (ptd->conns);
408  close (ptd->pcfd);
409
410  return NULL;
411}
412
413static void
414icmpr_print_help ()
415{
416  printf
417    ("exit - Exits the application.\nhelp - Print this help.\nshow - Show memif interfaces\n");
418}
419
420static void
421icmpr_show_memifs ()
422{
423  struct icmpr_main *im = &icmpr_main;
424  int i, j;
425  memif_socket_handle_t sh;
426
427  printf ("%u Threads %u Memifs (per thread)\n", im->threads,
428	  im->per_thread_if_num);
429  printf ("=================================\n");
430
431  for (i = 0; i < im->threads; i++)
432    {
433      sh = im->ptd[i].socket_handle;
434      printf ("Thread %u %s\n", i, memif_get_socket_filename (sh));
435      for (j = 0; j < im->per_thread_if_num; j++)
436	{
437	  printf ("\tMemif id %u\n\t%s\n", im->ptd[i].conns[j].id,
438		  im->ptd[i].conns[j].connected ? "Link up" : "Link down");
439	}
440    }
441}
442
443int
444main (int argc, char **argv)
445{
446  struct icmpr_main *im = &icmpr_main;
447  int rv, i;
448  int option_index = 0;
449  bool running;
450  char buffer[ICMPR_BUFFER_LENGTH];
451  uint64_t b = 1;
452
453  memset (im, 0, sizeof (struct icmpr_main));
454
455  /* Default args */
456  im->threads = 4;
457  im->per_thread_if_num = 1;
458
459  /* Parse args */
460  while ((rv =
461	  getopt_long (argc, argv, "t:i:", options, &option_index)) != (-1))
462    {
463      switch (rv)
464	{
465	case 't':
466	  im->threads = strtoul (optarg, NULL, 10);
467	  break;
468	case 'i':
469	  im->per_thread_if_num = strtoul (optarg, NULL, 10);
470	  break;
471	default:
472	  break;
473	}
474    }
475
476  /* Check args */
477  if (im->threads < 1)
478    {
479      printf ("threads < 1\n");
480      exit (EXIT_FAILURE);
481    }
482
483  if (im->per_thread_if_num < 1)
484    {
485      printf ("if_num < 1\n");
486      exit (EXIT_FAILURE);
487    }
488
489  /* Allocate memory */
490  im->ptd = malloc (sizeof (struct per_thread_data) * im->threads);
491  if (im->ptd == NULL)
492    {
493      printf ("%s\n", strerror (errno));
494      return -1;
495    }
496  im->pthread = malloc (sizeof (pthread_t) * im->threads);
497  if (im->pthread == NULL)
498    {
499      printf ("%s\n", strerror (errno));
500      return -1;
501    }
502
503  /* Initialize and create threads */
504  for (i = 0; i < im->threads; i++)
505    {
506      im->ptd[i].index = i;
507      im->ptd[i].if_num = im->per_thread_if_num;
508      pthread_create (&im->pthread[i], NULL, icmpr_thread_fn, &im->ptd[i]);
509    }
510
511  icmpr_print_help ();
512
513  /* Main loop */
514  running = true;
515  while (running)
516    {
517      printf ("cmd: ");
518      memset (buffer, 0, ICMPR_BUFFER_LENGTH);
519      if (fgets (buffer, ICMPR_BUFFER_LENGTH, stdin) != buffer)
520	{
521	  printf ("%s\n", strerror (errno));
522	  running = false;
523	}
524
525      if (strncmp (buffer, "exit", 4) == 0)
526	running = false;
527      else if (strncmp (buffer, "help", 4) == 0)
528	icmpr_print_help ();
529      else if (strncmp (buffer, "show", 4) == 0)
530	icmpr_show_memifs ();
531    }
532
533  for (i = 0; i < im->threads; i++)
534    {
535      /* Stop polling */
536      rv = write (im->ptd[i].pcfd, &b, sizeof (b));
537      if (rv < 0)
538	{
539	  printf ("Failed to cancel polling. %s\n", strerror (errno));
540	  exit (EXIT_FAILURE);
541	}
542      pthread_join (im->pthread[i], NULL);
543    }
544
545  free (im->pthread);
546  free (im->ptd);
547
548  return 0;
549}
550