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

 #include <async.h>
 #include <atomic.h>
 #include <fibril.h>
 #include <fibril_sync.h>
+#include <stdint.h>
 #include <ipc/ipc.h>
 #include <ipc/services.h>
 #include <sys/types.h>
 /** Original datagram length in bytes transfered to the error notification message.
  */
 #define ICMP_KEEP_LENGTH    8
+/** Free identifier numbers pool start.
+ */
+#define ICMP_FREE_IDS_START 1
+/** Free identifier numbers pool end.
+ */
+#define ICMP_FREE_IDS_END   MAX_UINT16
 /** Computes the ICMP datagram checksum.
  *  @param header The ICMP datagram header. Input/output parameter.
  *  @param length The total datagram length. Input parameter.
  *  @returns The computed checksum.
  */
  *  @returns EOK on success.
  *  @returns EINVAL if the data parameter is NULL.
  */
 int icmp_timeout_for_reply( void * data );
+/** Assigns a new identifier for the connection.
+ *  Fills the echo data parameter with the assigned values.
+ *  @param echo_data The echo data to be bound. Input/output parameter.
+ *  @returns Index of the inserted echo data.
+ *  @returns EBADMEM if the echo_data parameter is NULL.
+ *  @returns ENOTCONN if no free identifier have been found.
+ */
+int icmp_bind_free_id( icmp_echo_ref echo_data );
 /** ICMP reply timeout data.
  *  Used as a timeouting fibril argument.
  *  @see icmp_timeout_for_reply()
  */
 struct icmp_reply_timeout{
  */
 icmp_globals_t  icmp_globals;
 INT_MAP_IMPLEMENT( icmp_replies, icmp_reply_t );
-GENERIC_FIELD_IMPLEMENT( icmp_echo_data, icmp_echo_t );
+INT_MAP_IMPLEMENT( icmp_echo_data, icmp_echo_t );
 int icmp_echo_msg( int icmp_phone, size_t size, mseconds_t timeout, ip_ttl_t ttl, ip_tos_t tos, int dont_fragment, const struct sockaddr * addr, socklen_t addrlen ){
     icmp_echo_ref   echo_data;
     int             res;
     fibril_rwlock_write_lock( & icmp_globals.lock );
     // use the phone as the echo data index
-    echo_data = icmp_echo_data_get_index( & icmp_globals.echo_data, icmp_phone );
+    echo_data = icmp_echo_data_find( & icmp_globals.echo_data, icmp_phone );
     if( ! echo_data ){
         res = ENOENT;
     }else{
         res = icmp_echo( echo_data->id, echo_data->sequence, size, timeout, ttl, tos, dont_fragment, addr, addrlen );
+        if( echo_data->sequence < MAX_UINT16 ){
-        ++ echo_data->sequence;
+            ++ echo_data->sequence;
+        }else{
+            echo_data->sequence = 0;
+        }
+    }
     fibril_rwlock_write_unlock( & icmp_globals.lock );
     return res;
+}
     return icmp_send_packet( ICMP_PARAMETERPROB, code, packet, header, SERVICE_ICMP );
+}
 icmp_header_ref icmp_prepare_packet( packet_t packet ){
     icmp_header_ref header;
-    int             header_length;
+    size_t          header_length;
     size_t          total_length;
     total_length = packet_get_data_length( packet );
     if( total_length <= 0 ) return NULL;
     header_length = ip_client_header_length( packet );
     if( header_length <= 0 ) return NULL;
     // truncate if longer than 64 bits (without the IP header)
-    if(( total_length - header_length > ICMP_KEEP_LENGTH )
+    if(( total_length > header_length + ICMP_KEEP_LENGTH )
     && ( packet_trim( packet, 0, total_length - header_length - ICMP_KEEP_LENGTH ) != EOK )){
         return NULL;
+    }
     header = PACKET_PREFIX( packet, icmp_header_t );
     if( ! header ) return NULL;
     return ip_send_msg( icmp_globals.ip_phone, -1, packet, SERVICE_ICMP, error );
+}
 int icmp_connect_module( services_t service ){
     icmp_echo_ref   echo_data;
+    icmp_param_t    id;
     int             index;
     echo_data = ( icmp_echo_ref ) malloc( sizeof( * echo_data ));
     if( ! echo_data ) return ENOMEM;
     // assign a new identifier
     fibril_rwlock_write_lock( & icmp_globals.lock );
-    ++ icmp_globals.last_used_id;
-    echo_data->id = icmp_globals.last_used_id;
-    echo_data->sequence = 0;
-    // remember the assigned echo data
-    index = icmp_echo_data_add( & icmp_globals.echo_data, echo_data );
+    index = icmp_bind_free_id( echo_data );
     if( index < 0 ){
         free( echo_data );
+    }else{
+        id = echo_data->id;
+    }
     fibril_rwlock_write_unlock( & icmp_globals.lock );
-    // return the echo data index as the ICMP phone
+    // return the echo data identifier as the ICMP phone
-    return index;
+    return id;
+}
 int icmp_initialize( async_client_conn_t client_connection ){
     ERROR_DECLARE;
     if( error ){
         switch( error ){
             case SERVICE_ICMP:
                 // process error
-                // TODO remove debug dump
-                // length = icmp_client_header_length( packet );
                 result = icmp_client_process_packet( packet, & type, & code, NULL, NULL );
                 if( result < 0 ) return result;
                 length = ( size_t ) result;
+                // TODO remove debug dump
                 printf( "ICMP error %d (%d) in packet %d\n", type, code, packet_get_id( packet ) );
+                // remove the error header
                 ERROR_PROPAGATE( packet_trim( packet, length, 0 ));
                 break;
             default:
                 return ENOTSUP;
+        }
+    }
     // get rid of the ip header
-    result = ip_client_process_packet( packet, NULL, NULL, NULL, NULL, NULL );
+    length = ip_client_header_length( packet );
-    if( result < 0 ) return result;
-    ERROR_PROPAGATE( packet_trim( packet, ( size_t ) result, 0 ));
+    ERROR_PROPAGATE( packet_trim( packet, length, 0 ));
     length = packet_get_data_length( packet );
     if( length <= 0 ) return EINVAL;
     if( length < sizeof( icmp_header_t )) return EINVAL;
     data = packet_get_data( packet );
     if( ! data ) return EINVAL;
     // get icmp header
     header = ( icmp_header_ref ) data;
     // checksum
-/*  if(( header->checksum ) && ( ICMP_CHECKSUM( header, length ))){
-        // set the original message type on error notification
-        // type swap observed in Qemu
-        if( error ){
-            switch( header->type ){
-                case ICMP_ECHOREPLY:
-                    header->type = ICMP_ECHO;
-                    break;
-            }
-        }
-        if( ICMP_CHECKSUM( header, length )){
-            return EINVAL;
-        }
-    }
-*/  if( header->checksum ){
+    if( header->checksum ){
         while( ICMP_CHECKSUM( header, length )){
             // set the original message type on error notification
             // type swap observed in Qemu
             if( error ){
                 switch( header->type ){
     fibril_rwlock_t         lock;
     ipc_call_t              answer;
     int                     answer_count;
     size_t                  length;
     struct sockaddr *       addr;
-    icmp_param_t            id;
-    icmp_param_t            sequence = 0;
     ipc_callid_t            data_callid;
+    icmp_echo_ref           echo_data;
     /*
      * Accept the connection
      *  - Answer the first NET_ICMP_INIT call.
      */
     ipc_answer_0( callid, EOK );
     fibril_rwlock_initialize( & lock );
+    echo_data = ( icmp_echo_ref ) malloc( sizeof( * echo_data ));
+    if( ! echo_data ) return ENOMEM;
     // assign a new identifier
     fibril_rwlock_write_lock( & icmp_globals.lock );
-    ++ icmp_globals.last_used_id;
-    id = icmp_globals.last_used_id;
+    ERROR_CODE = icmp_bind_free_id( echo_data );
     fibril_rwlock_write_unlock( & icmp_globals.lock );
+    if( ERROR_CODE < 0 ){
+        free( echo_data );
+        return ERROR_CODE;
+    }
     while( keep_on_going ){
         refresh_answer( & answer, & answer_count );
         callid = async_get_call( & call );
                     if( ! addr ){
                         ERROR_CODE = ENOMEM;
                     }else{
                         if( ! ERROR_OCCURRED( ipc_data_write_finalize( data_callid, addr, length ))){
                             fibril_rwlock_write_lock( & icmp_globals.lock );
-                            ERROR_CODE = icmp_echo( id, sequence, ICMP_GET_SIZE( call ), ICMP_GET_TIMEOUT( call ), ICMP_GET_TTL( call ), ICMP_GET_TOS( call ), ICMP_GET_DONT_FRAGMENT( call ), addr, ( socklen_t ) length );
+                            ERROR_CODE = icmp_echo( echo_data->id, echo_data->sequence, ICMP_GET_SIZE( call ), ICMP_GET_TIMEOUT( call ), ICMP_GET_TTL( call ), ICMP_GET_TOS( call ), ICMP_GET_DONT_FRAGMENT( call ), addr, ( socklen_t ) length );
                             fibril_rwlock_write_unlock( & icmp_globals.lock );
                             free( addr );
+                            if( echo_data->sequence < MAX_UINT16 ){
-                            ++ sequence;
+                                ++ echo_data->sequence;
+                            }else{
+                                echo_data->sequence = 0;
+                            }
+                        }
+                    }
+                }
                 fibril_rwlock_write_unlock( & lock );
                 break;
+        }
         answer_call( callid, ERROR_CODE, & answer, answer_count );
+    }
+    // release the identifier
+    fibril_rwlock_write_lock( & icmp_globals.lock );
+    icmp_echo_data_exclude( & icmp_globals.echo_data, echo_data->id );
+    fibril_rwlock_write_unlock( & icmp_globals.lock );
     return EOK;
+}
 int icmp_process_message( ipc_call_t * call ){
     ERROR_DECLARE;
 int icmp_release_and_return( packet_t packet, int result ){
     pq_release( icmp_globals.net_phone, packet_get_id( packet ));
     return result;
+}
+int icmp_bind_free_id( icmp_echo_ref echo_data ){
+    icmp_param_t    index;
+    if( ! echo_data ) return EBADMEM;
+    // from the last used one
+    index = icmp_globals.last_used_id;
+    do{
+        ++ index;
+        // til the range end
+        if( index >= ICMP_FREE_IDS_END ){
+            // start from the range beginning
+            index = ICMP_FREE_IDS_START - 1;
+            do{
+                ++ index;
+                // til the last used one
+                if( index >= icmp_globals.last_used_id ){
+                    // none found
+                    return ENOTCONN;
+                }
+            }while( icmp_echo_data_find( & icmp_globals.echo_data, index ) != NULL );
+            // found, break immediately
+            break;
+        }
+    }while( icmp_echo_data_find( & icmp_globals.echo_data, index ) != NULL );
+    echo_data->id = index;
+    echo_data->sequence = 0;
+    return icmp_echo_data_add( & icmp_globals.echo_data, index, echo_data );
+}
 /** @}
  */

Subversion Repositories HelenOS

(root)/branches/network/uspace/srv/net/tl/icmp/icmp.c – Rev 4720 → 4721