FD.io VPP  v18.01.2-1-g9b554f3
Vector Packet Processing
icmp_proto.c
Go to the documentation of this file.
1 /*
2  *------------------------------------------------------------------
3  * Copyright (c) 2017 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 <stdint.h>
19 #include <net/if.h>
20 #include <sys/types.h>
21 #include <fcntl.h>
22 #include <sys/ioctl.h>
23 #include <sys/socket.h>
24 #include <sys/un.h>
25 #include <sys/uio.h>
26 #include <sys/mman.h>
27 #include <sys/prctl.h>
28 #include <inttypes.h>
29 #include <string.h>
30 #include <stdio.h>
31 #include <netdb.h>
32 #include <linux/ip.h>
33 #include <linux/icmp.h>
34 #include <arpa/inet.h>
35 #include <stdlib.h>
36 #include <netinet/if_ether.h>
37 #include <net/if_arp.h>
38 #include <asm/byteorder.h>
39 #include <byteswap.h>
40 
41 #include <icmp_proto.h>
42 
43 static uint16_t
44 cksum (void *addr, ssize_t len)
45 {
46  char *data = (char *) addr;
47 
48  uint32_t acc = 0xffff;
49 
50  ssize_t i;
51  for (i = 0; (i + 1) < len; i += 2)
52  {
53  uint16_t word;
54  memcpy (&word, data + i, 2);
55  acc += ntohs (word);
56  if (acc > 0xffff)
57  acc -= 0xffff;
58  }
59 
60  if (len & 1)
61  {
62  uint16_t word = 0;
63  memcpy (&word, data + len - 1, 1);
64  acc += ntohs (word);
65  if (acc > 0xffff)
66  acc -= 0xffff;
67  }
68  return htons (~acc);
69 }
70 
71 int
72 print_packet (void *pck)
73 {
74  if (pck == NULL)
75  {
76  printf ("ICMP_PROTO: no data\n");
77  return -1;
78  }
79  struct iphdr *ip;
80  struct icmphdr *icmp;
81  ip = (struct iphdr *) pck;
82  icmp = (struct icmphdr *) (pck + sizeof (struct iphdr));
83  printf ("received packet:\n");
84  printf ("\tiphdr:\n");
85  printf ("\t\tihl: %u\n\t\tversion: %u\n\t\tlen: %u\n\t\tid: %u\n",
86  ip->ihl, ip->version, __bswap_16 (ip->tot_len), ip->id);
87  printf ("\t\tprotocol: %u\n", ip->protocol);
88 
89  printf ("\t\tsaddr: ");
90  int i;
91  for (i = 0; i < 4; i++)
92  {
93  printf ("%u.", ((uint8_t *) & ip->saddr)[i]);
94  }
95  printf ("\n");
96 
97  printf ("\t\tdaddr: ");
98  for (i = 0; i < 4; i++)
99  {
100  printf ("%u.", ((uint8_t *) & ip->daddr)[i]);
101  }
102  printf ("\n");
103  printf ("\ticmphdr:\n");
104  printf ("\t\ttype: %s\n",
105  (icmp->type == ICMP_ECHO) ? "ICMP_ECHO" : "ICMP_ECHOREPLY");
106 
107  return 0;
108 }
109 
110 static ssize_t
111 resolve_arp (void *arp)
112 {
113  struct arphdr *resp = (struct arphdr *) arp;
114 
115  resp->ar_hrd = __bswap_16 (ARPHRD_ETHER);
116 
117  resp->ar_pro = __bswap_16 (0x0800);
118 
119  resp->ar_hln = 6;
120  resp->ar_pln = 4;
121 
122  resp->ar_op = __bswap_16 (ARPOP_REPLY);
123 
124  return sizeof (struct arphdr);
125 }
126 
127 static ssize_t
128 resolve_eth_arp (struct ether_arp *eth_arp, void *eth_arp_resp,
129  uint8_t ip_addr[4])
130 {
131  struct ether_arp *resp = (struct ether_arp *) eth_arp_resp;
132 
133  resolve_arp (&resp->ea_hdr);
134 
135  memcpy (resp->arp_tha, eth_arp->arp_sha, 6);
136  memcpy (resp->arp_tpa, eth_arp->arp_spa, 4);
137 
138  memcpy (resp->arp_sha,
139  (((struct ether_header *) (eth_arp_resp -
140  sizeof (struct
141  ether_header)))->ether_shost),
142  6);
143 
144  memcpy (resp->arp_spa, ip_addr, 4);
145 
146  return sizeof (struct ether_arp);
147 }
148 
149 static ssize_t
150 resolve_eth (struct ether_header *eth, void *eth_resp)
151 {
152  struct ether_header *resp = (struct ether_header *) eth_resp;
153  memcpy (resp->ether_dhost, eth->ether_shost, 6);
154 
155  uint8_t hw_addr[6];
156  int i;
157  for (i = 0; i < 6; i++)
158  {
159  hw_addr[i] = 'a';
160  }
161  memcpy (resp->ether_shost, hw_addr, 6);
162 
163  resp->ether_type = eth->ether_type;
164 
165  return sizeof (struct ether_header);
166 }
167 
168 static ssize_t
169 resolve_ip (struct iphdr *ip, void *ip_resp, uint8_t ip_addr[4])
170 {
171  struct iphdr *resp = (struct iphdr *) ip_resp;
172  resp->ihl = 5;
173  resp->version = 4;
174  resp->tos = 0;
175  /*len updated later */
176  resp->tot_len = 0x0000;
177  resp->id = 0;
178  resp->frag_off = 0;
179  resp->ttl = 0x40;
180  resp->protocol = 1;
181  ((uint8_t *) & resp->saddr)[0] = ip_addr[0];
182  ((uint8_t *) & resp->saddr)[1] = ip_addr[1];
183  ((uint8_t *) & resp->saddr)[2] = ip_addr[2];
184  ((uint8_t *) & resp->saddr)[3] = ip_addr[3];
185  resp->daddr = ip->saddr;
186 
187  /* resp->check = cksum (resp, sizeof (struct iphdr)); */
188 
189  return sizeof (struct iphdr);
190 }
191 
192 static ssize_t
193 resolve_icmp (struct icmphdr *icmp, void *icmp_resp)
194 {
195  struct icmphdr *resp = (struct icmphdr *) icmp_resp;
196  resp->type = 0x00;
197  resp->code = 0;
198  resp->un.echo.id = icmp->un.echo.id;
199  resp->un.echo.sequence = icmp->un.echo.sequence;
200 
201  /*resp->checksum = cksum (resp, sizeof (struct icmphdr)); */
202 
203  return sizeof (struct icmphdr);
204 }
205 
206 int
207 resolve_packet (void *in_pck, ssize_t in_size,
208  void *out_pck, uint32_t * out_size, uint8_t ip_addr[4])
209 {
210  struct ether_header *eh;
211  struct ether_arp *eah;
212  struct iphdr *ip, *ip_out;
213  struct icmphdr *icmp;
214  *out_size = 0;
215 
216  if ((in_pck == NULL) || (out_pck == NULL))
217  return -1;
218 
219  eh = (struct ether_header *) in_pck;
220  *out_size = resolve_eth (eh, out_pck);
221 
222  if (eh->ether_type == 0x0608)
223  {
224  eah = (struct ether_arp *) (in_pck + *out_size);
225  *out_size += resolve_eth_arp (eah, out_pck + *out_size, ip_addr);
226 
227  }
228  else if (eh->ether_type == 0x0008)
229  {
230 #ifdef ICMP_DBG
231  print_packet (in_pck + *out_size);
232 #endif
233  ip = (struct iphdr *) (in_pck + *out_size);
234  ip_out = (struct iphdr *) (out_pck + *out_size);
235  *out_size += resolve_ip (ip, out_pck + *out_size, ip_addr);
236  if (ip->protocol == 1)
237  {
238  icmp = (struct icmphdr *) (in_pck + *out_size);
239  *out_size += resolve_icmp (icmp, out_pck + *out_size);
240  ((struct icmphdr *) (out_pck + *out_size -
241  sizeof (struct icmphdr)))->checksum =
242  cksum (out_pck + *out_size - sizeof (struct icmphdr),
243  sizeof (struct icmphdr));
244  /* payload */
245  memcpy (out_pck + *out_size, in_pck + *out_size,
246  in_size - *out_size);
247  *out_size = in_size;
248  ip_out->tot_len =
249  __bswap_16 (*out_size - sizeof (struct ether_header));
250  ip_out->check = cksum (ip_out, sizeof (struct iphdr));
251  }
252  }
253  return 0;
254 }
255 
256 static ssize_t
257 generate_eth (struct ether_header *eh, uint8_t hw_daddr[6])
258 {
259  uint8_t hw_addr[6];
260  int i;
261  for (i = 0; i < 6; i++)
262  {
263  hw_addr[i] = 'a';
264  }
265  memcpy (eh->ether_shost, hw_addr, 6);
266  memcpy (eh->ether_dhost, hw_daddr, 6);
267 
268  eh->ether_type = 0x0008;
269 
270  return sizeof (struct ether_header);
271 }
272 
273 static ssize_t
274 generate_ip (struct iphdr *ip, uint8_t saddr[4], uint8_t daddr[4])
275 {
276  ip->ihl = 5;
277  ip->version = 4;
278  ip->tos = 0;
279  /*len updated later */
280  ip->tot_len = 0x5400;
281  ip->id = 0;
282  ip->frag_off = 0;
283  ip->ttl = 0x40;
284  ip->protocol = 1;
285  /* saddr */
286  ((uint8_t *) & ip->saddr)[0] = saddr[0];
287  ((uint8_t *) & ip->saddr)[1] = saddr[1];
288  ((uint8_t *) & ip->saddr)[2] = saddr[2];
289  ((uint8_t *) & ip->saddr)[3] = saddr[3];
290  /* daddr */
291  ((uint8_t *) & ip->daddr)[0] = daddr[0];
292  ((uint8_t *) & ip->daddr)[1] = daddr[1];
293  ((uint8_t *) & ip->daddr)[2] = daddr[2];
294  ((uint8_t *) & ip->daddr)[3] = daddr[3];
295 
296  ip->check = cksum (ip, sizeof (struct iphdr));
297 
298  return sizeof (struct iphdr);
299 }
300 
301 static ssize_t
302 generate_icmp (struct icmphdr *icmp, uint32_t seq)
303 {
304  icmp->type = ICMP_ECHO;
305  icmp->code = 0;
306  icmp->un.echo.id = 0;
307  icmp->un.echo.sequence = seq;
308 
309  return sizeof (struct icmphdr);
310 }
311 
312 int
313 generate_packet (void *pck, uint32_t * size, uint8_t saddr[4],
314  uint8_t daddr[4], uint8_t hw_daddr[6], uint32_t seq)
315 {
316  struct ether_header *eh;
317  struct iphdr *ip;
318  struct icmphdr *icmp;
319 
320  *size = 0;
321 
322  eh = (struct ether_header *) pck;
323  *size += generate_eth (eh, hw_daddr);
324 
325  ip = (struct iphdr *) (pck + *size);
326  *size += generate_ip (ip, saddr, daddr);
327 
328  icmp = (struct icmphdr *) (pck + *size);
329  *size += generate_icmp (icmp, seq);
330 
331  ((struct icmphdr *) (pck + *size - sizeof (struct icmphdr)))->checksum =
332  cksum (pck + *size - sizeof (struct icmphdr), sizeof (struct icmphdr));
333 
334  ip->tot_len = __bswap_16 (*size - sizeof (struct ether_header));
335  ip->check = 0;
336  ip->check = cksum (ip, sizeof (struct iphdr));
337 
338  return 0;
339 }
sll srl srl sll sra u16x4 i
Definition: vector_sse2.h:337
static ssize_t generate_ip(struct iphdr *ip, uint8_t saddr[4], uint8_t daddr[4])
Definition: icmp_proto.c:274
#define NULL
Definition: clib.h:55
static ssize_t resolve_arp(void *arp)
Definition: icmp_proto.c:111
int resolve_packet(void *in_pck, ssize_t in_size, void *out_pck, uint32_t *out_size, uint8_t ip_addr[4])
Definition: icmp_proto.c:207
int print_packet(void *pck)
Definition: icmp_proto.c:72
static ssize_t resolve_eth(struct ether_header *eth, void *eth_resp)
Definition: icmp_proto.c:150
static ssize_t resolve_eth_arp(struct ether_arp *eth_arp, void *eth_arp_resp, uint8_t ip_addr[4])
Definition: icmp_proto.c:128
static ssize_t resolve_icmp(struct icmphdr *icmp, void *icmp_resp)
Definition: icmp_proto.c:193
static ssize_t generate_icmp(struct icmphdr *icmp, uint32_t seq)
Definition: icmp_proto.c:302
int generate_packet(void *pck, uint32_t *size, uint8_t saddr[4], uint8_t daddr[4], uint8_t hw_daddr[6], uint32_t seq)
Definition: icmp_proto.c:313
u64 size
Definition: vhost-user.h:76
i64 word
Definition: types.h:111
static ssize_t generate_eth(struct ether_header *eh, uint8_t hw_daddr[6])
Definition: icmp_proto.c:257
static uint16_t cksum(void *addr, ssize_t len)
Definition: icmp_proto.c:44
vhost_vring_addr_t addr
Definition: vhost-user.h:83
static ssize_t resolve_ip(struct iphdr *ip, void *ip_resp, uint8_t ip_addr[4])
Definition: icmp_proto.c:169