Modules/VIARhineII - Fiddling
[tpg/acess2.git] / KernelLand / Modules / IPStack / link.c
1 /*
2  * Acess2 IP Stack
3  * - Link/Media Layer Interface
4  */
5 #include "ipstack.h"
6 #include "link.h"
7 #include "include/buffer.h"
8
9 // === CONSTANTS ===
10 #define MAX_PACKET_SIZE 2048
11
12 // === PROTOTYPES ===
13 void    Link_RegisterType(Uint16 Type, tPacketCallback Callback);
14 void    Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, tIPStackBuffer *Buffer);
15 void    Link_WatchDevice(tAdapter *Adapter);
16 // --- CRC ---
17 void    Link_InitCRC(void);
18 Uint32  Link_CalculateCRC(tIPStackBuffer *Buffer);
19 Uint32  Link_CalculatePartialCRC(Uint32 CRC, const void *Data, int Length);
20
21 // === GLOBALS ===
22  int    giRegisteredTypes = 0;
23  int    giRegisteredTypeSpace = 0;
24 struct {
25         Uint16  Type;
26         tPacketCallback Callback;
27 }       *gaRegisteredTypes;
28  int    gbLink_CRCTableGenerated = 0;
29 Uint32  gaiLink_CRCTable[256];
30
31 // === CODE ===
32 /**
33  * \fn void Link_RegisterType(Uint16 Type, tPacketCallback Callback)
34  * \brief Registers a callback for a specific packet type
35  * 
36  * \todo Make thread safe (place a mutex on the list)
37  */
38 void Link_RegisterType(Uint16 Type, tPacketCallback Callback)
39 {
40          int    i;
41         void    *tmp;
42         
43         for( i = giRegisteredTypes; i -- ; )
44         {
45                 if(gaRegisteredTypes[i].Type == Type) {
46                         Log_Warning("Net Link", "Attempt to register 0x%x twice", Type);
47                         return ;
48                 }
49                 // Ooh! Free slot!
50                 if(gaRegisteredTypes[i].Callback == NULL)       break;
51         }
52         
53         if(i == -1)
54         {
55                 giRegisteredTypeSpace += 5;
56                 tmp = realloc(gaRegisteredTypes, giRegisteredTypeSpace*sizeof(*gaRegisteredTypes));
57                 if(!tmp) {
58                         Log_Warning("Net Link",
59                                 "Out of heap space! (Attempted to allocate %i)",
60                                 giRegisteredTypeSpace*sizeof(*gaRegisteredTypes)
61                                 );
62                         return ;
63                 }
64                 gaRegisteredTypes = tmp;
65                 i = giRegisteredTypes;
66                 giRegisteredTypes ++;
67         }
68         
69         gaRegisteredTypes[i].Callback = Callback;
70         gaRegisteredTypes[i].Type = Type;
71 }
72
73 /**
74  * \fn void Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, int Length, void *Buffer)
75  * \brief Formats and sends a packet on the specified interface
76  */
77 void Link_SendPacket(tAdapter *Adapter, Uint16 Type, tMacAddr To, tIPStackBuffer *Buffer)
78 {
79          int    length = IPStack_Buffer_GetLength(Buffer);
80          int    ofs = 4 - (length & 3);
81         Uint8   buf[sizeof(tEthernetHeader) + ofs + 4];
82         tEthernetHeader *hdr = (void*)buf;
83
84         if( ofs == 4 )  ofs = 0;        
85
86         Log_Log("Net Link", "Sending %i bytes to %02x:%02x:%02x:%02x:%02x:%02x (Type 0x%x)",
87                 length, To.B[0], To.B[1], To.B[2], To.B[3], To.B[4], To.B[5], Type);
88
89         hdr->Dest = To;
90         hdr->Src = Adapter->MacAddr;
91         hdr->Type = htons(Type);
92         *(Uint32*)(buf + sizeof(tEthernetHeader) + ofs) = 0;
93
94         IPStack_Buffer_AppendSubBuffer(Buffer, sizeof(tEthernetHeader), ofs + 4, hdr, NULL, NULL);
95         
96         *(Uint32*)(buf + sizeof(tEthernetHeader) + ofs) = htonl( Link_CalculateCRC(Buffer) );
97
98         size_t outlen = 0;
99         void *data = IPStack_Buffer_CompactBuffer(Buffer, &outlen);
100         VFS_Write(Adapter->DeviceFD, outlen, data);
101         free(data);
102 }
103
104 void Link_WorkerThread(void *Ptr)
105 {
106         tAdapter        *Adapter = Ptr;
107         
108         Threads_SetName(Adapter->Device);
109         Log_Log("Net Link", "Thread %i watching '%s'", Threads_GetTID(), Adapter->Device);
110
111         // Child Thread
112         while(Adapter->DeviceFD != -1)
113         {
114                 Uint8   buf[MAX_PACKET_SIZE];
115                 tEthernetHeader *hdr = (void*)buf;
116                  int    ret, i;
117                 Uint32  checksum;
118                 
119                 // Wait for a packet (Read on a network device is blocking)
120                 //Log_Debug("NET", "Waiting on adapter FD#0x%x", Adapter->DeviceFD);
121                 ret = VFS_Read(Adapter->DeviceFD, MAX_PACKET_SIZE, buf);
122                 if(ret == -1)   break;
123                 
124                 if(ret < sizeof(tEthernetHeader)) {
125                         Log_Log("Net Link", "Recieved an undersized packet (%i < %i)",
126                                 ret, sizeof(tEthernetHeader));
127                         continue;
128                 }
129                 
130                 Log_Log("Net Link",
131                         "Packet from %02x:%02x:%02x:%02x:%02x:%02x"
132                         " to %02x:%02x:%02x:%02x:%02x:%02x (Type=%04x)",
133                         hdr->Src.B[0], hdr->Src.B[1], hdr->Src.B[2],
134                         hdr->Src.B[3], hdr->Src.B[4], hdr->Src.B[5],
135                         hdr->Dest.B[0], hdr->Dest.B[1], hdr->Dest.B[2],
136                         hdr->Dest.B[3], hdr->Dest.B[4], hdr->Dest.B[5],
137                         ntohs(hdr->Type)
138                         );
139                 checksum = *(Uint32*)&hdr->Data[ret-sizeof(tEthernetHeader)-4];
140                 //Log_Log("NET", "Checksum 0x%08x", checksum);
141                 // TODO: Check checksum
142                 
143                 // Check if there is a registered callback for this packet type
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                         continue;
152                 }
153                 
154                 // Call the callback
155                 gaRegisteredTypes[i].Callback(
156                         Adapter,
157                         hdr->Src,
158                         ret - sizeof(tEthernetHeader),
159                         hdr->Data
160                         );
161         }
162         
163         Log_Log("Net Link", "Watcher terminated (file closed)");
164         
165         Threads_Exit(0, 0);
166 }
167
168 /**
169  * \fn void Link_WatchDevice(tAdapter *Adapter)
170  * \brief Spawns a worker thread to watch the specified adapter
171  */
172 void Link_WatchDevice(tAdapter *Adapter)
173 {
174          int    tid;
175
176         if( !gbLink_CRCTableGenerated )
177                 Link_InitCRC();
178         
179         tid = Proc_SpawnWorker(Link_WorkerThread, Adapter);     // Create a new worker thread
180         
181         if(tid < 0) {
182                 Log_Warning("Net Link", "Unable to create watcher thread for '%s'", Adapter->Device);
183                 return ;
184         }
185         
186         Log_Log("Net Link", "Watching '%s' using tid %i", Adapter->Device, tid);
187 }
188
189 // From http://www.cl.cam.ac.uk/research/srg/bluebook/21/crc/node6.html
190 #define QUOTIENT        0x04c11db7
191 void Link_InitCRC(void)
192 {
193          int    i, j;
194         Uint32  crc;
195
196         for (i = 0; i < 256; i++)
197         {
198                 crc = i << 24;
199                 for (j = 0; j < 8; j++)
200                 {
201                         if (crc & 0x80000000)
202                                 crc = (crc << 1) ^ QUOTIENT;
203                         else
204                                 crc = crc << 1;
205                 }
206                 gaiLink_CRCTable[i] = crc;
207         }
208         
209         gbLink_CRCTableGenerated = 1;
210 }
211
212 Uint32 Link_CalculateCRC(tIPStackBuffer *Buffer)
213 {
214         Uint32  ret = 0xFFFFFFFF;
215         const void      *data;
216         size_t  length;
217
218          int    id = -1;
219         while( (id = IPStack_Buffer_GetBuffer(Buffer, id, &length, &data)) != -1 )
220         {
221                 ret = Link_CalculatePartialCRC(ret, data, length);
222         }
223
224         return ~ret;
225 }
226
227 Uint32 Link_CalculatePartialCRC(Uint32 CRC, const void *Data, int Length)
228 {
229         // x32 + x26 + x23 + x22 + x16 + x12 + x11 + x10 + x8 + x7 + x5 + x4 + x2 + x + 1
230         const Uint32    *data = Data;
231
232         for( int i = 0; i < Length/4; i++ )
233         {
234                 CRC = (CRC << 8 | *data++) ^ gaiLink_CRCTable[CRC >> 24];
235         }
236
237         return CRC;
238 }

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