reactos/drivers/net/afd/afd/routines.c

   1 /*
   2  * COPYRIGHT:   See COPYING in the top level directory
   3  * PROJECT:     ReactOS Ancillary Function Driver
   4  * FILE:        afd/routines.c
   5  * PURPOSE:     Support routines
   6  * PROGRAMMERS: Casper S. Hornstrup (chorns@users.sourceforge.net)
   7  * REVISIONS:
   8  *   CSH 01/02-2001 Created
   9  */
  10 #include <afd.h>
  11 #include <debug.h>
  12
  13
  14 ULONG WSABufferSize(
  15   LPWSABUF Buffers,
  16   DWORD BufferCount)
  17 {
  18   ULONG i;
  19   LPWSABUF p;
  20   ULONG Count = 0;
  21
  22   p = Buffers;
  23   for (i = 0; i < BufferCount; i++) {
  24     Count += p->len;
  25     p++;
  26   }
  27
  28   AFD_DbgPrint(MAX_TRACE, ("Buffer is %d bytes.\n", Count));
  29
  30   return Count;
  31 }
  32
  33
  34 NTSTATUS MergeWSABuffers(
  35   LPWSABUF Buffers,
  36   DWORD BufferCount,
  37   PVOID Destination,
  38   ULONG MaxLength,
  39   PULONG BytesCopied)
  40 {
  41   NTSTATUS Status;
  42   ULONG Length;
  43   LPWSABUF p;
  44   ULONG i;
  45
  46   *BytesCopied = 0;
  47   if (BufferCount == 0)
  48     return STATUS_SUCCESS;
  49
  50   p = Buffers;
  51
  52   AFD_DbgPrint(MAX_TRACE, ("Destination is 0x%X\n", Destination));
  53   AFD_DbgPrint(MAX_TRACE, ("p is 0x%X\n", p));
  54
  55   for (i = 0; i < BufferCount; i++) {
  56     Length = p->len;
  57     if (Length > MaxLength)
  58       /* Don't copy out of bounds */
  59       Length = MaxLength;
  60
  61     RtlCopyMemory(Destination, p->buf, Length);
  62     Destination += Length;
  63     AFD_DbgPrint(MAX_TRACE, ("Destination is 0x%X\n", Destination));
  64     p++;
  65     AFD_DbgPrint(MAX_TRACE, ("p is 0x%X\n", p));
  66
  67     *BytesCopied += Length;
  68
  69     MaxLength -= Length;
  70     if (MaxLength == 0)
  71       /* Destination buffer is full */
  72       break;
  73   }
  74
  75   return STATUS_SUCCESS;
  76 }
  77
  78 /*
  79  * NOTES: ReceiveQueueLock must be acquired for the FCB when called
  80  */
  81 NTSTATUS FillWSABuffers(
  82     PAFDFCB FCB,
  83     LPWSABUF Buffers,
  84     DWORD BufferCount,
  85     PULONG BytesCopied)
  86 {
  87   NTSTATUS Status;
  88   PUCHAR DstData, SrcData;
  89   UINT DstSize, SrcSize;
  90   UINT Count, Total;
  91   PAFD_BUFFER SrcBuffer;
  92   PLIST_ENTRY Entry;
  93   ULONG Size;
  94
  95   *BytesCopied = 0;
  96   if (BufferCount == 0)
  97     return STATUS_SUCCESS;
  98
  99   if (IsListEmpty(&FCB->ReceiveQueue))
 100     return STATUS_SUCCESS;
 101
 102   Entry = RemoveHeadList(&FCB->ReceiveQueue);
 103   SrcBuffer = CONTAINING_RECORD(Entry, AFD_BUFFER, ListEntry);
 104   SrcData = SrcBuffer->Buffer.buf;
 105   SrcSize = SrcBuffer->Buffer.len;
 106
 107   DstData = Buffers->buf;
 108   DstSize = Buffers->len;
 109
 110   /* Copy the data */
 111   for (Total = 0;;) {
 112     /* Find out how many bytes we can copy at one time */
 113     if (DstSize < SrcSize)
 114       Count = DstSize;
 115     else
 116       Count = SrcSize;
 117
 118     AFD_DbgPrint(MAX_TRACE, ("DstData (0x%X) SrcData (0x%X) Count (0x%X).\n",
 119       DstData, SrcData, Count));
 120
 121     RtlCopyMemory((PVOID)DstData, (PVOID)SrcData, Count);
 122
 123     Total += Count;
 124
 125     SrcSize -= Count;
 126     if (SrcSize == 0) {
 127       ExFreePool(SrcBuffer->Buffer.buf);
 128       ExFreePool(SrcBuffer);
 129
 130       /* No more bytes in source buffer. Proceed to the next buffer
 131          in the source buffer chain if there is one */
 132       if (IsListEmpty(&FCB->ReceiveQueue)) {
 133         SrcBuffer = NULL;
 134         SrcData = 0;
 135         SrcSize = 0;
 136         break;
 137       }
 138
 139       Entry = RemoveHeadList(&FCB->ReceiveQueue);
 140       SrcBuffer = CONTAINING_RECORD(Entry, AFD_BUFFER, ListEntry);
 141       SrcData = SrcBuffer->Buffer.buf;
 142       SrcSize = SrcBuffer->Buffer.len;
 143     }
 144
 145     DstSize -= Count;
 146     if (DstSize == 0) {
 147       /* No more bytes in destination buffer. Proceed to
 148          the next buffer in the destination buffer chain */
 149       BufferCount--;
 150       if (BufferCount < 1)
 151         break;
 152       Buffers++;
 153       DstData = Buffers->buf;
 154       DstSize = Buffers->len;
 155     }
 156   }
 157
 158   if (SrcSize > 0) {
 159     InsertHeadList(&FCB->ReceiveQueue, Entry);
 160   } else if (SrcBuffer != NULL) {
 161     ExFreePool(SrcBuffer->Buffer.buf);
 162     ExFreePool(SrcBuffer);
 163   }
 164
 165   *BytesCopied = Total;
 166
 167   return STATUS_SUCCESS;
 168 }
 169
 170 ULONG ChecksumCompute(
 171     PVOID Data,
 172     UINT Count,
 173     ULONG Seed)
 174 /*
 175  * FUNCTION: Calculate checksum of a buffer
 176  * ARGUMENTS:
 177  *     Data  = Pointer to buffer with data
 178  *     Count = Number of bytes in buffer
 179  *     Seed  = Previously calculated checksum (if any)
 180  * RETURNS:
 181  *     Checksum of buffer
 182  */
 183 {
 184   /* FIXME: This should be done in assembler */
 185
 186   register ULONG Sum = Seed;
 187
 188   while (Count > 1) {
 189     Sum             += *(PUSHORT)Data;
 190     Count           -= 2;
 191     (ULONG_PTR)Data += 2;
 192   }
 193
 194   /* Add left-over byte, if any */
 195   if (Count > 0)
 196     Sum += *(PUCHAR)Data;
 197
 198   /* Fold 32-bit sum to 16 bits */
 199   while (Sum >> 16)
 200     Sum = (Sum & 0xFFFF) + (Sum >> 16);
 201
 202   return ~Sum;
 203 }
 204
 205 VOID BuildIPv4Header(
 206   PIPv4_HEADER IPHeader,
 207   ULONG TotalSize,
 208   ULONG Protocol,
 209   PSOCKADDR SourceAddress,
 210   PSOCKADDR DestinationAddress)
 211 {
 212   PSOCKADDR_IN SrcNameIn = (PSOCKADDR_IN)SourceAddress;
 213   PSOCKADDR_IN DstNameIn = (PSOCKADDR_IN)DestinationAddress;
 214
 215   /* Version = 4, Length = 5 DWORDs */
 216   IPHeader->VerIHL = 0x45;
 217   /* Normal Type-of-Service */
 218   IPHeader->Tos = 0;
 219   /* Length of header and data */
 220   IPHeader->TotalLength = WH2N((USHORT)TotalSize);
 221   /* Identification */
 222   IPHeader->Id = 0;
 223   /* One fragment at offset 0 */
 224   IPHeader->FlagsFragOfs = 0;
 225   /* Time-to-Live is 128 */
 226   IPHeader->Ttl = 128;
 227   /* Protocol number */
 228   IPHeader->Protocol = Protocol;
 229   /* Checksum is 0 (calculated later) */
 230   IPHeader->Checksum = 0;
 231   /* Source address */
 232   IPHeader->SrcAddr = SrcNameIn->sin_addr.S_un.S_addr;
 233   /* Destination address */
 234   IPHeader->DstAddr = DstNameIn->sin_addr.S_un.S_addr;
 235
 236   /* Calculate checksum of IP header */
 237   IPHeader->Checksum = (USHORT)
 238     ChecksumCompute(IPHeader, sizeof(IPv4_HEADER), 0);
 239 }
 240
 241 /* EOF */