reactos/rosapps/tests/iptest/iptest.cpp

#include <iostream>
#include <list>
#include <string>
#include <sstream>
extern "C" {
    typedef unsigned short u_short;
#include <stdio.h>
#include <windows.h>
#include <winsock2.h>
#include <ddk/tdi.h>
#include <ddk/tdikrnl.h>
#include <ddk/tdiinfo.h>
#include <ddk/ndis.h>
#include <titypes.h>
#include <ip.h>
#include <tcp.h>
#include <receive.h>
#include <lan.h>
#include <routines.h>
};

/* Undis */
extern "C" VOID ExpInitLookasideLists();

std::list<std::string> output_packets;
DWORD DebugTraceLevel = 0x7fffffff;
PVOID GlobalBufferPool, GlobalPacketPool;

#define MAX_DG_SIZE 16384

char hwaddr[6] = { 0x08, 0x00, 0x20, 0x0b, 0xb7, 0xbb };

char hdr[14] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
		 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
		 0x08, 0x00 };

#define STRINGIFY(x) #x

void display_row( char *data, int off, int len ) {
    int i;

    printf( "%08x:", off );
    for( i = off; i < len && i < off + 16; i++ ) {
	printf( " %02x", data[i] & 0xff );
    }

    printf( " -- " );

    for( i = off; i < len && i < off + 16; i++ ) {
	printf( "%c", (data[i] >= ' ') ? data[i] : '.' );
    }

    printf( "\n" );
}

void connect_complete( void *context, NTSTATUS status, unsigned long count ) {
    printf( "Connection: status %x\n", status );
}

void receive_complete( void *context, NTSTATUS status, unsigned long count ) {
    printf( "Receive: status %s (bytes %d)\n", status, count );
    if( !status && count ) {
	for( int off = 0; off < count; off += 16 ) {
	    display_row( (char *)context, off, count );
	}
	printf( "\n" );
    }
}

class SocketObject {
public:
    virtual ~SocketObject() { }
    virtual int send( char *buf, int len, int *bytes, 
		      struct sockaddr_in *si ) = 0;
    virtual int recv( char *buf, int len, int *bytes,
		      struct sockaddr_in *si ) = 0;
};

UINT TdiAddressSizeFromType( UINT AddressType ) {
    switch( AddressType ) {
    case TDI_ADDRESS_TYPE_IP:
	return sizeof(TA_IP_ADDRESS);
    default:
	KeBugCheck( 0 );
    }
    return 0;
}

NTSTATUS TdiBuildNullConnectionInfoInPlace
( PTDI_CONNECTION_INFORMATION ConnInfo,
  ULONG Type ) 
/*
 * FUNCTION: Builds a NULL TDI connection information structure
 * ARGUMENTS:
 *     ConnectionInfo = Address of buffer to place connection information
 *     Type           = TDI style address type (TDI_ADDRESS_TYPE_XXX).
 * RETURNS:
 *     Status of operation
 */
{
  ULONG TdiAddressSize;
  
  TdiAddressSize = TdiAddressSizeFromType(Type);

  RtlZeroMemory(ConnInfo,
		sizeof(TDI_CONNECTION_INFORMATION) +
		TdiAddressSize);

  ConnInfo->OptionsLength = sizeof(ULONG);
  ConnInfo->RemoteAddressLength = 0;
  ConnInfo->RemoteAddress = NULL;

  return STATUS_SUCCESS;
}

NTSTATUS TdiBuildNullConnectionInfo
( PTDI_CONNECTION_INFORMATION *ConnectionInfo,
  ULONG Type )
