WebSVN – HelenOS – Diff – /branches/network/uspace/srv/net/tl/udp/udp.c

 #include <ipc/services.h>
 #include "../../err.h"
 #include "../../messages.h"
 #include "../../modules.h"
+#include "../../structures/dynamic_fifo.h"
 #include "../../structures/packet/packet_client.h"
 #include "../../include/in.h"
 #include "../../include/inet.h"
 #include "../../include/ip_client.h"
 #include "../../include/ip_interface.h"
 #include "../../include/ip_protocols.h"
 #include "../../include/socket.h"
 #include "../../include/socket_errno.h"
-//#include "../../include/udp_interface.h"
 #include "../../socket/socket_core.h"
 #include "../../socket/socket_messages.h"
 #include "../tl_messages.h"
 #include "udp.h"
 #include "udp_header.h"
 #include "udp_module.h"
+#define MAX_UDP_FRAGMENT_SIZE   65535
 int udp_received_msg( device_id_t device_id, packet_t packet, services_t receiver );
+int udp_process_packet( packet_t packet );
 int process_client_messages( ipc_callid_t callid, ipc_call_t * call, ipc_call_t * answer, int * answer_count );
-int udp_sendto_message( socket_cores_ref local_sockets, int socket_id, void * addr, size_t addrlen, void * data, size_t length, int flags );
+int udp_sendto_message( socket_cores_ref local_sockets, int socket_id, void * addr, size_t addrlen, int fragments, int flags );
+int udp_recvfrom_message( socket_cores_ref local_sockets, int socket_id, int flags );
-int socket_get_data( void ** data, size_t * length );
+int socket_read_data( void ** data, size_t * length );
+int socket_read_packet_data( packet_ref packet, size_t prefix, struct sockaddr_in * address_in );
+int socket_write_data( void * data, size_t data_length );
 udp_globals_t   udp_globals;
 /** Initializes the module.
  */
 int udp_initialize( async_client_conn_t client_connection ){
     ERROR_DECLARE;
-    udp_globals.port_search_start = 1025;
     udp_globals.ip_phone = ip_bind_service( SERVICE_IP, IPPROTO_UDP, SERVICE_UDP, client_connection, udp_received_msg );
     if( udp_globals.ip_phone < 0 ){
         return udp_globals.ip_phone;
+    }
     ERROR_PROPAGATE( ip_packet_size_req( udp_globals.ip_phone, -1, & udp_globals.addr_len, & udp_globals.prefix, & udp_globals.content, & udp_globals.suffix ));
     udp_globals.content -= sizeof( udp_header_t );
     return EOK;
+}
 int udp_received_msg( device_id_t device_id, packet_t packet, services_t receiver ){
+    ERROR_DECLARE;
+    if( ERROR_OCCURRED( udp_process_packet( packet ))){
+        pq_release( udp_globals.net_phone, packet_get_id( packet ));
+        return ERROR_CODE;
+    }
+    return EOK;
+}
+int udp_process_packet( packet_t packet ){
+    ERROR_DECLARE;
+    uint8_t *       src;
+    uint8_t *       dest;
+    int             length;
+    void *          data;
+    udp_header_ref  header;
+    socket_core_ref *   socket;
+    packet_t        next_packet;
+    int             total_length;
+//  uint16_t        checksum;
+    int             fragments;
+    packet_t        tmp_packet;
+    // get packet data
+    length = packet_get_addr( packet, & src, & dest );
+    if( length < 0 ) return length;
+    if( length != sizeof( in_addr_t )) return EINVAL;
-    // TODO received
+    // TODO received ipopts?
+    ERROR_PROPAGATE( ip_client_process_packet( packet, NULL, NULL, NULL, NULL, NULL ));
     //  TODO remove debug dump:
-    uint8_t *   data;
+/*  uint8_t *   rdata;
+    rdata = packet_get_data( packet );
+    printf( "Receiving udp packet:\n\tid\t= %d\n\tlength\t= %d\n\tdata\t= %.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX\n\t\t%.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX\n", packet_get_id( packet ), packet_get_data_length( packet ), rdata[ 0 ], rdata[ 1 ], rdata[ 2 ], rdata[ 3 ], rdata[ 4 ], rdata[ 5 ], rdata[ 6 ], rdata[ 7 ], rdata[ 8 ], rdata[ 9 ], rdata[ 10 ], rdata[ 11 ], rdata[ 12 ], rdata[ 13 ], rdata[ 14 ], rdata[ 15 ], rdata[ 16 ], rdata[ 17 ], rdata[ 18 ], rdata[ 19 ], rdata[ 20 ], rdata[ 21 ], rdata[ 22 ], rdata[ 23 ], rdata[ 24 ], rdata[ 25 ], rdata[ 26 ], rdata[ 27 ], rdata[ 28 ], rdata[ 29 ], rdata[ 30 ], rdata[ 31 ], rdata[ 32 ], rdata[ 33 ], rdata[ 34 ], rdata[ 35 ], rdata[ 36 ], rdata[ 37 ], rdata[ 38 ], rdata[ 39 ], rdata[ 40 ], rdata[ 41 ], rdata[ 42 ], rdata[ 43 ], rdata[ 44 ], rdata[ 45 ], rdata[ 46 ], rdata[ 47 ], rdata[ 48 ], rdata[ 49 ], rdata[ 50 ], rdata[ 51 ], rdata[ 52 ], rdata[ 53 ], rdata[ 54 ], rdata[ 55 ], rdata[ 56 ], rdata[ 57 ], rdata[ 58 ], rdata[ 59 ] );
+*/
+    length = packet_get_data_length( packet );
+    if( length < 0 ) return length;
+    if( length < sizeof( udp_header_t )) return NO_DATA;
     data = packet_get_data( packet );
+    if( ! data ) return NO_DATA;
+    // get udp header
+    header = ( udp_header_ref ) data;
+    // find the destination socket
+    socket = socket_ports_find( & udp_globals.sockets, ntohs( header->dest ));
+    if( ! socket ) return EADDRNOTAVAIL;
+    // count the received packet fragments
+    next_packet = packet;
+    fragments = 0;
+    total_length = ntohs( header->len );
+    do{
+        ++ fragments;
-    printf( "Receiving packet:\n\tid\t= %d\n\tlength\t= %d\n\tdata\t= %.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX\n\t\t%.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX:%.2hhX %.2hhX %.2hhX %.2hhX\n", packet_get_id( packet ), packet_get_data_length( packet ), data[ 0 ], data[ 1 ], data[ 2 ], data[ 3 ], data[ 4 ], data[ 5 ], data[ 6 ], data[ 7 ], data[ 8 ], data[ 9 ], data[ 10 ], data[ 11 ], data[ 12 ], data[ 13 ], data[ 14 ], data[ 15 ], data[ 16 ], data[ 17 ], data[ 18 ], data[ 19 ], data[ 20 ], data[ 21 ], data[ 22 ], data[ 23 ], data[ 24 ], data[ 25 ], data[ 26 ], data[ 27 ], data[ 28 ], data[ 29 ], data[ 30 ], data[ 31 ], data[ 32 ], data[ 33 ], data[ 34 ], data[ 35 ], data[ 36 ], data[ 37 ], data[ 38 ], data[ 39 ], data[ 40 ], data[ 41 ], data[ 42 ], data[ 43 ], data[ 44 ], data[ 45 ], data[ 46 ], data[ 47 ], data[ 48 ], data[ 49 ], data[ 50 ], data[ 51 ], data[ 52 ], data[ 53 ], data[ 54 ], data[ 55 ], data[ 56 ], data[ 57 ], data[ 58 ], data[ 59 ] );
+        length = packet_get_data_length( packet );
+        if( length < 0 ) return length;
+        if( ! length ) return NO_DATA;
+        if( total_length < length ){
+            // cut of the suffix if too long
+            ERROR_PROPAGATE( packet_trim( next_packet, 0, length - total_length ));
+            // relese the rest of the packet fragments
+            tmp_packet = pq_next( next_packet );
+            while( tmp_packet ){
+                next_packet = pq_detach( tmp_packet );
-    pq_release( udp_globals.net_phone, packet_get_id( packet ));
+                pq_release( udp_globals.net_phone, packet_get_id( tmp_packet ));
+                tmp_packet = next_packet;
+            }
+            break;
+        }
+        total_length -= length;
+    /*  if( header->header_checksum ){
+        }
+    */
+    }while(( next_packet = pq_next( next_packet )) && ( total_length > 0 ));
+    // queue the received packet
+    ERROR_PROPAGATE( dyn_fifo_push( &( ** socket ).received, packet_get_id( packet ), SOCKET_MAX_RECEIVED_SIZE ));
+    // notify the destination socket
+    async_msg_2(( ** socket ).phone, NET_SOCKET_RECEIVED, ( ** socket ).socket_id, fragments );
     return EOK;
+}
 int udp_message( ipc_callid_t callid, ipc_call_t * call, ipc_call_t * answer, int * answer_count ){
     ERROR_DECLARE;
     bool                    keep_on_going = true;
     socket_cores_t          local_sockets;
     int                     app_phone = IPC_GET_PHONE( call );
     void *                  addr;
     size_t                  addrlen;
-    void *                  data;
-    size_t                  length;
     /*
      * Accept the connection
      *  - Answer the first IPC_M_CONNECT_ME_TO call.
      */
         IPC_SET_ARG3( * answer, 0 );
         IPC_SET_ARG4( * answer, 0 );
         IPC_SET_ARG5( * answer, 0 );
         callid = async_get_call( call );
-        printf( "message %d\n", IPC_GET_METHOD( * call ));
+//      printf( "message %d\n", IPC_GET_METHOD( * call ));
         switch( IPC_GET_METHOD( * call )){
             case IPC_M_PHONE_HUNGUP:
                 keep_on_going = false;
                 res = EOK;
                 break;
             case NET_SOCKET:
-                res = socket_create( & local_sockets, app_phone );
+                res = socket_create( & local_sockets, app_phone, SOCKET_SET_SOCKET_ID( answer ));
+                * SOCKET_SET_HEADER_SIZE( answer ) = sizeof( udp_header_t );
+                * SOCKET_SET_DATA_FRAGMENT_SIZE( answer ) = MAX_UDP_FRAGMENT_SIZE;
+                * answer_count = 3;
                 break;
             case NET_SOCKET_BIND:
-                if( ERROR_OCCURRED( socket_get_data( & addr, & addrlen ))){
+                if( ERROR_OCCURRED( socket_read_data( & addr, & addrlen ))){
                     res = ERROR_CODE;
                     break;
+                }
                 res = socket_bind( & local_sockets, & udp_globals.sockets, SOCKET_GET_SOCKET_ID( call ), addr, addrlen );
                 free( addr );
                 break;
             case NET_SOCKET_SENDTO:
-                if( ERROR_OCCURRED( socket_get_data( & addr, & addrlen ))){
+                if( ERROR_OCCURRED( socket_read_data( & addr, & addrlen ))){
                     res = ERROR_CODE;
                     break;
+                }
-                if( ERROR_OCCURRED( socket_get_data( & data, & length ))){
-                    free( addr );
-                    res = ERROR_CODE;
-                    break;
-                }
-                res = udp_sendto_message( & local_sockets, SOCKET_GET_SOCKET_ID( call ), addr, addrlen, data, length, SOCKET_GET_FLAGS( call ));
+                res = udp_sendto_message( & local_sockets, SOCKET_GET_SOCKET_ID( call ), addr, addrlen, SOCKET_GET_DATA_FRAGMENTS( call ), SOCKET_GET_FLAGS( call ));
                 free( addr );
-                free( data );
                 break;
             case NET_SOCKET_RECVFROM:
+                res = udp_recvfrom_message( & local_sockets, SOCKET_GET_SOCKET_ID( call ), SOCKET_GET_FLAGS( call ));
+                if( res > 0 ){
-                // TODO read first received packet queue data continuesly
+                    * SOCKET_SET_READ_DATA_LENGTH( answer ) = res;
+                    * answer_count = 1;
-                res = ENOTSUP;
+                    res = EOK;
+                }
                 break;
             case NET_SOCKET_CLOSE:
                 res = socket_destroy( udp_globals.net_phone, SOCKET_GET_SOCKET_ID( call ), & local_sockets, & udp_globals.sockets );
                 break;
             case NET_SOCKET_GETSOCKOPT:
             default:
                 res = ENOTSUP;
                 break;
+        }
-        // TODO debug
-        printf( "res = %d\n", res );
+//      printf( "res = %d\n", res );
         switch( * answer_count ){
             case 0:     ipc_answer_0( callid, res );
                     continue;
             case 1:     ipc_answer_1( callid, res, IPC_GET_ARG1( * answer ));
     socket_cores_destroy( & local_sockets );
     return EOK;
+}
-int udp_sendto_message( socket_cores_ref local_sockets, int socket_id, void * addr, size_t addrlen, void * data, size_t length, int flags ){
+int udp_sendto_message( socket_cores_ref local_sockets, int socket_id, void * addr, size_t addrlen, int fragments, int flags ){
     ERROR_DECLARE;
     socket_core_ref         socket;
     struct sockaddr *       address;
     struct sockaddr_in *    address_in;
     packet_t                packet;
+    packet_t                next_packet;
     udp_header_ref          header;
+    int                     index;
+    int                     total_length;
+    int                     length;
     if( addrlen < sizeof( struct sockaddr )) return EINVAL;
     address = ( struct sockaddr * ) addr;
     switch( address->sa_family ){
         case AF_INET:
             if( addrlen != sizeof( struct sockaddr_in )) return EINVAL;
             address_in = ( struct sockaddr_in * ) addr;
             socket = socket_cores_find( local_sockets, socket_id );
             if( ! socket ) return ENOTSOCK;
-/*          if( socket->port < 0 ){
-                return ENOTCONN;
-                socket->port = udp_globals.port_search_start;
-                while( socket_ports_find( global_sockets, socket->port ) >= 0 ){
-                    socket_port = ( socket->port + 1 ) % ( 65535 - 1025 ) + 1025;
-                }
-                ERROR_PROPAGATE( socket_ports_add( global_sockets, socket->port );
-                udp_globals.port_search_start = socket->port + 1;
-            }
-*/          // TODO create and send fragmented packets
             // TODO do not ask all the time
             ERROR_PROPAGATE( ip_packet_size_req( udp_globals.ip_phone, -1, & udp_globals.addr_len, & udp_globals.prefix, & udp_globals.content, & udp_globals.suffix ));
-            packet = packet_get_4( udp_globals.net_phone, length, udp_globals.addr_len, udp_globals.prefix, udp_globals.suffix );
-            if( ! packet ) return ENOMEM;
+            // read the first packet fragment
-            if( ERROR_OCCURRED( packet_copy_data( packet, data, length ))
+            total_length = socket_read_packet_data( & packet, sizeof( udp_header_t ), address_in );
-            || ERROR_OCCURRED( packet_set_addr( packet, NULL, ( uint8_t * ) & address_in->sin_addr.s_addr, sizeof( address_in->sin_addr.s_addr )))){
-                pq_release( udp_globals.net_phone, packet_get_id( packet ));
+            if( total_length < 0 ) return total_length;
-                return ERROR_CODE;
+            // prefix the udp header
-            }
             header = PACKET_PREFIX( packet, udp_header_t );
             if( ! header ){
                 pq_release( udp_globals.net_phone, packet_get_id( packet ));
                 return ENOMEM;
+            }
+            // read the rest of the packet fragments
+            for( index = 1; index < fragments; ++ index ){
+                length = socket_read_packet_data( & next_packet, 0, address_in );
+                if( length < 0 ){
+                    pq_release( udp_globals.net_phone, packet_get_id( packet ));
+                    return length;
+                }
+                packet = pq_add( packet, next_packet, index, 0 );
+                total_length += length;
+            }
+            // set the udp header
             header->source = ( socket->port < 0 ) ? 0 : htons( socket->port );
             header->dest = htons( address_in->sin_port );
-            header->len = htons( length );
+            header->len = htons( total_length + sizeof( udp_header_t ));
             // TODO my ip address for the pseudo header checksum
             header->check = 0;
+            // prepare the first packet fragment
             if( ERROR_OCCURRED( ip_client_prepare_packet( packet, IPPROTO_UDP, 0, 0, 0, 0 ))){
                 pq_release( udp_globals.net_phone, packet_get_id( packet ));
                 return ERROR_CODE;
+            }
+            // send the packet
             return ip_send_msg( udp_globals.ip_phone, socket->device_id, packet, SERVICE_UDP );
-            break;
         // TODO IPv6
         default:
             return EAFNOSUPPORT;
+    }
     return EOK;
+}
+int udp_recvfrom_message( socket_cores_ref local_sockets, int socket_id, int flags ){
+    ERROR_DECLARE;
+    socket_core_ref socket;
+    int             packet_id;
+    packet_t        packet;
+    udp_header_ref  header;
+    struct sockaddr_in      address;
+    int             length;
+    packet_t        next_packet;
+    void *          data;
+    int             fragments;
+    int *           lengths;
+    int             index;
+    uint8_t *       addr;
+    // find the socket
+    socket = socket_cores_find( local_sockets, socket_id );
+    if( ! socket ) return ENOTSOCK;
+    // get the next received packet
+    packet_id = dyn_fifo_value( & socket->received );
+    if( packet_id < 0 ) return packet_id;
+    ERROR_PROPAGATE( packet_translate( udp_globals.net_phone, & packet, packet_id ));
+    // get udp header
+    data = packet_get_data( packet );
+    if( ! data ){
+        pq_release( udp_globals.net_phone, packet_id );
+        return NO_DATA;
+    }
+    header = ( udp_header_ref ) data;
+    // set the source address
+    address.sin_family = PF_INET;
+    address.sin_port = ntohs( header->dest );
+    length = packet_get_addr( packet, & addr, NULL );
+    if( length != sizeof( address.sin_addr.s_addr )){
+        pq_release( udp_globals.net_phone, packet_id );
+        return EINVAL;
+    }
+    address.sin_addr.s_addr = *(( uint32_t * ) addr );
+    bzero( & address.sin_zero, sizeof( address.sin_zero ));
+    // send the source address
+    ERROR_PROPAGATE( socket_write_data( & address, sizeof( address )));
+    next_packet = pq_next( packet );
+    if( ! next_packet ){
+        // write all if only one fragment
+        ERROR_PROPAGATE( socket_write_data( data + sizeof( udp_header_t ), packet_get_data_length( packet ) - sizeof( udp_header_t )));
+        // store the total length
+        length = packet_get_data_length( packet ) - sizeof( udp_header_t );
+    }else{
+        // count the packet fragments
+        fragments = 1;
+        next_packet = pq_next( packet );
+        while(( next_packet = pq_next( next_packet ))){
+            ++ fragments;
+        }
+        // compute and store the fragment lengths
+        lengths = ( int * ) malloc( sizeof( int ) * ( fragments + 1 ));
+        if( ! lengths ) return ENOMEM;
+        lengths[ 0 ] = packet_get_data_length( packet ) - sizeof( udp_header_t );
+        lengths[ fragments ] = lengths[ 0 ];
+        next_packet = pq_next( packet );
+        for( index = 1; index < fragments; ++ index ){
+            lengths[ index ] = packet_get_data_length( next_packet );
+            lengths[ fragments ] += lengths[ index ];
+            next_packet = pq_next( packet );
+        }while( next_packet );
+        // write the fragment lengths
+        ERROR_PROPAGATE( socket_write_data( lengths, sizeof( int ) * ( fragments + 1 )));
+        // write the first fragment
+        ERROR_PROPAGATE( socket_write_data( data + sizeof( udp_header_t ), lengths[ 0 ] ));
+        next_packet = pq_next( packet );
+        // write the rest of the fragments
+        for( index = 1; index < fragments; ++ index ){
+            ERROR_PROPAGATE( socket_write_data( packet_get_data( next_packet ), lengths[ index ] ));
+            next_packet = pq_next( packet );
+        }while( next_packet );
+        // store the total length
+        length = lengths[ fragments ];
+        free( lengths );
+    }
+    // release the packet
+    dyn_fifo_pop( & socket->received );
+    pq_release( udp_globals.net_phone, packet_get_id( packet ));
+    // return the total length
+    return length;
+}
+int socket_write_data( void * data, size_t data_length ){
+    size_t          length;
+    ipc_callid_t    callid;
+    if(( ! ipc_data_read_receive( & callid, & length ))
+    || ( length < data_length )){
+        return EINVAL;
+    }
+    return ipc_data_read_finalize( callid, data, data_length );
+}
-int socket_get_data( void ** data, size_t * length ){
+int socket_read_data( void ** data, size_t * length ){
     ERROR_DECLARE;
     ipc_callid_t    callid;
     if( !( data && length )) return EBADMEM;
         return ERROR_CODE;
+    }
     return EOK;
+}
+int socket_read_packet_data( packet_ref packet, size_t prefix, struct sockaddr_in * address_in ){
+    ERROR_DECLARE;
+    ipc_callid_t    callid;
+    size_t          length;
+    void *          data;
+    // get the data length
+    if( ! ipc_data_write_receive( & callid, & length )) return EINVAL;
+    // get a new packet
+    * packet = packet_get_4( udp_globals.net_phone, length, udp_globals.addr_len, prefix + udp_globals.prefix, udp_globals.suffix );
+    if( ! packet ) return ENOMEM;
+    // allocate space in the packet
+    data = packet_suffix( * packet, length );
+    if( ! data ){
+        pq_release( udp_globals.net_phone, packet_get_id( * packet ));
+        return ENOMEM;
+    }
+    // read the data into the packet
+    if( ERROR_OCCURRED( ipc_data_write_finalize( callid, data, length ))
+    // set the packet destination address
+    || ERROR_OCCURRED( packet_set_addr( * packet, NULL, ( uint8_t * ) & address_in->sin_addr.s_addr, sizeof( address_in->sin_addr.s_addr )))){
+        pq_release( udp_globals.net_phone, packet_get_id( * packet ));
+        return ERROR_CODE;
+    }
+    return length;
+}
 /** @}
  */

Subversion Repositories HelenOS

(root)/branches/network/uspace/srv/net/tl/udp/udp.c @ 4756 – Rev 4579 → 4589