rpp-test-sw/commands/cmd_ping.c

   1 /*
   2  * Copyright (C) 2012-2019 Czech Technical University in Prague
   3  *
   4  * Created on: 7.3.2019
   5  *
   6  * Authors:
   7  *     - Jakub Nejedly
   8  *
   9  * This document contains proprietary information belonging to Czech
  10  * Technical University in Prague. Passing on and copying of this
  11  * document, and communication of its contents is not permitted
  12  * without prior written authorization.
  13  *
  14  * File : cmd_ping.c
  15  *
  16  * Abstract:
  17  *
  18  *
  19  */
  20
  21 #include "cmd_ping.h"
  22
  23 #ifndef DOCGEN
  24
  25
  26 #include "cmdproc_freertos.h"
  27 #include "rpp/rpp.h"
  28
  29 #include "lwip/mem.h"
  30 #include "lwip/raw.h"
  31 #include "lwip/icmp.h"
  32 #include "lwip/netif.h"
  33 #include "lwip/sys.h"
  34 #include "lwip/timers.h"
  35 #include "lwip/inet_chksum.h"
  36
  37 #include "lwip/sockets.h"
  38 #include "lwip/inet.h"
  39
  40
  41 #define PING_RCV_TIMEO                    1000
  42 #define PING_DATA_SIZE                    32
  43 #define PING_ID                           0xAFAF
  44 #define PING_DELAY                        1000
  45
  46 static u16_t ping_seq_num;
  47 static u32_t ping_time;
  48
  49 static void ping_prepare_echo( struct icmp_echo_hdr *iecho, u16_t len){
  50
  51         size_t i;
  52         size_t data_len = len - sizeof(struct icmp_echo_hdr);
  53
  54         ICMPH_TYPE_SET(iecho, ICMP_ECHO);
  55         ICMPH_CODE_SET(iecho, 0);
  56         iecho->chksum = 0;
  57         iecho->id     = PING_ID;
  58         iecho->seqno  = htons(++ping_seq_num);
  59
  60         /* fill the additional data buffer with some data */
  61         for(i = 0; i < data_len; i++) {
  62                 ((char*)iecho)[sizeof(struct icmp_echo_hdr) + i] = (char)i;
  63         }
  64
  65         iecho->chksum = inet_chksum(iecho, len);
  66 }
  67
  68 static err_t ping_send(int s, ip_addr_t *addr) {
  69
  70         int err;
  71         struct icmp_echo_hdr *iecho;
  72         struct sockaddr_in to;
  73         size_t ping_size = sizeof(struct icmp_echo_hdr) + PING_DATA_SIZE;
  74         LWIP_ASSERT("ping_size is too big", ping_size <= 0xffff);
  75
  76         iecho = (struct icmp_echo_hdr *)mem_malloc((mem_size_t)ping_size);
  77         if (!iecho) {
  78                 return ERR_MEM;
  79         }
  80
  81         ping_prepare_echo(iecho, (u16_t)ping_size);
  82
  83         to.sin_len = sizeof(to);
  84         to.sin_family = AF_INET;
  85         inet_addr_from_ipaddr(&to.sin_addr, addr);
  86
  87         err = lwip_sendto(s, iecho, ping_size, 0, (struct sockaddr*)&to, sizeof(to));
  88
  89         mem_free(iecho);
  90
  91         return (err ? ERR_OK : ERR_VAL);
  92 }
  93
  94 static void ping_recv(int s){
  95
  96         char buf[256];
  97         int fromlen, len;
  98         struct sockaddr_in from;
  99         struct ip_hdr *iphdr;
 100         struct icmp_echo_hdr *iecho;
 101
 102         while((len = lwip_recvfrom(s, buf, sizeof(buf), 0, (struct sockaddr*)&from, (socklen_t*)&fromlen)) > 0
 103                   && (unsigned long)(sys_now() - ping_time) < PING_DELAY-10) {
 104                 if (len >= (int)(sizeof(struct ip_hdr)+sizeof(struct icmp_echo_hdr))) {
 105
 106                         ip_addr_t fromaddr;
 107                         inet_addr_to_ipaddr(&fromaddr, &from.sin_addr);
 108
 109                         rpp_sci_printf(" time=%lu ms\n", (unsigned long)(sys_now() - ping_time));
 110
 111                         iphdr = (struct ip_hdr *)buf;
 112                         iecho = (struct icmp_echo_hdr *)(buf + (IPH_HL(iphdr) * 4));
 113                         if ((iecho->id == PING_ID) && (iecho->seqno == htons(ping_seq_num))) {
 114                         /* do some ping result processing */
 115                                 return;
 116                         } else {
 117                                 rpp_sci_printf(" droped\n");
 118                         }
 119                 }
 120         }
 121         rpp_sci_printf(" time=%lu ms - timeout \n", (unsigned long)(sys_now() - ping_time));
 122
 123         /* do some ping result processing */
 124         return;
 125 }
 126
 127
 128 int cmd_do_eth_ping(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]){
 129
 130         // test ethinit
 131         if (!isPostInitialized()) {
 132                 rpp_sci_printf("Eth not initialized run 'ethinit' command first.\n");
 133                 return FAILURE;
 134         }
 135         err_t err = ERR_OK;
 136
 137         // parse params
 138         ip_addr_t ping_target;
 139
 140         if ( (err = rpp_eth_stringToIP(&ping_target, (uint8_t *)param[1])) != SUCCESS ) {
 141                 rpp_sci_printf("ERR: IP address format\r\n");
 142                 return BAD_OPTION;
 143         }
 144
 145         // socket creation
 146         int s;
 147         int timeout = PING_RCV_TIMEO;
 148         if ((s = lwip_socket(AF_INET, SOCK_RAW, IP_PROTO_ICMP)) < 0) {
 149                 rpp_sci_printf("ERR: lwip socket init\r\n");
 150                 return FAILURE;
 151         }
 152
 153         /*int ret = */
 154         lwip_setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
 155         //rpp_sci_printf("Socket opt result %d \n", ret);
 156
 157         while (1) {
 158
 159                 if (ping_send(s, &ping_target) == ERR_OK) {
 160                         rpp_sci_printf("ping: send to ");
 161                         rpp_sci_printf("%s", inet_ntoa(ping_target));
 162                         //rpp_sci_printf("\n");
 163
 164                         ping_time = sys_now();
 165                         ping_recv(s);
 166                 } else {
 167                         rpp_sci_printf("ping: send to ");
 168                         rpp_sci_printf("%s", inet_ntoa(ping_target));
 169                         rpp_sci_printf(" - error during sending \n");
 170                 }
 171                 sys_msleep(PING_DELAY);
 172         }
 173 }
 174
 175
 176
 177 #endif  /* DOCGEN */
 178
 179 cmd_des_t const cmd_des_ethping = {
 180         0, 0,
 181         "ethping","Command to send and receive ICMP packets",
 182         "### Command syntax ###\n"
 183         "\n"
 184         "    ethping [IP_address] \n"
 185         "\n"
 186         "### Description ###\n"
 187         "basic implementation of outgoing ping"
 188         "\n"
 189         "### Example ###\n"
 190         "\n"
 191         "    --> ethping 8.8.8.8\n",
 192         CMD_HANDLER(cmd_do_eth_ping), (void *)&cmd_list_ping
 193 };
 194
 195 /** List of commands for adc, defined as external */
 196 cmd_des_t const *cmd_list_ping[] = {
 197         &cmd_des_ethping,
 198         NULL
 199 };