rosapps/tests/iptest/iptest.cpp

   1 #include <iostream>
   2 #include <list>
   3 #include <string>
   4 #include <sstream>
   5 extern "C" {
   6     typedef unsigned short u_short;
   7 #include <stdio.h>
   8 #include <windows.h>
   9 #include <winsock2.h>
  10 #include <ddk/tdi.h>
  11 #include <ddk/tdikrnl.h>
  12 #include <ddk/tdiinfo.h>
  13 #include <ddk/ndis.h>
  14 #include <titypes.h>
  15 #include <ip.h>
  16 #include <tcp.h>
  17 #include <receive.h>
  18 #include <lan.h>
  19 #include <routines.h>
  20 };
  21
  22 /* Undis */
  23 extern "C" VOID ExpInitLookasideLists();
  24
  25 std::list<std::string> output_packets;
  26 DWORD DebugTraceLevel = 0x7fffffff;
  27 PVOID GlobalBufferPool, GlobalPacketPool;
  28
  29 #define MAX_DG_SIZE 16384
  30
  31 char hwaddr[6] = { 0x08, 0x00, 0x20, 0x0b, 0xb7, 0xbb };
  32
  33 char hdr[14] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  34                  0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  35                  0x08, 0x00 };
  36
  37 #define STRINGIFY(x) #x
  38
  39 void display_row( char *data, int off, int len ) {
  40     int i;
  41
  42     printf( "%08x:", off );
  43     for( i = off; i < len && i < off + 16; i++ ) {
  44         printf( " %02x", data[i] & 0xff );
  45     }
  46
  47     printf( " -- " );
  48
  49     for( i = off; i < len && i < off + 16; i++ ) {
  50         printf( "%c", (data[i] >= ' ') ? data[i] : '.' );
  51     }
  52
  53     printf( "\n" );
  54 }
  55
  56 void connect_complete( void *context, NTSTATUS status, unsigned long count ) {
  57     printf( "Connection: status %x\n", status );
  58 }
  59
  60 void receive_complete( void *context, NTSTATUS status, unsigned long count ) {
  61     printf( "Receive: status %s (bytes %d)\n", status, count );
  62     if( !status && count ) {
  63         for( int off = 0; off < count; off += 16 ) {
  64             display_row( (char *)context, off, count );
  65         }
  66         printf( "\n" );
  67     }
  68 }
  69
  70 class SocketObject {
  71 public:
  72     virtual ~SocketObject() { }
  73     virtual int send( char *buf, int len, int *bytes,
  74                       struct sockaddr_in *si ) = 0;
  75     virtual int recv( char *buf, int len, int *bytes,
  76                       struct sockaddr_in *si ) = 0;
  77 };
  78
  79 UINT TdiAddressSizeFromType( UINT AddressType ) {
  80     switch( AddressType ) {
  81     case TDI_ADDRESS_TYPE_IP:
  82         return sizeof(TA_IP_ADDRESS);
  83     default:
  84         KeBugCheck( 0 );
  85     }
  86     return 0;
  87 }
  88
  89 NTSTATUS TdiBuildNullConnectionInfoInPlace
  90 ( PTDI_CONNECTION_INFORMATION ConnInfo,
  91   ULONG Type )
  92 /*
  93  * FUNCTION: Builds a NULL TDI connection information structure
  94  * ARGUMENTS:
  95  *     ConnectionInfo = Address of buffer to place connection information
  96  *     Type           = TDI style address type (TDI_ADDRESS_TYPE_XXX).
  97  * RETURNS:
  98  *     Status of operation
  99  */
 100 {
 101   ULONG TdiAddressSize;
 102
 103   TdiAddressSize = TdiAddressSizeFromType(Type);
 104
 105   RtlZeroMemory(ConnInfo,
 106                 sizeof(TDI_CONNECTION_INFORMATION) +
 107                 TdiAddressSize);
 108
 109   ConnInfo->OptionsLength = sizeof(ULONG);
 110   ConnInfo->RemoteAddressLength = 0;
 111   ConnInfo->RemoteAddress = NULL;
 112
 113   return STATUS_SUCCESS;
 114 }
 115
 116 NTSTATUS TdiBuildNullConnectionInfo
 117 ( PTDI_CONNECTION_INFORMATION *ConnectionInfo,
 118   ULONG Type )
 119 /*
 120  * FUNCTION: Builds a NULL TDI connection information structure
 121  * ARGUMENTS:
 122  *     ConnectionInfo = Address of buffer pointer to allocate connection
 123  *                      information in
 124  *     Type           = TDI style address type (TDI_ADDRESS_TYPE_XXX).
 125  * RETURNS:
 126  *     Status of operation
 127  */
 128 {
 129   PTDI_CONNECTION_INFORMATION ConnInfo;
 130   ULONG TdiAddressSize;
 131   NTSTATUS Status;
 132
 133   TdiAddressSize = TdiAddressSizeFromType(Type);
 134
 135   ConnInfo = (PTDI_CONNECTION_INFORMATION)
 136     ExAllocatePool(NonPagedPool,
 137                    sizeof(TDI_CONNECTION_INFORMATION) +
 138                    TdiAddressSize);
 139   if (!ConnInfo)
 140     return STATUS_INSUFFICIENT_RESOURCES;
 141
 142   Status = TdiBuildNullConnectionInfoInPlace( ConnInfo, Type );
 143
 144   if (!NT_SUCCESS(Status))
 145       ExFreePool( ConnInfo );
 146   else
 147       *ConnectionInfo = ConnInfo;
 148
 149   ConnInfo->RemoteAddress = (PTA_ADDRESS)&ConnInfo[1];
 150   ConnInfo->RemoteAddressLength = TdiAddressSize;
 151
 152   return Status;
 153 }
 154
 155
 156 UINT TaLengthOfTransportAddress( PTRANSPORT_ADDRESS Addr ) {
 157     UINT AddrLen = 2 * sizeof( ULONG ) + Addr->Address[0].AddressLength;
 158     printf("AddrLen %x\n", AddrLen);
 159     return AddrLen;
 160 }
 161
 162 NTSTATUS
 163 TdiBuildConnectionInfoInPlace
 164 ( PTDI_CONNECTION_INFORMATION ConnectionInfo,
 165   PTA_ADDRESS Address ) {
 166     NTSTATUS Status = STATUS_SUCCESS;
 167
 168     RtlCopyMemory( ConnectionInfo->RemoteAddress,
 169                    Address,
 170                    ConnectionInfo->RemoteAddressLength );
 171
 172     return Status;
 173 }
 174
 175 NTSTATUS
 176 TdiBuildConnectionInfo
 177 ( PTDI_CONNECTION_INFORMATION *ConnectionInfo,
 178   PTA_ADDRESS Address ) {
 179   NTSTATUS Status = TdiBuildNullConnectionInfo( ConnectionInfo,
 180                                                 Address->AddressType );
 181
 182   if( NT_SUCCESS(Status) )
 183       TdiBuildConnectionInfoInPlace( *ConnectionInfo, Address );
 184
 185   return Status;
 186 }
 187
 188 class TCPSocketObject : public SocketObject {
 189 public:
 190     TCPSocketObject( std::string host, int port, NTSTATUS *status ) {
 191         TA_IP_ADDRESS ConnectTo;
 192         PTDI_CONNECTION_INFORMATION ConnInfo;
 193
 194         ConnectTo.TAAddressCount = 1;
 195         ConnectTo.Address[0].AddressLength = sizeof(TDI_ADDRESS_IP);
 196         ConnectTo.Address[0].AddressType = TDI_ADDRESS_TYPE_IP;
 197         ConnectTo.Address[0].Address[0].sin_port = htons(port);
 198         ConnectTo.Address[0].Address[0].in_addr = 0x6a020a0a;
 199
 200         TdiBuildConnectionInfo( &ConnInfo, (PTA_ADDRESS)&ConnectTo );
 201
 202         Connection = TCPAllocateConnectionEndpoint( NULL );
 203         *status = TCPSocket( Connection,
 204                              AF_INET,
 205                              SOCK_STREAM, IPPROTO_TCP );
 206         if( !*status )
 207             *status = TCPConnect( Connection,
 208                                   ConnInfo,
 209                                   NULL,
 210                                   connect_complete,
 211                                   NULL );
 212     }
 213
 214     ~TCPSocketObject() {
 215         TCPClose( Connection );
 216         if( Connection ) TCPFreeConnectionEndpoint( Connection );
 217     }
 218
 219     int send( char *buf, int len, int *bytes, struct sockaddr_in *si ) {
 220         NTSTATUS Status = STATUS_UNSUCCESSFUL;
 221
 222         if( Connection )
 223             Status = TCPSendData( Connection,
 224                                   buf,
 225                                   len,
 226                                   (PULONG)bytes,
 227                                   0 );
 228         return Status;
 229     }
 230
 231     int recv( char *buf, int len, int *bytes, struct sockaddr_in *si ) {
 232         NTSTATUS Status = STATUS_UNSUCCESSFUL;
 233
 234         if( Connection )
 235             Status = TCPSendData( Connection,
 236                                   buf,
 237                                   len,
 238                                   (PULONG)bytes,
 239                                   0 );
 240         return Status;
 241     }
 242
 243 private:
 244     PCONNECTION_ENDPOINT Connection;
 245 };
 246
 247 VOID SendPacket( PVOID Context,
 248                  PNDIS_PACKET NdisPacket,
 249                  UINT Offset,
 250                  PVOID LinkAddress,
 251                  USHORT Type ) {
 252     PCHAR DataOut;
 253     PUCHAR Addr = (PUCHAR)LinkAddress;
 254     UINT Size;
 255     std::string output_packet;
 256
 257     printf( "Sending packet: %02x:%02x:%02x:%02x:%02x:%02x\n",
 258             Addr[0], Addr[1], Addr[2], Addr[3], Addr[4], Addr[5] );
 259
 260     GetDataPtr( NdisPacket, Offset, &DataOut, &Size );
 261     for( int off = 0; off < Size; off += 16 ) {
 262         display_row( DataOut, off, Size );
 263     }
 264     printf( "\n" );
 265
 266     output_packet += std::string( hwaddr, sizeof(hwaddr) );
 267     output_packet += std::string( (char *)LinkAddress, sizeof(hwaddr) );
 268     output_packet += (char)(Type >> 8);
 269     output_packet += (char)Type;
 270     output_packet += std::string( DataOut + Offset, Size - Offset );
 271
 272     output_packets.push_back( output_packet );
 273 }
 274
 275 #if 0
 276 UINT CopyBufferToBufferChain
 277 ( PNDIS_BUFFER DstBuffer, UINT DstOffset, PCHAR SrcData, UINT Length ) {
 278     assert( 0 );
 279 }
 280 #endif
 281
 282 int main( int argc, char **argv ) {
 283     int asock = INVALID_SOCKET, selret, dgrecv, fromsize, err, port = 5001;
 284     int bytes, adapter_id, mtu, speed;
 285     char datagram[MAX_DG_SIZE];
 286     struct fd_set readf;
 287     struct timeval tv;
 288     struct sockaddr_in addr_from = { AF_INET }, addr_to;
 289     std::string word, cmdin, host;
 290     std::list<std::string>::iterator i;
 291     WSADATA wsadata;
 292     NTSTATUS Status;
 293     UNICODE_STRING RegistryUnicodePath;
 294     PCONNECTION_ENDPOINT Connection;
 295     PIP_INTERFACE Interface;
 296     IP_PACKET IPPacket;
 297     LLIP_BIND_INFO BindInfo;
 298     SocketObject *S = NULL;
 299
 300     RtlInitUnicodeString
 301         ( &RegistryUnicodePath,
 302           L"\\SYSTEM\\CurrentControlSet\\Services"
 303           L"\\Tcpip" );
 304
 305     ExpInitLookasideLists();
 306
 307     WSAStartup( 0x101, &wsadata );
 308
 309     if( argc > 1 ) port = atoi(argv[1]);
 310
 311     IPStartup( &RegistryUnicodePath );
 312
 313     BindInfo.Context = NULL;
 314     BindInfo.HeaderSize = sizeof(ETH_HEADER);
 315     BindInfo.MTU = 1500; /* MTU for ethernet */
 316     BindInfo.Address = (PUCHAR)hwaddr;
 317     BindInfo.AddressLength = sizeof(hwaddr);
 318     BindInfo.Transmit = SendPacket;
 319
 320     IPCreateInterface( &BindInfo );
 321
 322     asock = socket( AF_INET, SOCK_DGRAM, 0 );
 323
 324     addr_from.sin_port = htons( port );
 325
 326     if( bind( asock, (struct sockaddr *)&addr_from, sizeof( addr_from ) ) ) {
 327         printf( "Bind error\n" );
 328         return 0;
 329     }
 330
 331     while( true ) {
 332         FD_ZERO( &readf );
 333         FD_SET( asock, &readf );
 334         tv.tv_sec = 0;
 335         tv.tv_usec = 10000;
 336         selret = select( asock + 1, &readf, NULL, NULL, &tv );
 337
 338         if( FD_ISSET( asock, &readf ) ) {
 339             fromsize = sizeof( addr_from );
 340             dgrecv = recvfrom( asock, datagram, sizeof(datagram), 0,
 341                                (struct sockaddr *)&addr_from, &fromsize );
 342
 343             if( datagram[0] == 'C' && datagram[1] == 'M' &&
 344                 datagram[2] == 'D' && datagram[3] == ' ' ) {
 345                 int theport, bytes, recvret, off, bytin;
 346                 struct sockaddr_in nam;
 347                 std::string faddr, word;
 348                 std::istringstream
 349                     cmdin( std::string( datagram + 4, dgrecv - 4 ) );
 350
 351                 cmdin >> word;
 352
 353 /* UDP Section */
 354                 if( word == "udpsocket" ) {
 355 /* TCP Section */
 356                 } else if( word == "tcpsocket" ) {
 357                     cmdin >> host >> port;
 358                     S = new TCPSocketObject( host, port, &Status );
 359                     fprintf( stderr, "Socket: Result %x\n", Status );
 360                 } else if( word == "close" ) {
 361                     TCPClose( Connection );
 362                     TCPFreeConnectionEndpoint( Connection );
 363                 } else if( word == "type" ) {
 364                     std::string therest = &cmdin.str()[word.size()];
 365                     char* p = &therest[0];
 366                     p += strspn ( p, " \t" );
 367                     char* src = p;
 368                     char* dst = p;
 369                     while ( *src )
 370                     {
 371                         char c = *src++;
 372                         if ( c == '\r' || c == '\n' ) break;
 373                         if ( c == '\\' )
 374                         {
 375                             c = *src++;
 376                             switch ( c )
 377                             {
 378                             case 'b': c = '\b'; break;
 379                             case 'n': c = '\n'; break;
 380                             case 'r': c = '\r'; break;
 381                             case 't': c = '\t'; break;
 382                             case 'v': c = '\v'; break;
 383                             }
 384                         }
 385                         *dst++ = c;
 386                     }
 387                     *dst = '\0';
 388                     if( S )
 389                         err = S->send( p, strlen(p), &bytes, NULL );
 390                     if( err > 0 ) { bytin = err; err = 0; }
 391
 392                     if( err )
 393                         fprintf ( stderr, "OskitTCPConnect: error %d\n",
 394                                   err );
 395                     else {
 396                         printf ( "wrote %d bytes\n", bytin );
 397                     }
 398                 } else if( word == "send" ) {
 399                     off = 0;
 400                     while( cmdin >> word ) {
 401                         datagram[off++] =
 402                             atoi( (std::string("0x") + word).c_str() );
 403                     }
 404
 405                     if( (err = S->send( datagram, off, &bytin, NULL )) != 0 ) {
 406                         fprintf( stderr, "OskitTCPConnect: error %d\n", err );
 407                     } else {
 408                         printf( "wrote %d bytes\n", bytin );
 409                     }
 410                 } else if( word == "recv" ) {
 411                     cmdin >> bytes;
 412
 413                     if( (err = S->recv( datagram,
 414                                         sizeof(datagram),
 415                                         &bytes,
 416                                         NULL )) != 0 ) {
 417                         fprintf( stderr, "OskitTCPRecv: error %d\n", err );
 418                     }
 419
 420 /* Misc section */
 421                 } else if( word == "end" ) {
 422                     return 0;
 423                 }
 424             } else if( dgrecv > 14 ) {
 425                 addr_to = addr_from;
 426
 427                 if( datagram[12] == 8 && datagram[13] == 6 ) {
 428                     /* Answer arp query */
 429                     char laddr[4];
 430                     /* Mark patch as to the previous sender */
 431                     memcpy( datagram + 32, datagram + 6, 6 );
 432                     memcpy( datagram, datagram + 6, 6 );
 433                     /* Mark packet as from us */
 434                     memcpy( datagram + 22, hwaddr, 6 );
 435                     memcpy( datagram + 6, hwaddr, 6 );
 436                     /* Swap inet addresses */
 437                     memcpy( laddr, datagram + 28, 4 );
 438                     memcpy( datagram + 28, datagram + 38, 4 );
 439                     memcpy( datagram + 38, laddr, 4 );
 440                     /* Set reply opcode */
 441                     datagram[21] = 2;
 442
 443                     err = sendto( asock, datagram, dgrecv, 0,
 444                                   (struct sockaddr *)&addr_to,
 445                                   sizeof(addr_to) );
 446
 447                     if( err != 0 )
 448                         printf( "sendto: %d\n", err );
 449                 } else {
 450                     memcpy( hdr, datagram + 6, 6 );
 451                     memcpy( hdr + 6, datagram, 6 );
 452                     memcpy( hdr + 12, datagram + 12, 2 );
 453                     IPPacket.Header = datagram;
 454                     IPPacket.Data = datagram + 14;
 455                     IPPacket.TotalSize = dgrecv;
 456                     IPReceive( Interface, &IPPacket );
 457                 }
 458             }
 459         }
 460
 461         IPTimeout(NULL, NULL, NULL, NULL);
 462
 463         for( i = output_packets.begin(); i != output_packets.end(); i++ ) {
 464             err = sendto( asock, i->c_str(), i->size(), 0,
 465                           (struct sockaddr *)&addr_to, sizeof(addr_to) );
 466
 467             fprintf( stderr, "** SENDING PACKET %d bytes **\n", i->size() );
 468
 469             if( err != 0 )
 470                 printf( "sendto: %d\n", err );
 471         }
 472
 473         output_packets.clear();
 474     }
 475 }
 476