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