firmware/icmp.c (2342395) - flaneth · BlinkenArea

icmp.c

/* flaneth - flash and ethernet
   Copyright (C) 2007-2012 Stefan Schuermans <stefan@blinkenarea.org>
   Copyleft: GNU public license V2 - http://www.gnu.org/copyleft/gpl.html
   a BlinkenArea project - http://www.blinkenarea.org/ */

#include <string.h>

#include "checksum.h"
#include "debug.h"
#include "ethernet.h"
#include "icmp.h"
#include "ip.h"
#include "macros.h"
#include "nethelp.h"

// send an ICMP packet
// pData must point to a struct IcmpPacket with IcmpHdr.Type, IcmpHdr.Code
// and IpHdr.Dest already initialized
static void IcmpSend(unsigned char *pData, unsigned short Length)
{
  struct IcmpPacket *pIcmpPack;
  unsigned int chk;

// packet too short
  if (Length < sizeof(struct IcmpPacket))
    return;

// convert pointer to ICMP packet
  // (this saves us from always casting pData)
  pIcmpPack = (struct IcmpPacket *)pData;

debug_icmp_printf("send type=0x%02X len=%u", pIcmpPack->IcmpHdr.Type,
                    Length);

// fill in header values
  pIcmpPack->IcmpHdr.Chk = 0x0000;

// generate checksum
  chk =
      Checksum((unsigned char *)&pIcmpPack->IcmpHdr,
               Length - sizeof(struct EthernetHeader) -
               sizeof(struct IpHeader), 0x0000, 0x0000);
  pIcmpPack->IcmpHdr.Chk = htons(chk);

// send ICMP packet
  pIcmpPack->IpHdr.Proto = 0x01;        // ICMP
  IpSend(pData, Length);
}

// process a received ICMP echo request packet
static void IcmpEchoReqRecv(unsigned char *pData, unsigned short Length)
{
  struct IcmpEchoPacket *pIcmpEchoPack;

// packet too short
  if (Length < sizeof(struct IcmpEchoPacket))
    return;

// convert pointer to ICMP echo request/reply packet
  // (this saves us from always casting pData)
  pIcmpEchoPack = (struct IcmpEchoPacket *)pData;

// code not 0
  if (pIcmpEchoPack->IcmpHdr.Code != 0x00)
    return;

debug_icmp_printf("icmp echo len=%u", Length);

// send an ICMP echo reply
  // - use same buffer to send reply
  // - this saves us from needing to allocate a new buffer
  // - this saves us from needing to copy EchoHdr.Id, EchoHdr.Seq and the
  // data
  pIcmpEchoPack->IcmpHdr.Type = 0x00;   // ICMP echo reply
  pIcmpEchoPack->IcmpHdr.Code = 0x00;
  ip_cpy(pIcmpEchoPack->IpHdr.Dest, pIcmpEchoPack->IpHdr.Src);  // destination 
                                                                // IP is
                                                                // source IP
                                                                // of request
  IcmpSend(pData, Length);
}

// process a received ICMP packet
void IcmpRecv(unsigned char *pData, unsigned short Length)      // (extern)
{
  struct IcmpPacket *pIcmpPack;

// packet too short
  if (Length < sizeof(struct IcmpPacket))
    return;

// convert pointer to ICMP packet
  // (this saves us from always casting pData)
  pIcmpPack = (struct IcmpPacket *)pData;

// test checksum
  if (Checksum
      ((unsigned char *)&pIcmpPack->IcmpHdr,
       Length - sizeof(struct EthernetHeader) - sizeof(struct IpHeader),
       0x0000, 0x0000) != 0)
    return;

debug_icmp_printf("recv type=0x%02X len=%u", pIcmpPack->IcmpHdr.Type,
                    Length);

// branch according to type
  switch (pIcmpPack->IcmpHdr.Type) {
    // ICMP echo request
  case 0x08:
    IcmpEchoReqRecv(pData, Length);
    break;
  }
}

flaneth

change email address in header to blinkenarea address