Sockets.cpp

00001 /*
00002  * $Id: Sockets.c 29 2011-06-11 14:53:08Z benoit $
00003  * $Author: benoit $
00004  * $Date: 2011-06-11 16:53:08 +0200 (sam., 11 juin 2011) $
00005  * $Rev: 29 $
00006  * 
00007  * 
00008  * 
00009  * 
00010  * 
00011  */
00012  
00013 #include "NetIF.h"
00014 #include "Sockets.h"
00015 #include "IPv4.h"
00016 #include "UDPv4.h"
00017 #include "Debug.h"
00018 #include "CQueue.h"
00019 #include <string.h>
00020 #include <stdlib.h>
00021 
00022 
00023 #define    DEBUG_CURRENT_MODULE_NAME    "sockets"
00024 #define    DEBUG_CURRENT_MODULE_ID        DEBUG_MODULE_SOCKETS
00025 
00026 
00027 #define    UDPV4_DATA_OFFSET            8
00028 
00029 
00030 struct DataBlock
00031 {
00032     uint8_t        *dataPtr,
00033                 *readPtr;
00034     int16_t        totalSize, 
00035                 remainingSize;
00036 };
00037 typedef struct DataBlock DataBlock_t;
00038 
00039 
00040 enum State
00041 {
00042     State_Close = 0,
00043     State_Open,
00044     State_Bound,
00045 };
00046 typedef enum State State_t;
00047 
00048 
00049 struct Socket_Entry
00050 {
00051     Socket_Family_t     family;
00052     Socket_Protocol_t   protocol;
00053     int32_t             options;
00054     State_t             state;
00055     Socket_Addr_t       *localAddr,
00056                         *remoteAddr;
00057     CQueue_t            *dataQueue;
00058     int32_t             index;
00059 };
00060 typedef struct Socket_Entry Socket_Entry_t;
00061 
00062 
00063 static Socket_Entry_t    socketEntryTable[SOCKET_MAX_COUNT];
00064 static Bool_t            socketAPIInitialized = False;
00065 
00066 
00067 static void             Init(void);
00068 static int32_t          Hook(NetIF_t *netIF, Protocol_ID_t protocolID, NetPacket_t *packet);
00069 static void             Hook_UDPv4(NetIF_t *netIF, NetPacket_t *packet, Socket_Entry_t *entry);
00070 static Socket_Entry_t   *GetSocketEntry(Socket_t socket);
00071 static int32_t          BindUDPv4(Socket_Entry_t    *entry, Socket_AddrIn_t *addrIn);
00072 static int32_t          Recv_Data(Socket_Entry_t *entry, uint8_t *data, int32_t length);
00073 static int32_t          SendToUDPv4(Socket_Entry_t *entry, uint8_t *data, int32_t length, Socket_AddrIn_t *remoteAddr);
00074 
00075 
00076 Net_API_t    sockets = 
00077 {
00078     API_ID_Sockets,
00079     Init,
00080     Hook
00081 };
00082 
00083 
00084 static void Init(void)
00085 {
00086     if (socketAPIInitialized) goto Exit;
00087     
00088     DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Initializing"));
00089     memset(socketEntryTable, 0, sizeof(socketEntryTable));
00090     socketAPIInitialized = True;
00091     
00092 Exit:
00093     return;
00094 }
00095 
00096 
00097 static int32_t Hook(NetIF_t *netIF, Protocol_ID_t protocolID, NetPacket_t *packet)
00098 {
00099     int32_t                index = 0;
00100     Socket_Entry_t        *entry = NULL;
00101 
00102     DEBUG_MODULE(DEBUG_LEVEL_VERBOSE0, ("Hook(%s%d, %s, %d bytes)",
00103         netIF->name,
00104         netIF->index,
00105         protocol_IDNames[protocolID],
00106         packet->length
00107     ));
00108     
00109     for (index = 0; index < SOCKET_MAX_COUNT; index++)
00110     {
00111         entry = socketEntryTable + index;
00112         if (entry->state != State_Bound) continue;
00113         switch(protocolID)
00114         {
00115             case Protocol_ID_UDPv4:
00116                 if (entry->protocol == SOCK_DGRAM) Hook_UDPv4(netIF, packet, entry);
00117                 break;
00118                 
00119             default:
00120                 continue;
00121         }
00122     }
00123         
00124     return 0;
00125 }
00126 
00127 static void Hook_UDPv4(NetIF_t *netIF, NetPacket_t *packet, Socket_Entry_t *entry)
00128 {
00129     IPv4_Header_t       *ipv4Header;
00130     UDPv4_Header_t      *udpv4Header;
00131     Socket_AddrIn_t     *localAddrIn, *remoteAddrIn;
00132     int32_t             depth;
00133     DataBlock_t         *dataBlock;
00134 
00135     depth = packet->depth;
00136     ipv4Header = (IPv4_Header_t *)packet->headerPtrTable[depth];
00137     udpv4Header = (UDPv4_Header_t *)(ipv4Header + 1);
00138     localAddrIn = (Socket_AddrIn_t *)entry->localAddr;
00139     remoteAddrIn = (Socket_AddrIn_t *)entry->remoteAddr;
00140     DEBUG_MODULE(DEBUG_LEVEL_VERBOSE0, ("ports: %d.%d.%d.%d:%d to %d.%d.%d.%d:%d size:%d",
00141         ipv4Header->source.IP0,
00142         ipv4Header->source.IP1,
00143         ipv4Header->source.IP2,
00144         ipv4Header->source.IP3,
00145         ntohs(udpv4Header->destPort),
00146         localAddrIn->address.IP0,
00147         localAddrIn->address.IP1,
00148         localAddrIn->address.IP2,
00149         localAddrIn->address.IP3,
00150         ntohs(localAddrIn->port),
00151         ntohs(udpv4Header->length)
00152     ));
00153     if ((localAddrIn->port == udpv4Header->destPort) && ( (localAddrIn->address.addr == IPADDR_ANY) || (ipv4Header->dest.addr == localAddrIn->address.addr) ) )
00154     {
00155         if (!CQueue_IsFull(entry->dataQueue))
00156         {
00157             remoteAddrIn->address = ipv4Header->source;
00158             remoteAddrIn->port = udpv4Header->sourcePort;
00159             dataBlock = (DataBlock_t *)malloc(sizeof(DataBlock_t));
00160             if (dataBlock == NULL)
00161             {
00162                 mbedNet_LastError = mbedNetResult_NotEnoughMemory;
00163                 goto Exit;
00164             }
00165             dataBlock->totalSize = ntohs(udpv4Header->length) - sizeof(UDPv4_Header_t);
00166             dataBlock->remainingSize = dataBlock->totalSize;
00167             dataBlock->dataPtr = (uint8_t *)malloc(dataBlock->totalSize);
00168             if (dataBlock->dataPtr == NULL)
00169             {
00170                 free(dataBlock);
00171                 mbedNet_LastError = mbedNetResult_NotEnoughMemory;
00172                 goto Exit;
00173             }
00174             dataBlock->readPtr = dataBlock->dataPtr;
00175             memcpy(dataBlock->dataPtr, packet->data + sizeof(UDPv4_Header_t), dataBlock->totalSize);
00176             CQueue_Push(entry->dataQueue, (void *)dataBlock);
00177             DEBUG_MODULE(DEBUG_LEVEL_VERBOSE0, ("Added block of %d bytes to socket %d", dataBlock->totalSize, entry->index));
00178         }
00179     }
00180     
00181 Exit:
00182     return;
00183 }
00184 
00185 static int32_t BindUDPv4(Socket_Entry_t *entry, Socket_AddrIn_t *addrIn)
00186 {
00187     int32_t                 result = -1;
00188     Socket_AddrIn_t         *localAddrIn,
00189                             *remoteAddrIn;
00190 
00191     /* Allocate local internet v4 addr */
00192     entry->localAddr = (Socket_Addr_t *)malloc(sizeof(Socket_AddrIn_t));
00193     if (entry->localAddr == NULL)
00194     {
00195         mbedNet_LastError = mbedNetResult_NotEnoughMemory;
00196         goto Exit;
00197     }
00198 
00199     /* Allocate remote internet v4 addr */
00200     entry->remoteAddr = (Socket_Addr_t *)malloc(sizeof(Socket_AddrIn_t));
00201     if (entry->remoteAddr == NULL)
00202     {
00203         free(entry->localAddr);
00204         mbedNet_LastError = mbedNetResult_NotEnoughMemory;
00205         goto Exit;
00206     }
00207     
00208     /* Setup local socket address */
00209     localAddrIn = (Socket_AddrIn_t *)entry->localAddr;
00210     memcpy(localAddrIn, addrIn, sizeof(Socket_AddrIn_t));
00211 
00212     /* Setup remote socket adress, copy from local address, set port & address to zero */
00213     remoteAddrIn = (Socket_AddrIn_t *)entry->remoteAddr;
00214     *remoteAddrIn = *localAddrIn;
00215     remoteAddrIn->port = 0;
00216     remoteAddrIn->address.addr = 0;
00217         
00218     DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Binding socket %d to %d.%d.%d.%d:%d", 
00219         entry->index, 
00220         addrIn->address.IP0, 
00221         addrIn->address.IP1, 
00222         addrIn->address.IP2, 
00223         addrIn->address.IP3, 
00224         ntohs(addrIn->port)
00225     ));
00226     
00227 Exit:
00228     return result;
00229 }
00230 
00231 
00232 static int32_t Recv_Data(Socket_Entry_t *entry, uint8_t *data, int32_t length)
00233 {
00234     int32_t            count = 0;
00235     DataBlock_t        *dataBlock = NULL;
00236 
00237     CQueue_Peek(entry->dataQueue, (void **)&dataBlock);
00238     if (dataBlock->remainingSize <= length)
00239     {
00240         count = dataBlock->remainingSize;
00241         CQueue_Pop(entry->dataQueue, (void **)&dataBlock);
00242         memcpy(data, dataBlock->readPtr, count);
00243         free(dataBlock->dataPtr);
00244         free(dataBlock);
00245     }
00246     else
00247     {
00248         count = length;
00249         memcpy(data, dataBlock->readPtr, count);
00250         dataBlock->readPtr += count;
00251         dataBlock->remainingSize -= count;
00252     }
00253     return count;
00254 }
00255 
00256 
00257 static int32_t SendToUDPv4(Socket_Entry_t *entry, uint8_t *data, int32_t length, Socket_AddrIn_t *remoteAddrIn)
00258 {
00259     int32_t                count = -1,
00260                         totalLength;
00261     IPv4_Header_t        *ipv4Header;
00262     UDPv4_Header_t        *udpv4Header;
00263     Socket_AddrIn_t        *localAddrIn;
00264     
00265     localAddrIn = (Socket_AddrIn_t *)entry->localAddr;
00266     totalLength = length + sizeof(UDPv4_Header_t) + sizeof(IPv4_Header_t);
00267     ipv4Header = (IPv4_Header_t *)malloc(totalLength);
00268     if (ipv4Header == NULL)
00269     {
00270         DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Not enough memory (needed %d bytes)", totalLength));
00271         mbedNet_LastError = mbedNetResult_NotEnoughMemory;
00272         goto Exit;
00273     }
00274     
00275     memset(ipv4Header, 0, totalLength);
00276     
00277     DEBUG_SOURCE(DEBUG_LEVEL_VERBOSE0, ("UDPv4 sending %d bytes to %d.%d.%d.%d:%d",
00278         length,
00279         remoteAddrIn->address.IP0,
00280         remoteAddrIn->address.IP1,
00281         remoteAddrIn->address.IP2,
00282         remoteAddrIn->address.IP3,
00283         ntohs(remoteAddrIn->port)
00284     ));
00285     
00286     udpv4Header = (UDPv4_Header_t *)(ipv4Header + 1);
00287     
00288     ipv4Header->ihl = 5;
00289     ipv4Header->version = IPV4_VERSION;
00290     ipv4Header->tos = 0;
00291     ipv4Header->totalLength = htons(5 * 4 + totalLength);
00292     ipv4Header->id = 0;
00293     ipv4Header->fragmentFlags = 0;
00294     ipv4Header->ttl = NET_DEFAULT_TTL;
00295     ipv4Header->protocol = IPV4_PROTO_UDPV4;
00296     ipv4Header->dest = remoteAddrIn->address;
00297     
00298     udpv4Header->sourcePort = localAddrIn->port;
00299     udpv4Header->destPort = remoteAddrIn->port;
00300     udpv4Header->length = htons(length + sizeof(UDPv4_Header_t));
00301     
00302     memcpy(udpv4Header + 1, data, length);
00303     
00304     DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0)
00305     {
00306         IPv4_DumpIPv4Header("Sockets:", ipv4Header);
00307     }
00308     
00309     count = NetIF_SendIPv4Packet(ipv4Header);
00310     free(ipv4Header);
00311 
00312 Exit:
00313     return count;
00314 }
00315 
00316 
00317 Socket_t Sockets_Open(Socket_Family_t family, Socket_Protocol_t protocol, int32_t options)
00318 {
00319     int32_t            result = 0,
00320                     index = 0;
00321     Socket_Entry_t    *entry = NULL;
00322 
00323     if (family != AF_INET)
00324     {
00325         DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Protocol family not supported"));
00326         mbedNet_LastError = mbedNetResult_NotIplemented;
00327         result = -1;
00328         goto Exit;
00329     }
00330 
00331     for (index = 0; index < SOCKET_MAX_COUNT; index++)
00332     {
00333         if (socketEntryTable[index].state != State_Close) continue;
00334         entry = socketEntryTable + index;
00335         break;
00336     }
00337 
00338     if (entry == NULL)
00339     {
00340         DEBUG_MODULE(DEBUG_LEVEL_WARNING, ("Too many open sockets"));
00341         mbedNet_LastError = mbedNetResult_TooManyOpenSockets;
00342         result = -1;
00343         goto Exit;
00344     }
00345     
00346     entry->family = family;
00347     entry->protocol = protocol;
00348     entry->options = options;
00349     entry->state = State_Open;
00350     entry->dataQueue = NULL;
00351     entry->index = index;
00352     result = index;
00353 
00354 Exit:
00355     DEBUG_MODULE(DEBUG_LEVEL_INFO, ("opened socket %d", index));
00356     return result;
00357 }
00358 
00359 
00360 int32_t Sockets_Bind(Socket_t socket, Socket_Addr_t *addr, int32_t addrLen)
00361 {
00362     int32_t            result = -1;
00363     Socket_Entry_t    *entry;
00364 
00365     if ((entry = GetSocketEntry(socket)) == NULL) goto Exit;
00366     
00367     if (entry == NULL)
00368     {
00369         DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Socket %d not found", socket));
00370         mbedNet_LastError = mbedNetResult_NotEnoughMemory;
00371         result = -1;
00372         goto Exit;
00373     }
00374 
00375     /* Allocate address entry */
00376     switch(entry->family)
00377     {
00378         case AF_INET:
00379             switch(entry->protocol)
00380             {
00381                 case SOCK_DGRAM:
00382                     if (addrLen != sizeof(Socket_AddrIn_t))
00383                     {
00384                         mbedNet_LastError = mbedNetResult_InvalidParameter;
00385                         result = -1;
00386                         goto Exit;
00387                     }
00388                     result = BindUDPv4(entry, (Socket_AddrIn_t *)addr);
00389                     break;
00390                     
00391                 case SOCK_STREAM:
00392                     DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Protocol not supported"));
00393                     mbedNet_LastError = mbedNetResult_NotIplemented;
00394                     result = -1;
00395                     goto Exit;
00396                     
00397                 case SOCK_RAW:
00398                     DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Protocol not supported"));
00399                     mbedNet_LastError = mbedNetResult_NotIplemented;
00400                     result = -1;
00401                     goto Exit;
00402                     
00403                 default:
00404                     DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Unknown socket protocol"));
00405                     mbedNet_LastError = mbedNetResult_InvalidParameter;
00406                     result = -1;
00407                     goto Exit;
00408             }
00409             break;
00410             
00411         default:
00412             DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Protocol family not supported"));
00413             mbedNet_LastError = mbedNetResult_NotIplemented;
00414             result = -1;
00415             goto Exit;
00416     }
00417     
00418     entry->dataQueue = CQueue_Alloc(SOCKET_DATAQUEUE_ENTRY_COUNT);
00419     
00420     if (entry == NULL)
00421     {
00422         if (entry->localAddr) free(entry->localAddr);
00423         if (entry->remoteAddr) free(entry->remoteAddr);
00424         DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Not enough memory to allocate data queue"));
00425         mbedNet_LastError = mbedNetResult_NotEnoughMemory;
00426         result = -1;
00427         goto Exit;
00428     }
00429         
00430     entry->state = State_Bound;
00431     
00432     result = 0;
00433     
00434 Exit:
00435     return result;
00436 }
00437 
00438 
00439 int32_t Sockets_Send(Socket_t socket, uint8_t *data, int32_t length, int32_t flags)
00440 {
00441     int32_t            count = -1;
00442     Socket_Entry_t    *entry;
00443 
00444     entry = GetSocketEntry(socket);
00445     if (entry == NULL) goto Exit;
00446 
00447     if (entry->protocol == SOCK_DGRAM)
00448     {
00449         mbedNet_LastError = mbedNetResult_DestinationAddressRequired;
00450         goto Exit;
00451     }
00452 
00453     mbedNet_LastError = mbedNetResult_NotIplemented;
00454 
00455 Exit:
00456     return count;
00457 }
00458 
00459 
00460 int32_t Sockets_SendTo(Socket_t socket, uint8_t *data, int32_t length, int32_t flags, const Socket_Addr_t *remoteAddr, int32_t addrLen)
00461 {
00462     int32_t            count = -1;
00463     Socket_Entry_t    *entry;
00464 
00465 
00466     entry = GetSocketEntry(socket);
00467     if (entry == NULL) 
00468     {
00469         DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("socket not found!"));
00470         goto Exit;
00471     }
00472 
00473     switch(entry->family)
00474     {
00475         case AF_INET:
00476             switch(entry->protocol)
00477             {
00478                 case SOCK_DGRAM:
00479                     if (addrLen != sizeof(Socket_AddrIn_t))
00480                     {
00481                         DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Invalid socket address length"));
00482                         mbedNet_LastError = mbedNetResult_InvalidParameter;
00483                         goto Exit;
00484                     }
00485                     count = SendToUDPv4(entry, data, length, (Socket_AddrIn_t *)remoteAddr);
00486                     break;
00487                 
00488                 default:
00489                     DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Protocol not implemented"));
00490                     mbedNet_LastError = mbedNetResult_NotIplemented;
00491                     goto Exit;
00492             }
00493             break;
00494             
00495         default:
00496             DEBUG_SOURCE(DEBUG_LEVEL_ERROR, ("Protocol family not implemented"));
00497             mbedNet_LastError = mbedNetResult_NotIplemented;
00498             goto Exit;
00499     }
00500 
00501 Exit:
00502     return count;
00503 }
00504 
00505 
00506 int32_t Sockets_Recv(Socket_t socket, uint8_t *data, int32_t length, int32_t flags)
00507 {
00508     int32_t                count = -1;
00509     Socket_Entry_t        *entry;
00510     
00511     entry = GetSocketEntry(socket);
00512     if (entry == NULL) goto Exit;
00513 
00514     if (entry->protocol == SOCK_DGRAM)
00515     {
00516         mbedNet_LastError = mbedNetResult_DestinationAddressRequired;
00517         goto Exit;
00518     }
00519 
00520     if (CQueue_IsEmpty(entry->dataQueue))
00521     {
00522         mbedNet_LastError = mbedNetResult_WouldBlock;
00523         goto Exit;
00524     }
00525 
00526     count = Recv_Data(entry, data, length);
00527         
00528 Exit:
00529     return count;
00530 }
00531 
00532 
00533 int32_t Sockets_RecvFrom(Socket_t socket, uint8_t *data, int32_t length, int32_t flags, Socket_Addr_t *remoteAddr, int32_t *addrLen)
00534 {
00535     int32_t            count = -1;
00536     Socket_Entry_t    *entry;
00537     
00538     entry = GetSocketEntry(socket);
00539     if (entry == NULL) goto Exit;
00540 
00541     if (CQueue_IsEmpty(entry->dataQueue))
00542     {
00543         mbedNet_LastError = mbedNetResult_WouldBlock;
00544         goto Exit;
00545     }
00546     
00547     if (remoteAddr != NULL)
00548     {
00549         if (entry->localAddr->len > *addrLen)
00550         {
00551             mbedNet_LastError = mbedNetResult_BufferTooSmall;
00552             goto Exit;
00553         }
00554         memcpy(remoteAddr, entry->remoteAddr, entry->remoteAddr->len);
00555     }
00556         
00557     count = Recv_Data(entry, data, length);
00558 
00559 Exit:
00560     return count;
00561 }
00562 
00563 
00564 int32_t Sockets_Close(Socket_t socket)
00565 {
00566     int32_t            result = -1;
00567     Socket_Entry_t    *entry;
00568     void            *ptr;
00569 
00570     if ((entry = GetSocketEntry(socket)) == NULL) goto Exit;
00571     
00572     entry->state = State_Close;
00573     if (entry->localAddr) free(entry->localAddr);
00574     entry->localAddr = NULL;
00575     if (entry->remoteAddr) free(entry->remoteAddr);
00576     entry->remoteAddr = NULL;
00577     /* Free pending data blocks */
00578     while(CQueue_Peek(entry->dataQueue, &ptr) != -1)
00579     {
00580         free(ptr);
00581     }
00582     CQueue_Free(entry->dataQueue);
00583     entry->dataQueue = NULL;
00584     result = 0;
00585     
00586 Exit:
00587     DEBUG_MODULE(DEBUG_LEVEL_INFO, ("closed socket %d", socket));
00588     return result;
00589 }
00590 
00591 
00592 
00593 static Socket_Entry_t *GetSocketEntry(Socket_t socket)
00594 {
00595     Socket_Entry_t    *entry = NULL;
00596 
00597     if ((socket < 0) || (socket >= SOCKET_MAX_COUNT))
00598     {
00599         DEBUG_MODULE(DEBUG_LEVEL_ERROR, ("Invalid socket handle"));
00600         mbedNet_LastError = mbedNetResult_InvalidSocketHandle;
00601         goto Exit;
00602     }
00603     entry = socketEntryTable + socket;
00604 
00605     if (entry->state == State_Close)
00606     {
00607         DEBUG_MODULE(DEBUG_LEVEL_WARNING, ("Socket already closed"));
00608         mbedNet_LastError = mbedNetResult_SocketAlreadyClosed;
00609         entry = NULL;
00610         goto Exit;
00611     }
00612 Exit:
00613     return entry;
00614 }