/*
 * FUNCTION: Builds a NULL TDI connection information structure
 * ARGUMENTS:
 *     ConnectionInfo = Address of buffer pointer to allocate connection 
 *                      information in
 *     Type           = TDI style address type (TDI_ADDRESS_TYPE_XXX).
 * RETURNS:
 *     Status of operation
 */
{
  PTDI_CONNECTION_INFORMATION ConnInfo;
  ULONG TdiAddressSize;
  NTSTATUS Status;
  
  TdiAddressSize = TdiAddressSizeFromType(Type);

  ConnInfo = (PTDI_CONNECTION_INFORMATION)
    ExAllocatePool(NonPagedPool,
		   sizeof(TDI_CONNECTION_INFORMATION) +
		   TdiAddressSize);
  if (!ConnInfo)
    return STATUS_INSUFFICIENT_RESOURCES;

  Status = TdiBuildNullConnectionInfoInPlace( ConnInfo, Type );

  if (!NT_SUCCESS(Status)) 
      ExFreePool( ConnInfo );
  else
      *ConnectionInfo = ConnInfo;

  ConnInfo->RemoteAddress = (PTA_ADDRESS)&ConnInfo[1];
  ConnInfo->RemoteAddressLength = TdiAddressSize;

  return Status;
}


UINT TaLengthOfTransportAddress( PTRANSPORT_ADDRESS Addr ) {
    UINT AddrLen = 2 * sizeof( ULONG ) + Addr->Address[0].AddressLength;
    printf("AddrLen %x\n", AddrLen);
    return AddrLen;
}

NTSTATUS
TdiBuildConnectionInfoInPlace
( PTDI_CONNECTION_INFORMATION ConnectionInfo,
  PTA_ADDRESS Address ) {
    NTSTATUS Status = STATUS_SUCCESS;
    
    RtlCopyMemory( ConnectionInfo->RemoteAddress,
		   Address, 
		   ConnectionInfo->RemoteAddressLength );
    
    return Status;
}

NTSTATUS
TdiBuildConnectionInfo
( PTDI_CONNECTION_INFORMATION *ConnectionInfo,
  PTA_ADDRESS Address ) {
  NTSTATUS Status = TdiBuildNullConnectionInfo( ConnectionInfo,
						Address->AddressType );
  
  if( NT_SUCCESS(Status) )
      TdiBuildConnectionInfoInPlace( *ConnectionInfo, Address );
  
  return Status;
}

class TCPSocketObject : public SocketObject {
public:
    TCPSocketObject( std::string host, int port, NTSTATUS *status ) {
	TA_IP_ADDRESS ConnectTo;
	PTDI_CONNECTION_INFORMATION ConnInfo;

	ConnectTo.TAAddressCount = 1;
	ConnectTo.Address[0].AddressLength = sizeof(TDI_ADDRESS_IP);
	ConnectTo.Address[0].AddressType = TDI_ADDRESS_TYPE_IP;
	ConnectTo.Address[0].Address[0].sin_port = htons(port);
	ConnectTo.Address[0].Address[0].in_addr = 0x6a020a0a;

	TdiBuildConnectionInfo( &ConnInfo, (PTA_ADDRESS)&ConnectTo );

	Connection = TCPAllocateConnectionEndpoint( NULL );
	*status = TCPSocket( Connection, 
			     AF_INET,
			     SOCK_STREAM, IPPROTO_TCP );
	if( !*status ) 
	    *status = TCPConnect( Connection,
				  ConnInfo,
				  NULL,
				  connect_complete,
				  NULL );
    }

    ~TCPSocketObject() {
	TCPClose( Connection );
	if( Connection ) TCPFreeConnectionEndpoint( Connection );
    }

    int send( char *buf, int len, int *bytes, struct sockaddr_in *si ) {
	NTSTATUS Status = STATUS_UNSUCCESSFUL;

	if( Connection )
	    Status = TCPSendData( Connection, 
				  buf,
				  len,
				  (PULONG)bytes,
				  0 );
	return Status;
    }

    int recv( char *buf, int len, int *bytes, struct sockaddr_in *si ) {
	NTSTATUS Status = STATUS_UNSUCCESSFUL;

	if( Connection )
	    Status = TCPSendData( Connection, 
				  buf,
				  len,
				  (PULONG)bytes,
				  0 );
	return Status;
    }

private:
    PCONNECTION_ENDPOINT Connection;
};

