/*
* Acess2 IP Stack
- * - UDP Handling
+ * - By John Hodge (thePowersGang)
+ *
+ * udp.c
+ * - UDP Protocol handling
*/
+#define DEBUG 1
#include "ipstack.h"
#include <api_drv_common.h>
#include "udp.h"
// === PROTOTYPES ===
void UDP_Initialise();
void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer);
-void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length, void *Buffer);
+void UDP_IPError(tInterface *Interface, tIPErrorMode Code, const void *Address, int Length, const void *Buffer);
void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length);
// --- Client Channels
tVFS_Node *UDP_Channel_Init(tInterface *Interface);
-Uint64 UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer);
-Uint64 UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, const void *Buffer);
+size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags);
+size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags);
int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data);
void UDP_Channel_Close(tVFS_Node *Node);
// --- Helpers
-Uint16 UDP_int_AllocatePort();
- int UDP_int_MarkPortAsUsed(Uint16 Port);
+Uint16 UDP_int_AllocatePort(tUDPChannel *Channel);
+ int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port);
void UDP_int_FreePort(Uint16 Port);
+Uint16 UDP_int_MakeChecksum(tInterface *Iface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data);
+Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data);
+Uint16 UDP_int_FinaliseChecksum(Uint16 Value);
// === GLOBALS ===
tVFS_NodeType gUDP_NodeType = {
+ .TypeName = "UDP",
+ .Flags = VFS_NODETYPEFLAG_STREAM,
.Read = UDP_Channel_Read,
.Write = UDP_Channel_Write,
.IOCtl = UDP_Channel_IOCtl,
.Close = UDP_Channel_Close
};
-tMutex glUDP_Channels;
+tMutex glUDP_Channels; // TODO: Replace with a RWLock
tUDPChannel *gpUDP_Channels;
tMutex glUDP_Ports;
void UDP_Initialise()
{
IPStack_AddFile(&gUDP_SocketFile);
- //IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket, UDP_Unreachable);
- IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket);
+ IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket, UDP_IPError);
}
/**
int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
{
tUDPHeader *hdr = Buffer;
- tUDPChannel *chan;
- tUDPPacket *pack;
- int len;
- for(chan = List;
- chan;
- chan = chan->Next)
+ for(tUDPChannel *chan = List; chan; chan = chan->Next)
{
// Match local endpoint
+ LOG("(%p):%i - %s/%i:%i",
+ chan->Interface, chan->LocalPort,
+ IPStack_PrintAddress(chan->Remote.AddrType, &chan->Remote.Addr), chan->RemoteMask,
+ chan->Remote.Port
+ );
if(chan->Interface && chan->Interface != Interface) continue;
if(chan->LocalPort != ntohs(hdr->DestPort)) continue;
Log_Log("UDP", "Recieved packet for %p", chan);
// Create the cached packet
- len = ntohs(hdr->Length);
- pack = malloc(sizeof(tUDPPacket) + len);
+ int len = ntohs(hdr->Length);
+ tUDPPacket *pack = malloc(sizeof(tUDPPacket) + len);
pack->Next = NULL;
memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type));
pack->Remote.Port = ntohs(hdr->SourcePort);
{
tUDPHeader *hdr = Buffer;
- Log_Debug("UDP", "hdr->SourcePort = %i", ntohs(hdr->SourcePort));
- Log_Debug("UDP", "hdr->DestPort = %i", ntohs(hdr->DestPort));
- Log_Debug("UDP", "hdr->Length = %i", ntohs(hdr->Length));
- Log_Debug("UDP", "hdr->Checksum = 0x%x", ntohs(hdr->Checksum));
+ #if 1
+ size_t len = strlen( IPStack_PrintAddress(Interface->Type, Address) );
+ char tmp[len+1];
+ strcpy(tmp, IPStack_PrintAddress(Interface->Type, Address));
+ Log_Debug("UDP", "%i bytes %s:%i -> %s:%i (Cksum 0x%04x)",
+ ntohs(hdr->Length),
+ tmp, ntohs(hdr->SourcePort),
+ IPStack_PrintAddress(Interface->Type, Interface->Address), ntohs(hdr->DestPort),
+ ntohs(hdr->Checksum));
+ #endif
// Check registered connections
Mutex_Acquire(&glUDP_Channels);
/**
* \brief Handle an ICMP Unrechable Error
*/
-void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length, void *Buffer)
+void UDP_IPError(tInterface *Interface, tIPErrorMode Code, const void *Address, int Length, const void *Buffer)
{
}
*/
void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length)
{
- tUDPHeader *hdr;
+ tUDPHeader hdr;
- if(Channel->Interface && Channel->Interface->Type != AddrType) return ;
+ if(Channel->Interface && Channel->Interface->Type != AddrType) {
+ LOG("Bad interface type for channel packet, IF is %i, but packet is %i",
+ Channel->Interface->Type, AddrType);
+ return ;
+ }
+
+ // Create the packet
+ hdr.SourcePort = htons( Channel->LocalPort );
+ hdr.DestPort = htons( Port );
+ hdr.Length = htons( sizeof(tUDPHeader) + Length );
+ hdr.Checksum = 0;
+ hdr.Checksum = htons( UDP_int_MakeChecksum(Channel->Interface, Address, &hdr, Length, Data) );
+ tIPStackBuffer *buffer;
switch(AddrType)
{
case 4:
- // Create the packet
- hdr = malloc(sizeof(tUDPHeader)+Length);
- hdr->SourcePort = htons( Channel->LocalPort );
- hdr->DestPort = htons( Port );
- hdr->Length = htons( sizeof(tUDPHeader) + Length );
- hdr->Checksum = 0; // Checksum can be zero on IPv4
- memcpy(hdr->Data, Data, Length);
// Pass on the the IPv4 Layer
+ buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS);
+ IPStack_Buffer_AppendSubBuffer(buffer, Length, 0, Data, NULL, NULL);
+ IPStack_Buffer_AppendSubBuffer(buffer, sizeof(hdr), 0, &hdr, NULL, NULL);
// TODO: What if Channel->Interface is NULL here?
- IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, sizeof(tUDPHeader)+Length, hdr);
- // Free allocated packet
- free(hdr);
+ ASSERT(Channel->Interface);
+ IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer);
+ break;
+ default:
+ Log_Warning("UDP", "TODO: Implement on proto %i", AddrType);
break;
}
}
tUDPChannel *new;
new = calloc( sizeof(tUDPChannel), 1 );
new->Interface = Interface;
+ new->Node.Size = -1;
new->Node.ImplPtr = new;
new->Node.NumACLs = 1;
new->Node.ACLs = &gVFS_ACL_EveryoneRW;
return &new->Node;
}
-/**
- * \brief Read from the channel file (wait for a packet)
- */
-Uint64 UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
+tUDPPacket *UDP_Channel_WaitForPacket(tUDPChannel *chan, Uint VFSFlags)
{
- tUDPChannel *chan = Node->ImplPtr;
- tUDPPacket *pack;
- tUDPEndpoint *ep;
- int ofs, addrlen;
-
- if(chan->LocalPort == 0) {
- Log_Notice("UDP", "Channel %p sent with no local port", chan);
- return 0;
- }
-
- while(chan->Queue == NULL) Threads_Yield();
+ // EVIL - Yield until queue is created (avoids races)
+ while(chan->Queue == NULL)
+ Threads_Yield();
for(;;)
{
- VFS_SelectNode(Node, VFS_SELECT_READ, NULL, "UDP_Channel_Read");
+ tTime timeout_z = 0, *timeout = (VFSFlags & VFS_IOFLAG_NOBLOCK) ? &timeout_z : NULL;
+ int rv = VFS_SelectNode(&chan->Node, VFS_SELECT_READ, timeout, "UDP_Channel_Read");
+ if( rv == 0 ) {
+ errno = (VFSFlags & VFS_IOFLAG_NOBLOCK) ? EWOULDBLOCK : EINTR;
+ return NULL;
+ }
SHORTLOCK(&chan->lQueue);
if(chan->Queue == NULL) {
SHORTREL(&chan->lQueue);
continue;
}
- pack = chan->Queue;
+ tUDPPacket *pack = chan->Queue;
chan->Queue = pack->Next;
if(!chan->Queue) {
chan->QueueEnd = NULL;
- VFS_MarkAvaliable(Node, 0); // Nothing left
+ VFS_MarkAvaliable(&chan->Node, 0); // Nothing left
}
SHORTREL(&chan->lQueue);
- break;
+ return pack;
}
+ // Unreachable
+}
+/**
+ * \brief Read from the channel file (wait for a packet)
+ */
+size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
+{
+ tUDPChannel *chan = Node->ImplPtr;
+
+ if(chan->LocalPort == 0) {
+ Log_Notice("UDP", "Channel %p sent with no local port", chan);
+ return 0;
+ }
+
+ tUDPPacket *pack = UDP_Channel_WaitForPacket(chan, Flags);
+ if( !pack ) {
+ return 0;
+ }
+
+ size_t addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
+ tUDPEndpoint *ep = Buffer;
+ size_t ofs = 4 + addrlen;
+
// Check that the header fits
- addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
- ep = Buffer;
- ofs = 4 + addrlen;
if(Length < ofs) {
free(pack);
Log_Notice("UDP", "Insuficient space for header in buffer (%i < %i)", (int)Length, ofs);
/**
* \brief Write to the channel file (send a packet)
*/
-Uint64 UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, const void *Buffer)
+size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags)
{
tUDPChannel *chan = Node->ImplPtr;
const tUDPEndpoint *ep;
const void *data;
int ofs;
- if(chan->LocalPort == 0) return 0;
+
+ if(chan->LocalPort == 0) {
+ Log_Notice("UDP", "Write to channel %p with zero local port", chan);
+ return 0;
+ }
ep = Buffer;
ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
- return 0;
+ return Length;
}
/**
"getset_remoteport",
"getset_remotemask",
"set_remoteaddr",
+ "sendto",
+ "recvfrom",
NULL
};
/**
{
BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
- case 4: // getset_localport (returns bool success)
+ case 4: { // getset_localport (returns bool success)
if(!Data) LEAVE_RET('i', chan->LocalPort);
if(!CheckMem( Data, sizeof(Uint16) ) ) {
LOG("Invalid pointer %p", Data);
LEAVE_RET('i', -1);
}
// Set port
- chan->LocalPort = *(Uint16*)Data;
+ int req_port = *(Uint16*)Data;
// Permissions check (Ports lower than 1024 are root-only)
- if(chan->LocalPort != 0 && chan->LocalPort < 1024) {
+ if(req_port != 0 && req_port < 1024) {
if( Threads_GetUID() != 0 ) {
- LOG("Attempt by non-superuser to listen on port %i", chan->LocalPort);
- chan->LocalPort = 0;
+ LOG("Attempt by non-superuser to listen on port %i", req_port);
LEAVE_RET('i', -1);
}
}
// Allocate a random port if requested
- if( chan->LocalPort == 0 )
- chan->LocalPort = UDP_int_AllocatePort();
- else
- {
- // Else, mark the requested port as used
- if( UDP_int_MarkPortAsUsed(chan->LocalPort) == 0 ) {
- LOG("Port %i us currently in use", chan->LocalPort);
- chan->LocalPort = 0;
- LEAVE_RET('i', 0);
- }
- LEAVE_RET('i', 1);
+ if( req_port == 0 )
+ UDP_int_AllocatePort(chan);
+ // Else, mark the requested port as used
+ else if( UDP_int_ClaimPort(chan, req_port) ) {
+ LOG("Port %i is currently in use", req_port);
+ LEAVE_RET('i', 0);
+ }
+ LEAVE_RET('i', chan->LocalPort);
}
- LEAVE_RET('i', 1);
case 5: // getset_remoteport (returns bool success)
if(!Data) LEAVE_RET('i', chan->Remote.Port);
LEAVE_RET('i', -1);
}
chan->Remote.Port = *(Uint16*)Data;
- return 1;
+ LEAVE('i', chan->Remote.Port);
+ return chan->Remote.Port;
case 6: // getset_remotemask (returns bool success)
if(!Data) LEAVE_RET('i', chan->RemoteMask);
- if(!CheckMem(Data, sizeof(int))) LEAVE_RET('i', -1);
+ if(!CheckMem(Data, sizeof(int))) {
+ LOG("Data pointer invalid");
+ LEAVE_RET('i', -1);
+ }
if( !chan->Interface ) {
LOG("Can't set remote mask on NULL interface");
LEAVE_RET('i', -1);
}
- if( *(int*)Data > IPStack_GetAddressSize(chan->Interface->Type) )
+ int mask = *(int*)Data;
+ int addr_bits = IPStack_GetAddressSize(chan->Interface->Type) * 8;
+ if( mask > addr_bits ) {
+ LOG("Mask too large (%i > max %i)", mask, addr_bits);
LEAVE_RET('i', -1);
- chan->RemoteMask = *(int*)Data;
- return 1;
+ }
+ chan->RemoteMask = mask;
+ LEAVE('i', chan->RemoteMask);
+ return chan->RemoteMask;
case 7: // set_remoteaddr (returns bool success)
if( !chan->Interface ) {
LOG("Invalid pointer");
LEAVE_RET('i', -1);
}
+ LOG("Set remote addr %s", IPStack_PrintAddress(chan->Interface->Type, Data));
+ chan->Remote.AddrType = chan->Interface->Type;
memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type));
+ LEAVE('i', 0);
return 0;
+ case 8: { // sendto
+ if(!CheckMem(Data, 2*sizeof(void*)+2)) {
+ LOG("Data pointer invalid");
+ LEAVE_RET('i', -1);
+ }
+ const struct sSendToArgs {
+ const tUDPEndpoint* ep;
+ const void* buf;
+ const Uint16 buflen;
+ } info = *(const struct sSendToArgs*)Data;
+ LOG("sendto(buf=%p + %u, ep=%p)", info.buf, info.buflen, info.ep);
+ if(!CheckMem(info.ep, 2+2) || !CheckMem(info.ep, 2+2+IPStack_GetAddressSize(info.ep->AddrType)) ) {
+ LEAVE_RET('i', -1);
+ }
+ if(!CheckMem(info.buf, info.buflen)) {
+ LEAVE_RET('i', -1);
+ }
+
+ UDP_SendPacketTo(chan, info.ep->AddrType, &info.ep->Addr, info.ep->Port,
+ info.buf, (size_t)info.buflen);
+
+ LEAVE_RET('i', info.buflen); }
+ case 9: { // recvfrom
+ if(!CheckMem(Data, 2*sizeof(void*)+2)) {
+ LOG("Data pointer invalid");
+ LEAVE_RET('i', -1);
+ }
+ const struct sRecvFromArgs {
+ tUDPEndpoint* ep;
+ void* buf;
+ Uint16 buflen;
+ } info = *(const struct sRecvFromArgs*)Data;
+ LOG("recvfrom(buf=%p + %u, ep=%p)", info.buf, info.buflen, info.ep);
+ if(!CheckMem(info.ep, 2+2)) {
+ LEAVE_RET('i', -1);
+ }
+ if(!CheckMem(info.buf, info.buflen)) {
+ LEAVE_RET('i', -1);
+ }
+
+ tUDPPacket *pack = UDP_Channel_WaitForPacket(chan, 0);
+ if( pack == NULL ) {
+ LOG("No packet");
+ LEAVE_RET('i', 0);
+ }
+
+ size_t addrsize = IPStack_GetAddressSize(pack->Remote.AddrType);
+ if( !CheckMem(info.ep, 2+2+addrsize) ) {
+ LOG("Insufficient space for source address");
+ free(pack);
+ LEAVE_RET('i', -1);
+ }
+ info.ep->Port = pack->Remote.Port;
+ info.ep->AddrType = pack->Remote.AddrType;
+ memcpy(&info.ep->Addr, &pack->Remote.Addr, addrsize);
+
+ size_t retlen = (info.buflen < pack->Length ? info.buflen : pack->Length);
+ memcpy(info.buf, pack->Data, retlen);
+
+ free(pack);
+
+ LEAVE_RET('i', retlen); }
}
LEAVE_RET('i', 0);
}
/**
* \return Port Number on success, or zero on failure
*/
-Uint16 UDP_int_AllocatePort()
+Uint16 UDP_int_AllocatePort(tUDPChannel *Channel)
{
- int i;
Mutex_Acquire(&glUDP_Ports);
// Fast Search
- for( i = UDP_ALLOC_BASE; i < 0x10000; i += 32 )
- if( gUDP_Ports[i/32] != 0xFFFFFFFF )
- break;
- if(i == 0x10000) return 0;
- for( ;; i++ )
+ for( int base = UDP_ALLOC_BASE; base < 0x10000; base += 32 )
{
- if( !(gUDP_Ports[i/32] & (1 << (i%32))) )
- return i;
+ if( gUDP_Ports[base/32] == 0xFFFFFFFF )
+ continue ;
+ for( int i = 0; i < 32; i++ )
+ {
+ if( gUDP_Ports[base/32] & (1 << i) )
+ continue ;
+ gUDP_Ports[base/32] |= (1 << i);
+ Mutex_Release(&glUDP_Ports);
+ // If claim succeeds, good
+ if( UDP_int_ClaimPort(Channel, base + i) == 0 )
+ return base + i;
+ // otherwise keep looking
+ Mutex_Acquire(&glUDP_Ports);
+ break;
+ }
}
Mutex_Release(&glUDP_Ports);
+ return 0;
}
/**
* \brief Allocate a specific port
* \return Boolean Success
*/
-int UDP_int_MarkPortAsUsed(Uint16 Port)
+int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port)
{
- Mutex_Acquire(&glUDP_Ports);
- if( gUDP_Ports[Port/32] & (1 << (Port%32)) ) {
- return 0;
- Mutex_Release(&glUDP_Ports);
+ // Search channel list for a connection with same (or wildcard)
+ // interface, and same port
+ Mutex_Acquire(&glUDP_Channels);
+ for( tUDPChannel *ch = gpUDP_Channels; ch; ch = ch->Next)
+ {
+ if( ch == Channel )
+ continue ;
+ if( ch->Interface && ch->Interface != Channel->Interface )
+ continue ;
+ if( ch->LocalPort != Port )
+ continue ;
+ Mutex_Release(&glUDP_Channels);
+ return 1;
}
- gUDP_Ports[Port/32] |= 1 << (Port%32);
- Mutex_Release(&glUDP_Ports);
- return 1;
+ Channel->LocalPort = Port;
+ Mutex_Release(&glUDP_Channels);
+ return 0;
}
/**
gUDP_Ports[Port/32] &= ~(1 << (Port%32));
Mutex_Release(&glUDP_Ports);
}
+
+/**
+ *
+ */
+Uint16 UDP_int_MakeChecksum(tInterface *Interface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data)
+{
+ size_t addrsize = IPStack_GetAddressSize(Interface->Type);
+ struct {
+ Uint8 Zeroes;
+ Uint8 Protocol;
+ Uint16 UDPLength;
+ } pheader;
+
+ pheader.Zeroes = 0;
+ switch(Interface->Type)
+ {
+ case 4: pheader.Protocol = IP4PROT_UDP; break;
+ //case 6: pheader.Protocol = IP6PROT_UDP; break;
+ default:
+ Log_Warning("UDP", "Unimplemented _MakeChecksum proto %i", Interface->Type);
+ return 0;
+ }
+ pheader.UDPLength = Hdr->Length;
+
+ Uint16 csum = 0;
+ csum = UDP_int_PartialChecksum(csum, addrsize, Interface->Address);
+ csum = UDP_int_PartialChecksum(csum, addrsize, Dest);
+ csum = UDP_int_PartialChecksum(csum, sizeof(pheader), &pheader);
+ csum = UDP_int_PartialChecksum(csum, sizeof(tUDPHeader), Hdr);
+ csum = UDP_int_PartialChecksum(csum, Len, Data);
+
+ return UDP_int_FinaliseChecksum(csum);
+}
+
+static inline Uint16 _add_ones_complement16(Uint16 a, Uint16 b)
+{
+ // One's complement arithmatic, overflows increment bottom bit
+ return a + b + (b > 0xFFFF - a ? 1 : 0);
+}
+
+Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data)
+{
+ Uint16 ret = Prev;
+ const Uint16 *data = Data;
+ for( int i = 0; i < Len/2; i ++ )
+ ret = _add_ones_complement16(ret, htons(*data++));
+ if( Len % 2 == 1 )
+ ret = _add_ones_complement16(ret, htons(*(const Uint8*)data));
+ return ret;
+}
+
+Uint16 UDP_int_FinaliseChecksum(Uint16 Value)
+{
+ Value = ~Value; // One's complement it
+ return (Value == 0 ? 0xFFFF : Value);
+}
git.ucc.asn.au / tpg / acess2.git / blobdiff
UCC git Repository :: git.ucc.asn.au