X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Modules%2FIPStack%2Fudp.c;h=ad34f856be965ff3034d37a9073fa7a30290fb26;hb=8941c6df2457604e1bab574bccca973189482f61;hp=f014c557ed2c9f490cf355b840055742a11eaf05;hpb=b3fa9a08edcbc459bd8e9df73186e292470ebfc3;p=tpg%2Facess2.git diff --git a/Modules/IPStack/udp.c b/Modules/IPStack/udp.c index f014c557..ad34f856 100644 --- a/Modules/IPStack/udp.c +++ b/Modules/IPStack/udp.c @@ -11,12 +11,8 @@ // === PROTOTYPES === void UDP_Initialise(); void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer); -// --- Listening Server -tVFS_Node *UDP_Server_Init(tInterface *Interface); -char *UDP_Server_ReadDir(tVFS_Node *Node, int ID); -tVFS_Node UDP_Server_FindDir(tVFS_Node *Node, char *Name); - int UDP_Server_IOCtl(tVFS_Node *Node, int ID, void *Data); -void UDP_Server_Close(tVFS_Node *Node); +void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length, void *Buffer); +void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, void *Address, Uint16 Port, 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); @@ -29,12 +25,13 @@ Uint16 UDP_int_AllocatePort(); void UDP_int_FreePort(Uint16 Port); // === GLOBALS === -tSpinlock glUDP_Channels; +tMutex glUDP_Channels; tUDPChannel *gpUDP_Channels; -tSpinlock glUDP_Ports; + +tMutex glUDP_Ports; Uint32 gUDP_Ports[0x10000/32]; -//tSocketFile gUDP_ServerFile = {NULL, "udps", UDP_Server_Init}; -tSocketFile gUDP_ClientFile = {NULL, "udpc", UDP_Channel_Init}; + +tSocketFile gUDP_SocketFile = {NULL, "udp", UDP_Channel_Init}; // === CODE === /** @@ -43,69 +40,91 @@ tSocketFile gUDP_ClientFile = {NULL, "udpc", UDP_Channel_Init}; */ void UDP_Initialise() { - IPStack_AddFile(&gUDP_ClientFile); + IPStack_AddFile(&gUDP_SocketFile); + //IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket, UDP_Unreachable); IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket); } /** - * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer) - * \brief Handles a packet from the IP Layer + * \brief Scan a list of tUDPChannels and find process the first match + * \return 0 if no match was found, -1 on error and 1 if a match was found */ -void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer) +int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer) { tUDPHeader *hdr = Buffer; tUDPChannel *chan; tUDPPacket *pack; int len; - Log("[UDP ] hdr->SourcePort = %i", ntohs(hdr->SourcePort)); - Log("[UDP ] hdr->DestPort = %i", ntohs(hdr->DestPort)); - Log("[UDP ] hdr->Length = %i", ntohs(hdr->Length)); - Log("[UDP ] hdr->Checksum = 0x%x", ntohs(hdr->Checksum)); - - // Check registered connections - LOCK(&glUDP_Channels); - for(chan = gpUDP_Channels; + for(chan = List; chan; chan = chan->Next) { - if(chan->Interface != Interface) continue; + // Match local endpoint + if(chan->Interface && chan->Interface != Interface) continue; if(chan->LocalPort != ntohs(hdr->DestPort)) continue; - if(chan->RemotePort != ntohs(hdr->SourcePort)) continue; - if(Interface->Type == 4) { - if(IP4_EQU(chan->RemoteAddr.v4, *(tIPv4*)Address)) continue; - } - else if(Interface->Type == 6) { - if(IP6_EQU(chan->RemoteAddr.v6, *(tIPv6*)Address)) continue; - } - else { - Warning("[UDP ] Address type %i unknown", Interface->Type); - RELEASE(&glUDP_Channels); - return ; + // Check for remote port restriction + if(chan->Remote.Port && chan->Remote.Port != ntohs(hdr->SourcePort)) + continue; + // Check for remote address restriction + if(chan->RemoteMask) + { + if(chan->Remote.AddrType != Interface->Type) continue; + if(!IPStack_CompareAddress(Interface->Type, Address, + &chan->Remote.Addr, chan->RemoteMask) + ) + continue; } + Log_Log("UDP", "Recieved packet for %p", chan); // Create the cached packet len = ntohs(hdr->Length); pack = malloc(sizeof(tUDPPacket) + len); pack->Next = NULL; + memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type)); + pack->Remote.Port = ntohs(hdr->SourcePort); pack->Length = len; memcpy(pack->Data, hdr->Data, len); // Add the packet to the channel's queue - LOCK(&chan->lQueue); + SHORTLOCK(&chan->lQueue); if(chan->Queue) chan->QueueEnd->Next = pack; else chan->QueueEnd = chan->Queue = pack; - RELEASE(&chan->lQueue); - RELEASE(&glUDP_Channels); - return ; + SHORTREL(&chan->lQueue); + Mutex_Release(&glUDP_Channels); + return 1; } + return 0; +} + +/** + * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer) + * \brief Handles a packet from the IP Layer + */ +void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer) +{ + 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)); - // TODO: Server/Listener + // Check registered connections + Mutex_Acquire(&glUDP_Channels); + UDP_int_ScanList(gpUDP_Channels, Interface, Address, Length, Buffer); + Mutex_Release(&glUDP_Channels); +} + +/** + * \brief Handle an ICMP Unrechable Error + */ +void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length, void *Buffer) +{ - RELEASE(&glUDP_Channels); } /** @@ -114,22 +133,25 @@ void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe * \param Data Packet data * \param Length Length in bytes of packet data */ -void UDP_SendPacket(tUDPChannel *Channel, void *Data, size_t Length) +void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, void *Address, Uint16 Port, void *Data, size_t Length) { tUDPHeader *hdr; + + if(Channel->Interface && Channel->Interface->Type != AddrType) return ; - switch(Channel->Interface->Type) + switch(AddrType) { case 4: // Create the packet hdr = malloc(sizeof(tUDPHeader)+Length); hdr->SourcePort = htons( Channel->LocalPort ); - hdr->DestPort = htons( Channel->RemotePort ); - hdr->Length = htons( Length ); + 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 - IPv4_SendPacket(Channel->Interface, Channel->RemoteAddr.v4, IP4PROT_UDP, 0, sizeof(tUDPHeader)+Length, hdr); + // 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); break; @@ -150,10 +172,10 @@ tVFS_Node *UDP_Channel_Init(tInterface *Interface) new->Node.IOCtl = UDP_Channel_IOCtl; new->Node.Close = UDP_Channel_Close; - LOCK(&glUDP_Channels); + Mutex_Acquire(&glUDP_Channels); new->Next = gpUDP_Channels; gpUDP_Channels = new; - RELEASE(&glUDP_Channels); + Mutex_Release(&glUDP_Channels); return &new->Node; } @@ -165,32 +187,50 @@ Uint64 UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buf { tUDPChannel *chan = Node->ImplPtr; tUDPPacket *pack; + tUDPEndpoint *ep; + int ofs; if(chan->LocalPort == 0) return 0; - if(chan->RemotePort == 0) return 0; while(chan->Queue == NULL) Threads_Yield(); for(;;) { - LOCK(&chan->lQueue); + VFS_SelectNode(Node, VFS_SELECT_READ, NULL, "UDP_Channel_Read"); + SHORTLOCK(&chan->lQueue); if(chan->Queue == NULL) { - RELEASE(&chan->lQueue); + SHORTREL(&chan->lQueue); continue; } pack = chan->Queue; chan->Queue = pack->Next; - if(!chan->Queue) chan->QueueEnd = NULL; - RELEASE(&chan->lQueue); + if(!chan->Queue) { + chan->QueueEnd = NULL; + VFS_MarkAvaliable(Node, 0); // Nothing left + } + SHORTREL(&chan->lQueue); break; } + + // Check that the header fits + ep = Buffer; + ofs = 4 + IPStack_GetAddressSize(pack->Remote.AddrType); + if(Length < ofs) { + free(pack); + return 0; + } + + // Fill header + ep->Port = pack->Remote.Port; + ep->AddrType = pack->Remote.AddrType; + memcpy(&ep->Addr, &pack->Remote.Addr, IPStack_GetAddressSize(pack->Remote.AddrType)); - // Clip length to packet length - if(Length > pack->Length) Length = pack->Length; - // Copy packet data from cache - memcpy(Buffer, pack->Data, Length); + // Copy packet data + if(Length > ofs + pack->Length) Length = ofs + pack->Length; + memcpy((char*)Buffer + ofs, pack->Data, Length - ofs); + // Free cached packet - free(pack); + free(pack); return Length; } @@ -201,9 +241,17 @@ Uint64 UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buf Uint64 UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer) { tUDPChannel *chan = Node->ImplPtr; - if(chan->RemotePort == 0) return 0; + tUDPEndpoint *ep; + void *data; + int ofs; + if(chan->LocalPort == 0) return 0; - UDP_SendPacket(chan, Buffer, (size_t)Length); + ep = Buffer; + ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType ); + + data = (char*)Buffer + ofs; + + UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, Buffer, (size_t)Length - ofs); return 0; } @@ -215,6 +263,7 @@ static const char *casIOCtls_Channel[] = { DRV_IOCTLNAMES, "getset_localport", "getset_remoteport", + "set_remotemask", "set_remoteaddr", NULL }; @@ -261,26 +310,37 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data) LEAVE_RET('i', 1); case 5: // getset_remoteport (returns bool success) - if(!Data) LEAVE_RET('i', chan->RemotePort); + if(!Data) LEAVE_RET('i', chan->Remote.Port); if(!CheckMem( Data, sizeof(Uint16) ) ) { LOG("Invalid pointer %p", Data); LEAVE_RET('i', -1); } - chan->RemotePort = *(Uint16*)Data; + chan->Remote.Port = *(Uint16*)Data; return 1; - case 6: // set_remoteaddr (returns bool success) - switch(chan->Interface->Type) - { - case 4: - if(!CheckMem(Data, sizeof(tIPv4))) { - LOG("Invalid pointer %p", Data); - LEAVE_RET('i', -1); - } - chan->RemoteAddr.v4 = *(tIPv4*)Data; - break; + case 6: // getset_remotemask (returns bool success) + if(!Data) LEAVE_RET('i', chan->RemoteMask); + if(!CheckMem(Data, sizeof(int))) LEAVE_RET('i', -1); + if( !chan->Interface ) { + LOG("Can't set remote mask on NULL interface"); + LEAVE_RET('i', -1); } - break; + if( *(int*)Data > IPStack_GetAddressSize(chan->Interface->Type) ) + LEAVE_RET('i', -1); + chan->RemoteMask = *(int*)Data; + return 1; + + case 7: // set_remoteaddr (returns bool success) + if( !chan->Interface ) { + LOG("Can't set remote address on NULL interface"); + LEAVE_RET('i', -1); + } + if(!CheckMem(Data, IPStack_GetAddressSize(chan->Interface->Type))) { + LOG("Invalid pointer"); + LEAVE_RET('i', -1); + } + memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type)); + return 0; } LEAVE_RET('i', 0); } @@ -294,7 +354,7 @@ void UDP_Channel_Close(tVFS_Node *Node) tUDPChannel *prev; // Remove from the main list first - LOCK(&glUDP_Channels); + Mutex_Acquire(&glUDP_Channels); if(gpUDP_Channels == chan) gpUDP_Channels = gpUDP_Channels->Next; else @@ -303,14 +363,14 @@ void UDP_Channel_Close(tVFS_Node *Node) prev->Next && prev->Next != chan; prev = prev->Next); if(!prev->Next) - Warning("[UDP ] Bookeeping Fail, channel %p is not in main list", chan); + Log_Warning("UDP", "Bookeeping Fail, channel %p is not in main list", chan); else prev->Next = prev->Next->Next; } - RELEASE(&glUDP_Channels); + Mutex_Release(&glUDP_Channels); // Clear Queue - LOCK(&chan->lQueue); + SHORTLOCK(&chan->lQueue); while(chan->Queue) { tUDPPacket *tmp; @@ -318,7 +378,7 @@ void UDP_Channel_Close(tVFS_Node *Node) chan->Queue = tmp->Next; free(tmp); } - RELEASE(&chan->lQueue); + SHORTREL(&chan->lQueue); // Free channel structure free(chan); @@ -330,7 +390,7 @@ void UDP_Channel_Close(tVFS_Node *Node) Uint16 UDP_int_AllocatePort() { int i; - LOCK(&glUDP_Ports); + Mutex_Acquire(&glUDP_Ports); // Fast Search for( i = UDP_ALLOC_BASE; i < 0x10000; i += 32 ) if( gUDP_Ports[i/32] != 0xFFFFFFFF ) @@ -341,7 +401,7 @@ Uint16 UDP_int_AllocatePort() if( !(gUDP_Ports[i/32] & (1 << (i%32))) ) return i; } - RELEASE(&glUDP_Ports); + Mutex_Release(&glUDP_Ports); } /** @@ -350,13 +410,13 @@ Uint16 UDP_int_AllocatePort() */ int UDP_int_MarkPortAsUsed(Uint16 Port) { - LOCK(&glUDP_Ports); + Mutex_Acquire(&glUDP_Ports); if( gUDP_Ports[Port/32] & (1 << (Port%32)) ) { return 0; - RELEASE(&glUDP_Ports); + Mutex_Release(&glUDP_Ports); } gUDP_Ports[Port/32] |= 1 << (Port%32); - RELEASE(&glUDP_Ports); + Mutex_Release(&glUDP_Ports); return 1; } @@ -365,7 +425,7 @@ int UDP_int_MarkPortAsUsed(Uint16 Port) */ void UDP_int_FreePort(Uint16 Port) { - LOCK(&glUDP_Ports); + Mutex_Acquire(&glUDP_Ports); gUDP_Ports[Port/32] &= ~(1 << (Port%32)); - RELEASE(&glUDP_Ports); + Mutex_Release(&glUDP_Ports); }