firmware.dartboard/icmp.c (e8658d5) - flaneth · BlinkenArea

icmp.c

/* flaneth - flash and ethernet - dartboard mod
 * version 0.1 date 2008-11-09
 * Copyright (C) 2007-2008 Stefan Schuermans <stefan@schuermans.info>
 * 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

initial commit after making CF identify work