Rewrite from scratch a TCP/IP stack for mbed. So far the following parts are usable: Drivers: - EMAC driver (from CMSIS 2.0) Protocols: - Ethernet protocol - ARP over ethernet for IPv4 - IPv4 over Ethernet - ICMPv4 over IPv4 - UDPv4 over IPv4 APIs: - Sockets for UDPv4 The structure of this stack is designed to be very modular. Each protocol can register one or more protocol to handle its payload, and in each protocol, an API can be hooked (like Sockets for example). This is an early release.
Sockets.c
- Committer:
- Benoit
- Date:
- 2011-06-12
- Revision:
- 0:19f5f51584de
File content as of revision 0:19f5f51584de:
/* * $Id: Sockets.c 29 2011-06-11 14:53:08Z benoit $ * $Author: benoit $ * $Date: 2011-06-11 16:53:08 +0200 (sam., 11 juin 2011) $ * $Rev: 29 $ * * * * * */ #include "NetIF.h" #include "Sockets.h" #include "IPv4.h" #include "UDPv4.h" #include "Debug.h" #include "Queue.h" #include <string.h> #include <stdlib.h> #define DEBUG_CURRENT_MODULE_NAME "sockets" #define DEBUG_CURRENT_MODULE_ID DEBUG_MODULE_SOCKETS #define UDPV4_DATA_OFFSET 8 struct DataBlock { uint8_t *dataPtr, *readPtr; int16_t totalSize, remainingSize; }; typedef struct DataBlock DataBlock_t; enum State { State_Close = 0, State_Open, State_Bound, }; typedef enum State State_t; struct Socket_Entry { //Bool_t inUse; Socket_Family_t family; Socket_Protocol_t protocol; int32_t options; State_t state; Socket_Addr_t *localAddr, *remoteAddr; Queue_t *dataQueue; int32_t index; }; typedef struct Socket_Entry Socket_Entry_t; static Socket_Entry_t socketEntryTable[SOCKET_MAX_COUNT]; static Bool_t socketAPIInitialized = False; static void Init(void); static int32_t Hook(NetIF_t *netIF, Protocol_ID_t protocolID, Packet_t *packet); static void Hook_UDPv4(NetIF_t *netIF, Packet_t *packet, Socket_Entry_t *entry); static Socket_Entry_t *GetSocketEntry(Socket_t socket); static int32_t BindUDPv4(Socket_Entry_t *entry, Socket_AddrIn_t *addrIn); static int32_t Recv_Data(Socket_Entry_t *entry, uint8_t *data, int32_t length); static int32_t SendToUDPv4(Socket_Entry_t *entry, uint8_t *data, int32_t length, Socket_AddrIn_t *remoteAddr); Net_API_t sockets = { API_ID_Sockets, Init, Hook }; static void Init(void) { if (socketAPIInitialized) goto Exit; DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Initializing")); memset(socketEntryTable, 0, sizeof(socketEntryTable)); socketAPIInitialized = True; Exit: return; } static int32_t Hook(NetIF_t *netIF, Protocol_ID_t protocolID, Packet_t *packet) { int32_t index = 0; Socket_Entry_t *entry = NULL; DEBUG_MODULE(DEBUG_LEVEL_VERBOSE0, ("Hook(%s%d, %s, %d bytes)", netIF->name, netIF->index, protocol_IDNames[protocolID], packet->length )); for (index = 0; index < SOCKET_MAX_COUNT; index++) { entry = socketEntryTable + index; if (entry->state != State_Bound) continue; switch(protocolID) { case Protocol_ID_UDPv4: if (entry->protocol == SOCK_DGRAM) Hook_UDPv4(netIF, packet, entry); break; default: continue; } } return 0; } static void Hook_UDPv4(NetIF_t *netIF, Packet_t *packet, Socket_Entry_t *entry) { IPv4_Header_t *ipv4Header; UDPv4_Header_t *udpv4Header; Socket_AddrIn_t *localAddrIn, *remoteAddrIn; int32_t depth; DataBlock_t *dataBlock; depth = packet->depth; ipv4Header = (IPv4_Header_t *)packet->headerPtrTable[depth]; udpv4Header = (UDPv4_Header_t *)(ipv4Header + 1); localAddrIn = (Socket_AddrIn_t *)entry->localAddr; remoteAddrIn = (Socket_AddrIn_t *)entry->remoteAddr; DEBUG_MODULE(DEBUG_LEVEL_VERBOSE0, ("ports: %d.%d.%d.%d:%d to %d.%d.%d.%d:%d size:%d", ipv4Header->source.IP0, ipv4Header->source.IP1, ipv4Header->source.IP2, ipv4Header->source.IP3, ntohs(udpv4Header->destPort), localAddrIn->address.IP0, localAddrIn->address.IP1, localAddrIn->address.IP2, localAddrIn->address.IP3, ntohs(localAddrIn->port), ntohs(udpv4Header->length) )); if ((localAddrIn->port == udpv4Header->destPort) && ( (localAddrIn->address.addr == IPADDR_ANY) || (ipv4Header->dest.addr == localAddrIn->address.addr) ) ) { if (!Queue_IsFull(entry->dataQueue)) { remoteAddrIn->address = ipv4Header->source; remoteAddrIn->port = udpv4Header->sourcePort; dataBlock = (DataBlock_t *)malloc(sizeof(DataBlock_t)); if (dataBlock == NULL) { mbedNet_LastError = mbedNetResult_NotEnoughMemory; goto Exit; } dataBlock->totalSize = ntohs(udpv4Header->length) - sizeof(UDPv4_Header_t); dataBlock->remainingSize = dataBlock->totalSize; dataBlock->dataPtr = (uint8_t *)malloc(dataBlock->totalSize); if (dataBlock->dataPtr == NULL) { free(dataBlock); mbedNet_LastError = mbedNetResult_NotEnoughMemory; goto Exit; } dataBlock->readPtr = dataBlock->dataPtr; memcpy(dataBlock->dataPtr, packet->data + sizeof(UDPv4_Header_t), dataBlock->totalSize); Queue_Push(entry->dataQueue, (void *)dataBlock); DEBUG_MODULE(DEBUG_LEVEL_VERBOSE0, ("Added block of %d bytes to socket %d", dataBlock->totalSize, entry->index)); } } Exit: return; } static int32_t BindUDPv4(Socket_Entry_t *entry, Socket_AddrIn_t *addrIn) { int32_t result = -1; Socket_AddrIn_t *localAddrIn, *remoteAddrIn; entry->localAddr = (Socket_Addr_t *)malloc(sizeof(Socket_AddrIn_t)); if (entry->localAddr == NULL) { mbedNet_LastError = mbedNetResult_NotEnoughMemory; goto Exit; } entry->remoteAddr = (Socket_Addr_t *)malloc(sizeof(Socket_AddrIn_t)); if (entry->remoteAddr == NULL) { free(entry->localAddr); mbedNet_LastError = mbedNetResult_NotEnoughMemory; goto Exit; } localAddrIn = (Socket_AddrIn_t *)entry->localAddr; remoteAddrIn = (Socket_AddrIn_t *)entry->remoteAddr; memcpy(localAddrIn, addrIn, sizeof(Socket_AddrIn_t)); memset(remoteAddrIn, 0, sizeof(Socket_AddrIn_t)); remoteAddrIn->family = addrIn->family; remoteAddrIn->len = addrIn->len; localAddrIn->port = addrIn->port; DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Binding socket %d to %d.%d.%d.%d:%d", entry->index, addrIn->address.IP0, addrIn->address.IP1, addrIn->address.IP2, addrIn->address.IP3, ntohs(addrIn->port) )); Exit: return result; } static int32_t Recv_Data(Socket_Entry_t *entry, uint8_t *data, int32_t length) { int32_t count = 0; DataBlock_t *dataBlock = NULL; Queue_Peek(entry->dataQueue, (void **)&dataBlock); if (dataBlock->remainingSize <= length) { count = dataBlock->remainingSize; Queue_Pop(entry->dataQueue, (void **)&dataBlock); memcpy(data, dataBlock->readPtr, count); free(dataBlock->dataPtr); free(dataBlock); } else { count = length; memcpy(data, dataBlock->readPtr, count); dataBlock->readPtr += count; dataBlock->remainingSize -= count; } return count; } static int32_t SendToUDPv4(Socket_Entry_t *entry, uint8_t *data, int32_t length, Socket_AddrIn_t *remoteAddr) { int32_t count = -1, totalLength; IPv4_Header_t *ipv4Header; UDPv4_Header_t *udpv4Header; Socket_AddrIn_t *localAddrIn, *remoteAddrIn; localAddrIn = (Socket_AddrIn_t *)entry->localAddr; remoteAddrIn = (Socket_AddrIn_t *)entry->remoteAddr; totalLength = length + sizeof(UDPv4_Header_t) + sizeof(IPv4_Header_t); ipv4Header = (IPv4_Header_t *)malloc(totalLength); if (ipv4Header == NULL) { DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Not enough memory (needed %d bytes)", totalLength)); mbedNet_LastError = mbedNetResult_NotEnoughMemory; goto Exit; } memset(ipv4Header, 0, totalLength); DEBUG_SOURCE(DEBUG_LEVEL_VERBOSE0, ("UDPv4 sending %d bytes to %d.%d.%d.%d:%d", length, remoteAddr->address.IP0, remoteAddr->address.IP1, remoteAddr->address.IP2, remoteAddr->address.IP3, ntohs(remoteAddr->port) )); udpv4Header = (UDPv4_Header_t *)(ipv4Header + 1); ipv4Header->ihl = 5; ipv4Header->version = IPV4_VERSION; ipv4Header->tos = 0; ipv4Header->totalLength = htons(5 * 4 + totalLength); ipv4Header->id = 0; ipv4Header->fragmentFlags = 0; ipv4Header->ttl = NET_DEFAULT_TTL; ipv4Header->protocol = IPV4_PROTO_UDPV4; ipv4Header->dest = remoteAddr->address; udpv4Header->sourcePort = localAddrIn->port; udpv4Header->destPort = remoteAddrIn->port; udpv4Header->length = htons(length + sizeof(UDPv4_Header_t)); memcpy(udpv4Header + 1, data, length); DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0) { IPv4_DumpIPv4Header("Sockets:", ipv4Header); } count = NetIF_SendIPv4Packet(ipv4Header); free(ipv4Header); Exit: return count; } Socket_t Sockets_Open(Socket_Family_t family, Socket_Protocol_t protocol, int32_t options) { int32_t result = 0, index = 0; Socket_Entry_t *entry = NULL; if (family != AF_INET) { DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Protocol family not supported")); mbedNet_LastError = mbedNetResult_NotIplemented; result = -1; goto Exit; } for (index = 0; index < SOCKET_MAX_COUNT; index++) { if (socketEntryTable[index].state != State_Close) continue; entry = socketEntryTable + index; break; } if (entry == NULL) { DEBUG_MODULE(DEBUG_LEVEL_WARNING, ("Too many open sockets")); mbedNet_LastError = mbedNetResult_TooManyOpenSockets; result = -1; goto Exit; } entry->family = family; entry->protocol = protocol; entry->options = options; entry->state = State_Open; entry->dataQueue = NULL; entry->index = index; result = index; Exit: DEBUG_MODULE(DEBUG_LEVEL_INFO, ("opened socket %d", index)); return result; } int32_t Sockets_Bind(Socket_t socket, Socket_Addr_t *addr, int32_t addrLen) { int32_t result = -1; Socket_Entry_t *entry; if ((entry = GetSocketEntry(socket)) == NULL) goto Exit; if (entry == NULL) { DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Socket %d not found", socket)); mbedNet_LastError = mbedNetResult_NotEnoughMemory; result = -1; goto Exit; } /* Allocate address entry */ switch(entry->family) { case AF_INET: switch(entry->protocol) { case SOCK_DGRAM: if (addrLen != sizeof(Socket_AddrIn_t)) { mbedNet_LastError = mbedNetResult_InvalidParameter; result = -1; goto Exit; } result = BindUDPv4(entry, (Socket_AddrIn_t *)addr); break; case SOCK_STREAM: DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Protocol not supported")); mbedNet_LastError = mbedNetResult_NotIplemented; result = -1; goto Exit; case SOCK_RAW: DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Protocol not supported")); mbedNet_LastError = mbedNetResult_NotIplemented; result = -1; goto Exit; default: DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Unknown socket protocol")); mbedNet_LastError = mbedNetResult_InvalidParameter; result = -1; goto Exit; } break; default: DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Protocol family not supported")); mbedNet_LastError = mbedNetResult_NotIplemented; result = -1; goto Exit; } entry->dataQueue = Queue_Alloc(SOCKET_DATAQUEUE_ENTRY_COUNT); if (entry == NULL) { free(entry->localAddr); free(entry->remoteAddr); DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Not enough memory to allocate data queue")); mbedNet_LastError = mbedNetResult_NotEnoughMemory; result = -1; goto Exit; } entry->state = State_Bound; result = 0; Exit: return result; } int32_t Sockets_Send(Socket_t socket, uint8_t *data, int32_t length, int32_t flags) { int32_t count = -1; Socket_Entry_t *entry; entry = GetSocketEntry(socket); if (entry == NULL) goto Exit; if (entry->protocol == SOCK_DGRAM) { mbedNet_LastError = mbedNetResult_DestinationAddressRequired; goto Exit; } mbedNet_LastError = mbedNetResult_NotIplemented; Exit: return count; } int32_t Sockets_SendTo(Socket_t socket, uint8_t *data, int32_t length, int32_t flags, const Socket_Addr_t *remoteAddr, int32_t addrLen) { int32_t count = -1; Socket_Entry_t *entry; entry = GetSocketEntry(socket); if (entry == NULL) { DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("socket not found!")); goto Exit; } switch(entry->family) { case AF_INET: switch(entry->protocol) { case SOCK_DGRAM: if (addrLen != sizeof(Socket_AddrIn_t)) { DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Invalid socket address length")); mbedNet_LastError = mbedNetResult_InvalidParameter; goto Exit; } count = SendToUDPv4(entry, data, length, (Socket_AddrIn_t *)remoteAddr); break; default: DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Protocol not implemented")); mbedNet_LastError = mbedNetResult_NotIplemented; goto Exit; } break; default: DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Protocol family not implemented")); mbedNet_LastError = mbedNetResult_NotIplemented; goto Exit; } Exit: return count; } int32_t Sockets_Recv(Socket_t socket, uint8_t *data, int32_t length, int32_t flags) { int32_t count = -1; Socket_Entry_t *entry; entry = GetSocketEntry(socket); if (entry == NULL) goto Exit; if (Queue_IsEmpty(entry->dataQueue)) { mbedNet_LastError = mbedNetResult_WouldBlock; goto Exit; } count = Recv_Data(entry, data, length); Exit: return count; } int32_t Sockets_RecvFrom(Socket_t socket, uint8_t *data, int32_t length, int32_t flags, Socket_Addr_t *remoteAddr, int32_t *addrLen) { int32_t count = -1; Socket_Entry_t *entry; entry = GetSocketEntry(socket); if (entry == NULL) goto Exit; if (Queue_IsEmpty(entry->dataQueue)) { mbedNet_LastError = mbedNetResult_WouldBlock; goto Exit; } if (remoteAddr != NULL) { if (entry->localAddr->len > *addrLen) { mbedNet_LastError = mbedNetResult_BufferTooSmall; goto Exit; } memcpy(remoteAddr, entry->remoteAddr, entry->remoteAddr->len); } count = Recv_Data(entry, data, length); Exit: return count; } int32_t Sockets_Close(Socket_t socket) { int32_t result = -1; Socket_Entry_t *entry; void *ptr; if ((entry = GetSocketEntry(socket)) == NULL) goto Exit; entry->state = State_Close; free(entry->localAddr); entry->localAddr = NULL; free(entry->remoteAddr); entry->remoteAddr = NULL; /* Free pending data blocks */ while(Queue_Peek(entry->dataQueue, &ptr) != -1) { free(ptr); } Queue_Free(entry->dataQueue); entry->dataQueue = NULL; result = 0; Exit: DEBUG_MODULE(DEBUG_LEVEL_INFO, ("closed socket %d", socket)); return result; } static Socket_Entry_t *GetSocketEntry(Socket_t socket) { Socket_Entry_t *entry = NULL; if ((socket < 0) || (socket >= SOCKET_MAX_COUNT)) { DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Invalid socket handle")); mbedNet_LastError = mbedNetResult_InvalidSocketHandle; goto Exit; } entry = socketEntryTable + socket; if (entry->state == State_Close) { DEBUG_MODULE(DEBUG_LEVEL_WARNING, ("Socket already closed")); mbedNet_LastError = mbedNetResult_SocketAlreadyClosed; entry = NULL; goto Exit; } Exit: return entry; }