VOID SendPacket( PVOID Context, 
		 PNDIS_PACKET NdisPacket, 
		 UINT Offset, 
		 PVOID LinkAddress,
		 USHORT Type ) {
    PCHAR DataOut;
    PUCHAR Addr = (PUCHAR)LinkAddress;
    UINT Size;
    std::string output_packet;

    printf( "Sending packet: %02x:%02x:%02x:%02x:%02x:%02x\n", 
	    Addr[0], Addr[1], Addr[2], Addr[3], Addr[4], Addr[5] );
    
    GetDataPtr( NdisPacket, Offset, &DataOut, &Size );
    for( int off = 0; off < Size; off += 16 ) {
	display_row( DataOut, off, Size );
    }
    printf( "\n" );

    output_packet += std::string( hwaddr, sizeof(hwaddr) );
    output_packet += std::string( (char *)LinkAddress, sizeof(hwaddr) );
    output_packet += (char)(Type >> 8);
    output_packet += (char)Type;
    output_packet += std::string( DataOut + Offset, Size - Offset );

    output_packets.push_back( output_packet );
}

#if 0
UINT CopyBufferToBufferChain
( PNDIS_BUFFER DstBuffer, UINT DstOffset, PCHAR SrcData, UINT Length ) {
    assert( 0 );
}
#endif

