static void
ping_recv(int s, const ip_addr_t *addr)
{
char buf[200];
socklen_t fromlen;
int len;
struct sockaddr_storage from;
ip_addr_t ip_from;
LWIP_UNUSED_ARG(addr);
len = lwip_recvfrom(s, buf, sizeof(buf), 0, (struct sockaddr*)&from, &fromlen);
#if LWIP_IPV4
if(!IP_IS_V6(addr)) {
struct sockaddr_in *from4 = (struct sockaddr_in*)&from;
inet_addr_to_ipaddr(ip_2_ip4(&ip_from), &from4->sin_addr);
}
#endif /* LWIP_IPV4 */
#if LWIP_IPV6
if(IP_IS_V6(addr)) {
struct sockaddr_in6 *from6 = (struct sockaddr_in6*)&from;
inet6_addr_to_ip6addr(ip_2_ip6(&ip_from), &from6->sin6_addr);
}
#endif /* LWIP_IPV6 */
printf("Received %d bytes from %s\n", len, ipaddr_ntoa(&ip_from));
}
static void
ping_recv(int s)
{
char buf[64];
int fromlen, len;
struct sockaddr_in from;
struct ip_hdr *iphdr;
struct icmp_echo_hdr *iecho;
fromlen = sizeof(from);
while((len = lwip_recvfrom(s, buf, sizeof(buf), 0, (struct sockaddr*)&from, (socklen_t*)&fromlen)) > 0) {
if (len >= (int)(sizeof(struct ip_hdr)+sizeof(struct icmp_echo_hdr))) {
ip_addr_t fromaddr;
inet_addr_to_ipaddr(&fromaddr, &from.sin_addr);
LWIP_DEBUGF( PING_DEBUG, ("ping: recv "));
ip_addr_debug_print(PING_DEBUG, &fromaddr);
LWIP_DEBUGF( PING_DEBUG, (" %"U32_F" ms\r\n", (sys_now() - ping_time)));
iphdr = (struct ip_hdr *)buf;
iecho = (struct icmp_echo_hdr *)(buf + (IPH_HL(iphdr) * 4));
if ((iecho->id == PING_ID) && (iecho->seqno == htons(ping_seq_num))) {
/* do some ping result processing */
PING_RESULT((ICMPH_TYPE(iecho) == ICMP_ER));
return;
} else {
LWIP_DEBUGF( PING_DEBUG, ("ping: drop\r\n"));
}
}
}
if (len == 0) {
LWIP_DEBUGF( PING_DEBUG, ("ping: recv - %"U32_F" ms - timeout\r\n", (sys_now()-ping_time)));
}
/* do some ping result processing */
PING_RESULT(0);
}
static void
ping_recv(int s)
{
char* buf = NULL;
int fromlen, len;
struct sockaddr_in from;
struct ip_hdr *iphdr;
struct icmp_echo_hdr *iecho;
int ms;
BOOL bResult = FALSE;
//Allocate a buffer to contain the received data.
buf = (char*)KMemAlloc(1500,KMEM_SIZE_TYPE_ANY);
if(NULL == buf)
{
return;
}
while((len = lwip_recvfrom(s, buf, 1500, 0, (struct sockaddr*)&from, (socklen_t*)&fromlen)) > 0)
{
if(len >= (int)(sizeof(struct ip_hdr)+sizeof(struct icmp_echo_hdr)))
{
ip_addr_t fromaddr;
inet_addr_to_ipaddr(&fromaddr, &from.sin_addr);
//Get times between sent and receive.
ms = sys_now() - ping_time;
ms *= SYSTEM_TIME_SLICE;
iphdr = (struct ip_hdr *)buf;
iecho = (struct icmp_echo_hdr *)(buf + (IPH_HL(iphdr) * 4));
if (((iecho->id == PING_ID) && (iecho->seqno == htons(ping_seq_num)) && iecho->type == ICMP_ER))
{
len = len - sizeof(struct ip_hdr) - sizeof(struct icmp_echo_hdr); //Adjust received data's length,since it
//includes IP and ICMP headers.
_hx_printf(" [%d] Reply from %s,size = %d,time = %d(ms)\r\n",ping_pkt_seq,inet_ntoa(fromaddr),len,ms);
ping_succ ++;
bResult = TRUE;
}
else
{
//printf(" ping : Received invalid replay,drop it.\r\n");
}
}
}
if (!bResult)
{
_hx_printf(" [%d] Request time out.\r\n",ping_pkt_seq);
}
if(buf) //Release it.
{
KMemFree(buf,KMEM_SIZE_TYPE_ANY,0);
}
}
/******************************************************************************
* FunctionName : user_devicefind_data_process
* Description : Processing the received data from the host
* Parameters : arg -- Additional argument to pass to the callback function
* pusrdata -- The received data (or NULL when the connection has been closed!)
* length -- The length of received data
* Returns : none
*******************************************************************************/
LOCAL void
user_devicefind_data_process(char *pusrdata, unsigned short length, struct sockaddr_in *premote_addr)
{
char *DeviceBuffer;//40
char *Device_mac_buffer;//60
char hwaddr[6];
int len;
struct ip_info ipconfig;
struct ip_info ipconfig_r;
if (pusrdata == NULL) {
return;
}
if (wifi_get_opmode() != STATION_MODE) {
wifi_get_ip_info(SOFTAP_IF, &ipconfig);
wifi_get_macaddr(SOFTAP_IF, hwaddr);
inet_addr_to_ipaddr(&ipconfig_r.ip,&premote_addr->sin_addr);
if(!ip_addr_netcmp(&ipconfig_r.ip, &ipconfig.ip, &ipconfig.netmask)) {
//printf("udpclient connect with sta\n");
wifi_get_ip_info(STATION_IF, &ipconfig);
wifi_get_macaddr(STATION_IF, hwaddr);
}
} else {
wifi_get_ip_info(STATION_IF, &ipconfig);
wifi_get_macaddr(STATION_IF, hwaddr);
}
//DF_DEBUG("%s\n", pusrdata);
DeviceBuffer = (char*)malloc(len_ip_rsp);
memset(DeviceBuffer, 0, len_ip_rsp);
if (length == strlen(device_find_request) &&
strncmp(pusrdata, device_find_request, strlen(device_find_request)) == 0) {
sprintf(DeviceBuffer, "%s" MACSTR " " IPSTR, device_find_response_ok,
MAC2STR(hwaddr), IP2STR(&ipconfig.ip));
length = strlen(DeviceBuffer);
DF_DEBUG("%s %d\n", DeviceBuffer,length);
sendto(sock_fd,DeviceBuffer, length, 0, (struct sockaddr *)premote_addr, sizeof(struct sockaddr_in));
} else if (length == (strlen(device_find_request) + 18)) {
Device_mac_buffer = (char*)malloc(len_mac_msg);
memset(Device_mac_buffer, 0, len_mac_msg);
sprintf(Device_mac_buffer, "%s " MACSTR , device_find_request, MAC2STR(hwaddr));
if (strncmp(Device_mac_buffer, pusrdata, strlen(device_find_request) + 18) == 0){
sprintf(DeviceBuffer, "%s" MACSTR " " IPSTR, device_find_response_ok,
MAC2STR(hwaddr), IP2STR(&ipconfig.ip));
length = strlen(DeviceBuffer);
DF_DEBUG("%s %d\n", DeviceBuffer,length);
sendto(sock_fd,DeviceBuffer, length, 0, (struct sockaddr *)premote_addr, sizeof(struct sockaddr_in));
}
if(Device_mac_buffer)free(Device_mac_buffer);
}
if(DeviceBuffer)free(DeviceBuffer);
}
int netstack_send_echo(u8 *ripaddr, u16 size, u16 seqno,
struct netstack_echo_reply *reply)
{
u64 ts;
int s, i, err;
char buf[64];
size_t fromlen, off, len = sizeof(struct icmp_echo_hdr) + size;
ip_addr_t to_addr, from_addr;
struct sockaddr_in sock;
struct ip_hdr *iphdr;
struct icmp_echo_hdr *iecho;
LWIP_ASSERT("ping_size is too big\n", len <= 0xffff);
/* Prepare target address */
IP4_ADDR(&to_addr, ripaddr[0],ripaddr[1],ripaddr[2],ripaddr[3]);
/* Open RAW socket */
if ((s = lwip_socket(AF_INET, SOCK_RAW, IP_PROTO_ICMP)) < 0) {
vmm_printf("%s: failed to open ICMP socket\n", __func__);
return VMM_EFAIL;
}
/* Set socket option */
i = PING_RCV_TIMEO;
lwip_setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, &i, sizeof(i));
/* Prepare socket address */
sock.sin_len = sizeof(sock);
sock.sin_family = AF_INET;
inet_addr_from_ipaddr(&sock.sin_addr, &to_addr);
/* Prepare ECHO request */
iecho = (struct icmp_echo_hdr *)vmm_zalloc(len);
if (!iecho) {
return VMM_ENOMEM;
}
ICMPH_TYPE_SET(iecho, ICMP_ECHO);
ICMPH_CODE_SET(iecho, 0);
iecho->chksum = 0;
iecho->id = PING_ID;
iecho->seqno = htons(seqno);
for (i = 0; i < size; i++) {
((char*)iecho)[sizeof(struct icmp_echo_hdr) + i] = (char)i;
}
iecho->chksum = inet_chksum(iecho, len);
/* Send ECHO request */
err = lwip_sendto(s, iecho, len, 0,
(struct sockaddr*)&sock, sizeof(sock));
vmm_free(iecho);
if (!err) {
return VMM_EFAIL;
}
/* Get reference timestamp */
ts = vmm_timer_timestamp();
/* Wait for ECHO reply */
err = VMM_EFAIL;
off = lwip_recvfrom(s, buf, sizeof(buf), 0,
(struct sockaddr*)&sock, (socklen_t*)&fromlen);
if (off >= (sizeof(struct ip_hdr) + sizeof(struct icmp_echo_hdr))) {
inet_addr_to_ipaddr(&from_addr, &sock.sin_addr);
iphdr = (struct ip_hdr *)buf;
iecho = (struct icmp_echo_hdr *)(buf + (IPH_HL(iphdr) * 4));
if ((iecho->id == PING_ID) &&
(iecho->seqno == htons(seqno))) {
reply->ripaddr[0] = ip4_addr1(&from_addr);
reply->ripaddr[1] = ip4_addr2(&from_addr);
reply->ripaddr[2] = ip4_addr3(&from_addr);
reply->ripaddr[3] = ip4_addr4(&from_addr);
reply->ttl = IPH_TTL(iphdr);
reply->len = len;
reply->seqno = seqno;
reply->rtt =
udiv64(vmm_timer_timestamp() - ts, 1000);
err = VMM_OK;
}
}
while (off < len) {
off = lwip_recvfrom(s, buf, sizeof(buf), 0,
(struct sockaddr*)&sock, (socklen_t*)&fromlen);
}
/* Close RAW socket */
lwip_close(s);
return err;
}
