X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=KernelLand%2FModules%2FIPStack%2Fudp.c;h=9037439cf80023b273974d12ae2c5a512ee9fa51;hb=e14ac96d48bb4ef5cfc845e345df4770e1145ceb;hp=eb8281b41828677fd12bde1ca51c37f7156817cf;hpb=bf0187772ecfb475eedf5e0e9b8460b4f1a3f445;p=tpg%2Facess2.git diff --git a/KernelLand/Modules/IPStack/udp.c b/KernelLand/Modules/IPStack/udp.c index eb8281b4..9037439c 100644 --- a/KernelLand/Modules/IPStack/udp.c +++ b/KernelLand/Modules/IPStack/udp.c @@ -1,7 +1,11 @@ /* * Acess2 IP Stack - * - UDP Handling + * - By John Hodge (thePowersGang) + * + * udp.c + * - UDP Protocol handling */ +#define DEBUG 1 #include "ipstack.h" #include #include "udp.h" @@ -11,27 +15,32 @@ // === 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); -size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer); -size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t 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; @@ -47,8 +56,7 @@ tSocketFile gUDP_SocketFile = {NULL, "udp", UDP_Channel_Init}; 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); } /** @@ -58,15 +66,15 @@ void UDP_Initialise() 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; @@ -86,8 +94,8 @@ int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, in 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); @@ -117,8 +125,16 @@ void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe { tUDPHeader *hdr = Buffer; - Log_Debug("UDP", "%i bytes :%i->:%i (Cksum 0x%04x)", - ntohs(hdr->Length), ntohs(hdr->SourcePort), ntohs(hdr->Length), 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); @@ -129,7 +145,7 @@ void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe /** * \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) { } @@ -144,23 +160,33 @@ void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, U { 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.SourcePort = htons( Channel->LocalPort ); - hdr.DestPort = htons( Port ); - hdr.Length = htons( sizeof(tUDPHeader) + Length ); - hdr.Checksum = 0; // Checksum can be zero on IPv4 // Pass on the the IPv4 Layer - tIPStackBuffer *buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS); + 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? + ASSERT(Channel->Interface); IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer); - IPStack_Buffer_DestroyBuffer(buffer); + break; + default: + Log_Warning("UDP", "TODO: Implement on proto %i", AddrType); break; } } @@ -171,6 +197,7 @@ tVFS_Node *UDP_Channel_Init(tInterface *Interface) 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; @@ -184,45 +211,59 @@ tVFS_Node *UDP_Channel_Init(tInterface *Interface) return &new->Node; } -/** - * \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) +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); @@ -247,7 +288,7 @@ size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buff /** * \brief Write to the channel file (send a packet) */ -size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t 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; @@ -278,6 +319,8 @@ static const char *casIOCtls_Channel[] = { "getset_remoteport", "getset_remotemask", "set_remoteaddr", + "sendto", + "recvfrom", NULL }; /** @@ -291,36 +334,31 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data) { 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); @@ -329,19 +367,28 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data) 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 ) { @@ -352,8 +399,73 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data) 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); } @@ -400,37 +512,55 @@ void UDP_Channel_Close(tVFS_Node *Node) /** * \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; } /** @@ -442,3 +572,59 @@ void UDP_int_FreePort(Uint16 Port) 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); +}