iptest.cpp

Go to the documentation of this file.

#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();
    }
}