Merge branch 'master' of git://localhost/acess2
[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 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 }

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