KernelLand/Modules/IPStack/link.c

   1 /*
   2  * Acess2 Networking Stack
   3  * - By John Hodge (thePowersGang)
   4  *
   5  * link.c
   6  * - Ethernet/802.3 Handling code
   7  * TODO: Rename file
   8  */
   9 #include "ipstack.h"
  10 #include "link.h"
  11 #include "include/buffer.h"
  12 #include "include/adapters_int.h"
  13
  14 // === CONSTANTS ===
  15 #define MAX_PACKET_SIZE 2048
  16
  17 // === PROTOTYPES ===
  18 void    Link_RegisterType(Uint16 Type, tPacketCallback Callback);
  19 void    Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, tIPStackBuffer *Buffer);
  20  int    Link_HandlePacket(tAdapter *Adapter, tIPStackBuffer *Buffer);
  21 // --- CRC ---
  22 void    Link_InitCRC(void);
  23 Uint32  Link_CalculateCRC(tIPStackBuffer *Buffer);
  24 Uint32  Link_CalculatePartialCRC(Uint32 CRC, const void *Data, int Length);
  25
  26 // === GLOBALS ===
  27  int    giRegisteredTypes = 0;
  28  int    giRegisteredTypeSpace = 0;
  29 struct {
  30         Uint16  Type;
  31         tPacketCallback Callback;
  32 }       *gaRegisteredTypes;
  33  int    gbLink_CRCTableGenerated = 0;
  34 Uint32  gaiLink_CRCTable[256];
  35
  36 // === CODE ===
  37 /**
  38  * \fn void Link_RegisterType(Uint16 Type, tPacketCallback Callback)
  39  * \brief Registers a callback for a specific packet type
  40  *
  41  * \todo Make thread safe (place a mutex on the list)
  42  */
  43 void Link_RegisterType(Uint16 Type, tPacketCallback Callback)
  44 {
  45          int    i;
  46         void    *tmp;
  47
  48         for( i = giRegisteredTypes; i -- ; )
  49         {
  50                 if(gaRegisteredTypes[i].Type == Type) {
  51                         Log_Warning("Net Link", "Attempt to register 0x%x twice", Type);
  52                         return ;
  53                 }
  54                 // Ooh! Free slot!
  55                 if(gaRegisteredTypes[i].Callback == NULL)       break;
  56         }
  57
  58         if(i == -1)
  59         {
  60                 giRegisteredTypeSpace += 5;
  61                 tmp = realloc(gaRegisteredTypes, giRegisteredTypeSpace*sizeof(*gaRegisteredTypes));
  62                 if(!tmp) {
  63                         Log_Warning("Net Link",
  64                                 "Out of heap space! (Attempted to allocate %i)",
  65                                 giRegisteredTypeSpace*sizeof(*gaRegisteredTypes)
  66                                 );
  67                         return ;
  68                 }
  69                 gaRegisteredTypes = tmp;
  70                 i = giRegisteredTypes;
  71                 giRegisteredTypes ++;
  72         }
  73
  74         gaRegisteredTypes[i].Callback = Callback;
  75         gaRegisteredTypes[i].Type = Type;
  76 }
  77
  78 /**
  79  * \fn void Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, int Length, void *Buffer)
  80  * \brief Formats and sends a packet on the specified interface
  81  */
  82 void Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, tIPStackBuffer *Buffer)
  83 {
  84          int    length = IPStack_Buffer_GetLength(Buffer);
  85          int    ofs = (4 - (length & 3)) & 3;
  86         Uint8   buf[sizeof(tEthernetHeader) + ofs + 4];
  87         Uint32  *checksum = (void*)(buf + sizeof(tEthernetHeader) + ofs);
  88         tEthernetHeader *hdr = (void*)buf;
  89
  90         Log_Log("Net Link", "Sending %i bytes to %02x:%02x:%02x:%02x:%02x:%02x (Type 0x%x)",
  91                 length, To.B[0], To.B[1], To.B[2], To.B[3], To.B[4], To.B[5], Type);
  92
  93         hdr->Dest = To;
  94         memcpy(&hdr->Src, Adapter->HWAddr, 6);  // TODO: Remove hard coded 6
  95         hdr->Type = htons(Type);
  96         memset(hdr, 0, ofs+4);  // zero padding and checksum
  97
  98         if( (Adapter->Type->Flags & ADAPTERFLAG_OFFLOAD_MAC) )
  99                 IPStack_Buffer_AppendSubBuffer(Buffer, sizeof(tEthernetHeader), ofs, hdr, NULL, NULL);
 100         else
 101         {
 102                 IPStack_Buffer_AppendSubBuffer(Buffer, sizeof(tEthernetHeader), ofs + 4, hdr, NULL, NULL);
 103                 *checksum = htonl( Link_CalculateCRC(Buffer) );
 104                 Log_Debug("Net Link", "Non-Offloaded: 0x%x, 0x%x", *checksum, ntohl(*checksum));
 105         }
 106
 107         Log_Log("Net Link", " from %02x:%02x:%02x:%02x:%02x:%02x",
 108                 hdr->Src.B[0], hdr->Src.B[1], hdr->Src.B[2],
 109                 hdr->Src.B[3], hdr->Src.B[4], hdr->Src.B[5]
 110                 );
 111
 112         Adapter_SendPacket(Adapter, Buffer);
 113 }
 114
 115 int Link_HandlePacket(tAdapter *Adapter, tIPStackBuffer *Buffer)
 116 {
 117         size_t  len = 0;
 118         void    *data = IPStack_Buffer_CompactBuffer(Buffer, &len);
 119
 120         tEthernetHeader *hdr = (void*)data;
 121
 122         if(len < sizeof(tEthernetHeader)) {
 123                 Log_Log("Net Link", "Recieved an undersized packet (%i < %i)",
 124                         len, sizeof(tEthernetHeader));
 125                 free(data);
 126                 return 1;
 127         }
 128
 129         Log_Log("Net Link",
 130                 "Packet from %02x:%02x:%02x:%02x:%02x:%02x"
 131                 " to %02x:%02x:%02x:%02x:%02x:%02x (Type=%04x)",
 132                 hdr->Src.B[0], hdr->Src.B[1], hdr->Src.B[2],
 133                 hdr->Src.B[3], hdr->Src.B[4], hdr->Src.B[5],
 134                 hdr->Dest.B[0], hdr->Dest.B[1], hdr->Dest.B[2],
 135                 hdr->Dest.B[3], hdr->Dest.B[4], hdr->Dest.B[5],
 136                 ntohs(hdr->Type)
 137                 );
 138 //      Uint32 checksum = *(Uint32*)(data + len + 4);
 139         //Log_Log("NET", "Checksum 0x%08x", checksum);
 140         // TODO: Check checksum
 141
 142         // Check if there is a registered callback for this packet type
 143          int    i;
 144         for( i = giRegisteredTypes; i--; )
 145         {
 146                 if(gaRegisteredTypes[i].Type == ntohs(hdr->Type))       break;
 147         }
 148         // No? Ignore it
 149         if( i == -1 ) {
 150                 Log_Log("Net Link", "Unregistered type 0x%x", ntohs(hdr->Type));
 151
 152                 free(data);
 153                 return 1;
 154         }
 155
 156         // Call the callback
 157         gaRegisteredTypes[i].Callback(
 158                 Adapter,
 159                 hdr->Src,
 160                 len - sizeof(tEthernetHeader),
 161                 hdr->Data
 162                 );
 163
 164         free(data);
 165
 166         return 0;
 167 }
 168
 169 // From http://www.cl.cam.ac.uk/research/srg/bluebook/21/crc/node6.html
 170 #define QUOTIENT        0x04c11db7
 171 void Link_InitCRC(void)
 172 {
 173          int    i, j;
 174         Uint32  crc;
 175
 176         for (i = 0; i < 256; i++)
 177         {
 178                 crc = i << 24;
 179                 for (j = 0; j < 8; j++)
 180                 {
 181                         if (crc & 0x80000000)
 182                                 crc = (crc << 1) ^ QUOTIENT;
 183                         else
 184                                 crc = crc << 1;
 185                 }
 186                 gaiLink_CRCTable[i] = crc;
 187         }
 188
 189         gbLink_CRCTableGenerated = 1;
 190 }
 191
 192 Uint32 Link_CalculateCRC(tIPStackBuffer *Buffer)
 193 {
 194         Uint32  ret = 0xFFFFFFFF;
 195         const void      *data;
 196         size_t  length;
 197
 198          int    id = -1;
 199         while( (id = IPStack_Buffer_GetBuffer(Buffer, id, &length, &data)) != -1 )
 200         {
 201                 ret = Link_CalculatePartialCRC(ret, data, length);
 202         }
 203
 204         return ~ret;
 205 }
 206
 207 Uint32 Link_CalculatePartialCRC(Uint32 CRC, const void *Data, int Length)
 208 {
 209         // x32 + x26 + x23 + x22 + x16 + x12 + x11 + x10 + x8 + x7 + x5 + x4 + x2 + x + 1
 210         const Uint32    *data = Data;
 211
 212         for( int i = 0; i < Length/4; i++ )
 213         {
 214                 CRC = (CRC << 8 | *data++) ^ gaiLink_CRCTable[CRC >> 24];
 215         }
 216
 217         return CRC;
 218 }