Modules/VIARhineII - RX and TX works, but TX is a hack atm
[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);
86         Uint8   buf[sizeof(tEthernetHeader) + ofs + 4];
87         tEthernetHeader *hdr = (void*)buf;
88
89         if( ofs == 4 )  ofs = 0;        
90
91         Log_Log("Net Link", "Sending %i bytes to %02x:%02x:%02x:%02x:%02x:%02x (Type 0x%x)",
92                 length, To.B[0], To.B[1], To.B[2], To.B[3], To.B[4], To.B[5], Type);
93
94         hdr->Dest = To;
95         memcpy(&hdr->Src, Adapter->HWAddr, 6);  // TODO: Remove hard coded 6
96         hdr->Type = htons(Type);
97         *(Uint32*)(buf + sizeof(tEthernetHeader) + ofs) = 0;
98
99         IPStack_Buffer_AppendSubBuffer(Buffer, sizeof(tEthernetHeader), ofs + 4, hdr, NULL, NULL);
100         
101         *(Uint32*)(buf + sizeof(tEthernetHeader) + ofs) = htonl( Link_CalculateCRC(Buffer) );
102
103         Log_Log("Net Link", " from %02x:%02x:%02x:%02x:%02x:%02x",
104                 hdr->Src.B[0], hdr->Src.B[1], hdr->Src.B[2],
105                 hdr->Src.B[3], hdr->Src.B[4], hdr->Src.B[5]
106                 );
107
108         Adapter_SendPacket(Adapter, Buffer);
109 }
110
111 int Link_HandlePacket(tAdapter *Adapter, tIPStackBuffer *Buffer)
112 {
113         size_t  len = 0;
114         void    *data = IPStack_Buffer_CompactBuffer(Buffer, &len);
115         
116         tEthernetHeader *hdr = (void*)data;
117          int    i;
118         Uint32  checksum;
119
120         if(len < sizeof(tEthernetHeader)) {
121                 Log_Log("Net Link", "Recieved an undersized packet (%i < %i)",
122                         len, sizeof(tEthernetHeader));
123                 free(data);
124                 return 1;
125         }
126                 
127         Log_Log("Net Link",
128                 "Packet from %02x:%02x:%02x:%02x:%02x:%02x"
129                 " to %02x:%02x:%02x:%02x:%02x:%02x (Type=%04x)",
130                 hdr->Src.B[0], hdr->Src.B[1], hdr->Src.B[2],
131                 hdr->Src.B[3], hdr->Src.B[4], hdr->Src.B[5],
132                 hdr->Dest.B[0], hdr->Dest.B[1], hdr->Dest.B[2],
133                 hdr->Dest.B[3], hdr->Dest.B[4], hdr->Dest.B[5],
134                 ntohs(hdr->Type)
135                 );
136         checksum = *(Uint32*)&hdr->Data[len-sizeof(tEthernetHeader)-4];
137         //Log_Log("NET", "Checksum 0x%08x", checksum);
138         // TODO: Check checksum
139         
140         // Check if there is a registered callback for this packet type
141         for( i = giRegisteredTypes; i--; )
142         {
143                 if(gaRegisteredTypes[i].Type == ntohs(hdr->Type))       break;
144         }
145         // No? Ignore it
146         if( i == -1 ) {
147                 Log_Log("Net Link", "Unregistered type 0x%x", ntohs(hdr->Type));
148                 
149                 free(data);     
150                 return 1;
151         }
152         
153         // Call the callback
154         gaRegisteredTypes[i].Callback(
155                 Adapter,
156                 hdr->Src,
157                 len - sizeof(tEthernetHeader),
158                 hdr->Data
159                 );
160         
161         free(data);
162         
163         return 0;
164 }
165
166 // From http://www.cl.cam.ac.uk/research/srg/bluebook/21/crc/node6.html
167 #define QUOTIENT        0x04c11db7
168 void Link_InitCRC(void)
169 {
170          int    i, j;
171         Uint32  crc;
172
173         for (i = 0; i < 256; i++)
174         {
175                 crc = i << 24;
176                 for (j = 0; j < 8; j++)
177                 {
178                         if (crc & 0x80000000)
179                                 crc = (crc << 1) ^ QUOTIENT;
180                         else
181                                 crc = crc << 1;
182                 }
183                 gaiLink_CRCTable[i] = crc;
184         }
185         
186         gbLink_CRCTableGenerated = 1;
187 }
188
189 Uint32 Link_CalculateCRC(tIPStackBuffer *Buffer)
190 {
191         Uint32  ret = 0xFFFFFFFF;
192         const void      *data;
193         size_t  length;
194
195          int    id = -1;
196         while( (id = IPStack_Buffer_GetBuffer(Buffer, id, &length, &data)) != -1 )
197         {
198                 ret = Link_CalculatePartialCRC(ret, data, length);
199         }
200
201         return ~ret;
202 }
203
204 Uint32 Link_CalculatePartialCRC(Uint32 CRC, const void *Data, int Length)
205 {
206         // x32 + x26 + x23 + x22 + x16 + x12 + x11 + x10 + x8 + x7 + x5 + x4 + x2 + x + 1
207         const Uint32    *data = Data;
208
209         for( int i = 0; i < Length/4; i++ )
210         {
211                 CRC = (CRC << 8 | *data++) ^ gaiLink_CRCTable[CRC >> 24];
212         }
213
214         return CRC;
215 }

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