int main( int argc, char **argv ) {
    int asock = INVALID_SOCKET, selret, dgrecv, fromsize, err, port = 5001;
    int bytes, adapter_id, mtu, speed;
    char datagram[MAX_DG_SIZE];
    struct fd_set readf;
    struct timeval tv;
    struct sockaddr_in addr_from = { AF_INET }, addr_to;
    std::string word, cmdin, host;
    std::list<std::string>::iterator i;
    WSADATA wsadata;
    NTSTATUS Status;
    UNICODE_STRING RegistryUnicodePath;
    PCONNECTION_ENDPOINT Connection;
    PIP_INTERFACE Interface;
    IP_PACKET IPPacket;
    LLIP_BIND_INFO BindInfo;
    SocketObject *S = NULL;

    RtlInitUnicodeString
	( &RegistryUnicodePath, 
	  L"\\SYSTEM\\CurrentControlSet\\Services"
	  L"\\Tcpip" );

    ExpInitLookasideLists();

    WSAStartup( 0x101, &wsadata );

    if( argc > 1 ) port = atoi(argv[1]);

    IPStartup( &RegistryUnicodePath );

    BindInfo.Context = NULL;
    BindInfo.HeaderSize = sizeof(ETH_HEADER);
    BindInfo.MTU = 1500; /* MTU for ethernet */
    BindInfo.Address = (PUCHAR)hwaddr;
    BindInfo.AddressLength = sizeof(hwaddr);
    BindInfo.Transmit = SendPacket;
    
    IPCreateInterface( &BindInfo );

    asock = socket( AF_INET, SOCK_DGRAM, 0 );

    addr_from.sin_port = htons( port );

    if( bind( asock, (struct sockaddr *)&addr_from, sizeof( addr_from ) ) ) {
	printf( "Bind error\n" );
	return 0;
    }
    
    while( true ) {
	FD_ZERO( &readf );
	FD_SET( asock, &readf );
	tv.tv_sec = 0; 
	tv.tv_usec = 10000;
	selret = select( asock + 1, &readf, NULL, NULL, &tv );
	
	if( FD_ISSET( asock, &readf ) ) {
	    fromsize = sizeof( addr_from );
	    dgrecv = recvfrom( asock, datagram, sizeof(datagram), 0,
			       (struct sockaddr *)&addr_from, &fromsize );
	    
	    if( datagram[0] == 'C' && datagram[1] == 'M' &&
		datagram[2] == 'D' && datagram[3] == ' ' ) {
		int theport, bytes, recvret, off, bytin;
		struct sockaddr_in nam;
		std::string faddr, word;
		std::istringstream 
		    cmdin( std::string( datagram + 4, dgrecv - 4 ) );
		
		cmdin >> word;
		
/* UDP Section */
		if( word == "udpsocket" ) {
/* TCP Section */		
		} else if( word == "tcpsocket" ) {
		    cmdin >> host >> port;
		    S = new TCPSocketObject( host, port, &Status );
		    fprintf( stderr, "Socket: Result %x\n", Status );
		} else if( word == "close" ) {
		    TCPClose( Connection );
		    TCPFreeConnectionEndpoint( Connection );
		} else if( word == "type" ) {
		    std::string therest = &cmdin.str()[word.size()];
		    char* p = &therest[0];
		    p += strspn ( p, " \t" );
		    char* src = p;
		    char* dst = p;
		    while ( *src )
		    {
			char c = *src++;
			if ( c == '\r' || c == '\n' ) break;
			if ( c == '\\' )
			{
			    c = *src++;
			    switch ( c )
			    {
			    case 'b': c = '\b'; break;
			    case 'n': c = '\n'; break;
			    case 'r': c = '\r'; break;
			    case 't': c = '\t'; break;
			    case 'v': c = '\v'; break;
			    }
			}
			*dst++ = c;
		    }
		    *dst = '\0';
		    if( S ) 
			err = S->send( p, strlen(p), &bytes, NULL );
		    if( err > 0 ) { bytin = err; err = 0; }
		    
		    if( err ) 
			fprintf ( stderr, "OskitTCPConnect: error %d\n", 
				  err );
		    else {
			printf ( "wrote %d bytes\n", bytin );
		    }		    
		} else if( word == "send" ) {
		    off = 0;
		    while( cmdin >> word ) {
			datagram[off++] = 
			    atoi( (std::string("0x") + word).c_str() );
		    }
		    
		    if( (err = S->send( datagram, off, &bytin, NULL )) != 0 ) {
			fprintf( stderr, "OskitTCPConnect: error %d\n", err );
		    } else {
			printf( "wrote %d bytes\n", bytin );
		    }
		} else if( word == "recv" ) {
		    cmdin >> bytes;
		    
		    if( (err = S->recv( datagram, 
					sizeof(datagram),
					&bytes, 
					NULL )) != 0 ) {
			fprintf( stderr, "OskitTCPRecv: error %d\n", err );
		    }
		    
/* Misc section */
		} else if( word == "end" ) {
		    return 0;
		}
	    } else if( dgrecv > 14 ) {
		addr_to = addr_from;
		
		if( datagram[12] == 8 && datagram[13] == 6 ) {
		    /* Answer arp query */
		    char laddr[4];
		    /* Mark patch as to the previous sender */
		    memcpy( datagram + 32, datagram + 6, 6 );
		    memcpy( datagram, datagram + 6, 6 );
		    /* Mark packet as from us */
		    memcpy( datagram + 22, hwaddr, 6 );
		    memcpy( datagram + 6, hwaddr, 6 );
		    /* Swap inet addresses */
		    memcpy( laddr, datagram + 28, 4 );
		    memcpy( datagram + 28, datagram + 38, 4 );
		    memcpy( datagram + 38, laddr, 4 );
		    /* Set reply opcode */
		    datagram[21] = 2;
		    
		    err = sendto( asock, datagram, dgrecv, 0,
				  (struct sockaddr *)&addr_to, 
				  sizeof(addr_to) );
		    
		    if( err != 0 )
			printf( "sendto: %d\n", err );
		} else {
		    memcpy( hdr, datagram + 6, 6 );
		    memcpy( hdr + 6, datagram, 6 );
		    memcpy( hdr + 12, datagram + 12, 2 );
		    IPPacket.Header = datagram;
		    IPPacket.Data = datagram + 14;
		    IPPacket.TotalSize = dgrecv;
		    IPReceive( Interface, &IPPacket );
		}
	    }
	}
	
	IPTimeout(NULL, NULL, NULL, NULL);
	
	for( i = output_packets.begin(); i != output_packets.end(); i++ ) {
	    err = sendto( asock, i->c_str(), i->size(), 0, 
			  (struct sockaddr *)&addr_to, sizeof(addr_to) );
	    
	    fprintf( stderr, "** SENDING PACKET %d bytes **\n", i->size() );
	    
	    if( err != 0 )
		printf( "sendto: %d\n", err );
	}
	
	output_packets.clear();
    }
}