113 lines
2.9 KiB
C
113 lines
2.9 KiB
C
/*
|
|
* VasEBoot -- GRand Unified Bootloader
|
|
* Copyright (C) 2010,2011 Free Software Foundation, Inc.
|
|
*
|
|
* VasEBoot is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* VasEBoot is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with VasEBoot. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <VasEBoot/net.h>
|
|
#include <VasEBoot/net/ip.h>
|
|
#include <VasEBoot/net/netbuff.h>
|
|
|
|
struct icmp_header
|
|
{
|
|
VasEBoot_uint8_t type;
|
|
VasEBoot_uint8_t code;
|
|
VasEBoot_uint16_t checksum;
|
|
} VasEBoot_PACKED;
|
|
|
|
struct ping_header
|
|
{
|
|
VasEBoot_uint16_t id;
|
|
VasEBoot_uint16_t seq;
|
|
} VasEBoot_PACKED;
|
|
|
|
enum
|
|
{
|
|
ICMP_ECHO_REPLY = 0,
|
|
ICMP_ECHO = 8,
|
|
};
|
|
|
|
VasEBoot_err_t
|
|
VasEBoot_net_recv_icmp_packet (struct VasEBoot_net_buff *nb,
|
|
struct VasEBoot_net_network_level_interface *inf,
|
|
const VasEBoot_net_link_level_address_t *ll_src,
|
|
const VasEBoot_net_network_level_address_t *src)
|
|
{
|
|
struct icmp_header *icmph;
|
|
VasEBoot_err_t err;
|
|
VasEBoot_uint16_t checksum;
|
|
|
|
/* Ignore broadcast. */
|
|
if (!inf)
|
|
{
|
|
VasEBoot_netbuff_free (nb);
|
|
return VasEBoot_ERR_NONE;
|
|
}
|
|
|
|
icmph = (struct icmp_header *) nb->data;
|
|
|
|
if (nb->tail - nb->data < (VasEBoot_ssize_t) sizeof (*icmph))
|
|
{
|
|
VasEBoot_netbuff_free (nb);
|
|
return VasEBoot_ERR_NONE;
|
|
}
|
|
|
|
checksum = icmph->checksum;
|
|
icmph->checksum = 0;
|
|
if (checksum != VasEBoot_net_ip_chksum (nb->data, nb->tail - nb->data))
|
|
{
|
|
icmph->checksum = checksum;
|
|
return VasEBoot_ERR_NONE;
|
|
}
|
|
icmph->checksum = checksum;
|
|
|
|
err = VasEBoot_netbuff_pull (nb, sizeof (*icmph));
|
|
if (err)
|
|
return err;
|
|
|
|
switch (icmph->type)
|
|
{
|
|
case ICMP_ECHO:
|
|
{
|
|
struct VasEBoot_net_buff *nb_reply;
|
|
struct icmp_header *icmphr;
|
|
if (icmph->code)
|
|
break;
|
|
nb_reply = VasEBoot_netbuff_make_pkt (nb->tail - nb->data + sizeof (*icmphr));
|
|
if (!nb_reply)
|
|
{
|
|
VasEBoot_netbuff_free (nb);
|
|
return VasEBoot_errno;
|
|
}
|
|
VasEBoot_memcpy (nb_reply->data + sizeof (*icmphr), nb->data, nb->tail - nb->data);
|
|
icmphr = (struct icmp_header *) nb_reply->data;
|
|
icmphr->type = ICMP_ECHO_REPLY;
|
|
icmphr->code = 0;
|
|
icmphr->checksum = 0;
|
|
icmphr->checksum = VasEBoot_net_ip_chksum ((void *) nb_reply->data,
|
|
nb_reply->tail - nb_reply->data);
|
|
err = VasEBoot_net_send_ip_packet (inf, src, ll_src,
|
|
nb_reply, VasEBoot_NET_IP_ICMP);
|
|
|
|
VasEBoot_netbuff_free (nb);
|
|
VasEBoot_netbuff_free (nb_reply);
|
|
return err;
|
|
}
|
|
};
|
|
|
|
VasEBoot_netbuff_free (nb);
|
|
return VasEBoot_ERR_NONE;
|
|
}
|