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

UCC git Repository :: git.ucc.asn.au