WiP (add path to peer algorithm added)
[oweals/gnunet.git] / src / mesh / gnunet-service-mesh.c
index 760545021359861967c87617df5a792870927cab..df4f39eedb11e99d0eefb58212093ed010a06136 100644 (file)
  * @author Bartlomiej Polot
  *
  * STRUCTURE:
- * - MESH NETWORK MESSAGES
  * - DATA STRUCTURES
  * - GLOBAL VARIABLES
+ * - MESH NETWORK HANDLER HELPERS
  * - MESH NETWORK HANDLES
+ * - MESH LOCAL HANDLER HELPERS
  * - MESH LOCAL HANDLES
+ * - PERIODIC FUNCTIONS
  * - MAIN FUNCTIONS (main & run)
  * 
  * TODO:
- * - soft stateing (keep-alive (CHANGE?) / timeout / disconnect) -- not a message issue
  * - error reporting (CREATE/CHANGE/ADD/DEL?) -- new message!
  * - partial disconnect reporting -- same as error reporting?
  * - add vs create? change vs. keep-alive? same msg or different ones? -- thinking...
  * - speed requirement specification (change?) in mesh API -- API call
+ * - add ping message (connection confirmation, others?)
  */
 
 #include "platform.h"
 #include "gnunet_protocols.h"
 
 #include "mesh.h"
+#include "mesh_protocol.h"
 #include "gnunet_dht_service.h"
 
-/******************************************************************************/
-/********************      MESH NETWORK MESSAGES     **************************/
-/******************************************************************************/
-
-/**
- * Message for mesh path management
- */
-struct GNUNET_MESH_ManipulatePath
-{
-    /**
-     * Type: GNUNET_MESSAGE_TYPE_MESH_PATH_[CREATE|CHANGE|ADD|DEL]
-     *
-     * Size: sizeof(struct GNUNET_MESH_ManipulatePath) +
-     *       path_length * sizeof (struct GNUNET_PeerIdentity)
-     */
-    struct GNUNET_MessageHeader header;
-
-    /**
-     * Global id of the tunnel this path belongs to,
-     * unique in conjunction with the origin.
-     */
-    uint32_t tid GNUNET_PACKED;
-
-    /**
-     * Information about speed requirements.  If the tunnel cannot sustain the 
-     * minimum bandwidth, packets are to be dropped.
-     */
-    uint32_t speed_min GNUNET_PACKED;
-
-    /**
-     * 64-bit alignment.
-     */
-    uint32_t reserved GNUNET_PACKED;
-
-    /**
-     * path_length structs defining the *whole* path from the origin [0] to the
-     * final destination [path_length-1].
-     */
-    /* struct GNUNET_PeerIdentity peers[path_length]; */
-};
-
-/**
- * Message for mesh data traffic to all tunnel targets.
- */
-struct GNUNET_MESH_OriginMulticast
-{
-    /**
-     * Type: GNUNET_MESSAGE_TYPE_DATA_MULTICAST
-     */
-    struct GNUNET_MessageHeader header;
-
-    /**
-     * TID of the tunnel
-     */
-    uint32_t tid GNUNET_PACKED;
-
-    /**
-     * OID of the tunnel
-     */
-    struct GNUNET_PeerIdentity oid;
+#define REFRESH_PATH_TIME GNUNET_TIME_relative_multiply(\
+                                    GNUNET_TIME_UNIT_SECONDS,\
+                                    300)
 
-    /**
-     * Payload follows
-     */
-};
 
+/******************************************************************************/
+/************************      DATA STRUCTURES     ****************************/
+/******************************************************************************/
 
 /**
- * Message for mesh data traffic to a particular destination from origin.
+ * Information regarding a path
  */
-struct GNUNET_MESH_DataMessageFromOrigin
+struct MeshPath
 {
-    /**
-     * Type: GNUNET_MESSAGE_TYPE_DATA_MESSAGE_FROM_ORIGIN
-     */
-    struct GNUNET_MessageHeader header;
 
     /**
-     * TID of the tunnel
+     * Linked list
      */
-    uint32_t tid GNUNET_PACKED;
+    struct MeshPath             *next;
+    struct MeshPath             *prev;
 
     /**
-     * OID of the tunnel
+     * Whether the path is serving traffic in a tunnel or is a backup
      */
-    struct GNUNET_PeerIdentity oid;
+    int                         in_use;
 
     /**
-     * Destination.
+     * List of all the peers that form the path from origin to target
      */
-    struct GNUNET_PeerIdentity destination;
+    GNUNET_PEER_Id              *peers;
 
     /**
-     * Payload follows
+     * Number of peers (hops) in the path
      */
+    unsigned int                length;
 };
 
 
 /**
- * Message for mesh data traffic from a tunnel participant to origin.
- */
-struct GNUNET_MESH_DataMessageToOrigin
-{
-    /**
-     * Type: GNUNET_MESSAGE_TYPE_DATA_MESSAGE_TO_ORIGIN
-     */
-    struct GNUNET_MessageHeader header;
-
-    /**
-     * TID of the tunnel
-     */
-    uint32_t tid GNUNET_PACKED;
-
-    /**
-     * OID of the tunnel
-     */
-    struct GNUNET_PeerIdentity oid;
-
-    /**
-     * Sender of the message.
-     */
-    struct GNUNET_PeerIdentity sender;
-
-    /**
-     * Payload follows
-     */
-};
-
-/**
- * Message for mesh flow control
+ * All the states a peer participating in a tunnel can be in.
  */
-struct GNUNET_MESH_SpeedNotify
+enum MeshPeerState
 {
     /**
-     * Type: GNUNET_MESSAGE_TYPE_DATA_SPEED_NOTIFY
-     */
-    struct GNUNET_MessageHeader header;
-
-    /**
-     * TID of the tunnel
-     */
-    uint32_t tid GNUNET_PACKED;
-
-    /**
-     * OID of the tunnel
+     * Path to the peer not known yet
      */
-    struct GNUNET_PeerIdentity oid;
+    MESH_PEER_SEARCHING,
 
-    /**
-     * Slowest link down the path (above minimum speed requirement).
-     */
-    uint32_t speed_min;
-
-};
-
-/******************************************************************************/
-/************************      DATA STRUCTURES     ****************************/
-/******************************************************************************/
-
-/**
- * All the states a peer participating in a tunnel can be in.
- */
-enum PeerState
-{
     /**
      * Request sent, not yet answered.
      */
@@ -226,21 +113,15 @@ enum PeerState
     /**
      * Peer connected previosly but not responding
      */
-    MESH_PEER_RECONNECTING,
-
+    MESH_PEER_RECONNECTING
 };
 
+
 /**
  * Struct containing all information regarding a given peer
  */
-struct PeerInfo
+struct MeshPeerInfo
 {
-    /**
-     * Double linked list
-     */
-    struct PeerInfo             *next;
-    struct PeerInfo             *prev;
-
     /**
      * ID of the peer
      */
@@ -249,116 +130,105 @@ struct PeerInfo
     /**
      * Is the peer reachable? Is the peer even connected?
      */
-    enum PeerState              state;
+    enum MeshPeerState          state;
+
+    /**
+     * Last time we heard from this peer
+     */
+    struct GNUNET_TIME_Absolute last_contact;
 
     /**
-     * When to try to establish contact again?
+     * Number of attempts to reconnect so far
      */
-    struct GNUNET_TIME_Absolute next_reconnect_attempt;
+    int                         n_reconnect_attempts;
 
     /**
-     * Who to send the data to --- FIXME what about multiple (alternate) paths?
+     * Paths to reach the peer
      */
-    GNUNET_PEER_Id              first_hop;
+    struct MeshPath             *path;
+    struct MeshPath             *path_tail;
 
     /**
-     * Max data rate to this peer
+     * Handle to stop the DHT search for a path to this peer
      */
-    uint32_t                    max_speed;
+    struct GNUNET_DHT_GetHandle *dhtget;
 };
 
 
-typedef uint32_t MESH_PathID;
 /**
- * Information regarding a path
+ * Data scheduled to transmit (to local client or remote peer)
  */
-struct Path
+struct MeshQueue
 {
     /**
      * Double linked list
      */
-    struct Path                 *next;
-    struct Path                 *prev;
-
-    /**
-     * Id of the path, in case it's needed
-     */
-    MESH_PathID                 id;
-
-    /**
-     * Whether the path is serving traffic in a tunnel or is a backup
-     */
-    int                         in_use;
+    struct MeshQueue            *next;
+    struct MeshQueue            *prev;
 
     /**
-     * List of all the peers that form the path from origin to target
+     * Target of the data (NULL if target is client)
      */
-    GNUNET_PEER_Id              *peers;
-};
+    struct MeshPeerInfo         *peer;
 
-struct MESH_queue
-{
     /**
-     * Double linked list
+     * Client to send the data to (NULL if target is peer)
      */
-    struct MESH_queue          *next;
-    struct MESH_queue          *prev;
+    struct MeshClient           *client;
 
     /**
      * Size of the message to transmit
      */
     unsigned int                size;
-    
+
     /**
      * How old is the data?
      */
     struct GNUNET_TIME_Absolute timestamp;
-    
+
     /**
      * Data itself
      */
     struct GNUNET_MessageHeader *data;
 };
 
-
-struct Client; /* FWD declaration */
 /**
- * Struct containing all information regarding a tunnel
- * For an intermediate node the improtant info used will be:
- * - OID        \ To identify
- * - TID        / the tunnel
- * - paths[0]   | To know where to send it next
- * - metainfo: ready, speeds, accounting
- * For an end node more fields will be needed (client-handling)
+ * Globally unique tunnel identification (owner + number)
+ * DO NOT USE OVER THE NETWORK
  */
-struct MESH_tunnel
-{
-
+struct MESH_TunnelID {
     /**
-     * Double linked list
+     * Node that owns the tunnel
      */
-    struct MESH_tunnel          *next;
-    struct MESH_tunnel          *prev;
+    GNUNET_PEER_Id      oid;
 
     /**
-     * Origin ID: Node that created the tunnel
+     * Tunnel number to differentiate all the tunnels owned by the node oid
+     * ( tid < GNUNET_MESH_LOCAL_TUNNEL_ID_MARK )
      */
-    GNUNET_PEER_Id              oid;
+    MESH_TunnelNumber   tid;
+};
 
-    /**
-     * Tunnel number (unique for a given oid)
-     */
-    MESH_TunnelID               tid;
 
+struct MeshClient; /* FWD declaration */
+/**
+ * Struct containing all information regarding a tunnel
+ * For an intermediate node the improtant info used will be:
+ * - id        Tunnel unique identification
+ * - paths[0]  To know where to send it next
+ * - metainfo: ready, speeds, accounting
+ */
+struct MeshTunnel
+{
     /**
-     * Minimal speed for this tunnel in kb/s
+     * Tunnel ID
      */
-    uint32_t                    speed_min;
+    struct MESH_TunnelID        id;
 
     /**
-     * Maximal speed for this tunnel in kb/s
+     * Local tunnel number ( >= GNUNET_MESH_LOCAL_TUNNEL_ID_MARK or 0 )
      */
-    uint32_t                    speed_max;
+    MESH_TunnelNumber           local_tid;
 
     /**
      * Last time the tunnel was used
@@ -366,10 +236,9 @@ struct MESH_tunnel
     struct GNUNET_TIME_Absolute timestamp;
 
     /**
-     * Peers in the tunnel, for future optimizations
+     * Peers in the tunnelindexed by PeerIdentity (MeshPeerInfo)
      */
-    struct PeerInfo             *peers_head;
-    struct PeerInfo             *peers_tail;
+    struct GNUNET_CONTAINER_MultiHashMap* peers;
 
     /**
      * Number of peers that are connected and potentially ready to receive data
@@ -381,58 +250,52 @@ struct MESH_tunnel
      */
     unsigned int                peers_total;
 
-    /**
-     * Paths (used and backup)
-     */
-    struct Path                 *paths_head;
-    struct Path                 *paths_tail;
 
     /**
-     * If this tunnel was created by a local client, what's its handle?
+     * Client owner of the tunnel, if any
      */
-    struct Client               *client;
+    struct MeshClient           *client;
 
     /**
      * Messages ready to transmit
      */
-    struct MESH_queue           *out_head;
-    struct MESH_queue           *out_tail;
-
-    /**
-     * Messages received and not processed
-     */
-    struct MESH_queue           *in_head;
-    struct MESH_queue           *in_tail;
+    struct MeshQueue            *queue_head;
+    struct MeshQueue            *queue_tail;
 
 };
 
 /**
  * Struct containing information about a client of the service
  */
-struct Client
+struct MeshClient
 {
     /**
-     * Double linked list
+     * Linked list
      */
-    struct Client               *next;
-    struct Client               *prev;
+    struct MeshClient           *next;
+    struct MeshClient           *prev;
 
     /**
-     * Tunnels that belong to this client, for convenience on disconnect
+     * Tunnels that belong to this client, indexed by local id
      */
-    struct MESH_tunnel          *tunnels_head;
-    struct MESH_tunnel          *tunnels_tail;
+    struct GNUNET_CONTAINER_MultiHashMap* tunnels;
 
     /**
      * Handle to communicate with the client
      */
     struct GNUNET_SERVER_Client *handle;
 
+    /**
+     * Applications that this client has claimed to provide
+     */
+    GNUNET_MESH_ApplicationType *apps;
+    unsigned int                app_counter;
+
     /**
      * Messages that this client has declared interest in
      */
-    GNUNET_MESH_ApplicationType *messages_subscribed;
-    unsigned int                subscription_counter;
+    uint16_t                    *types;
+    unsigned int                type_counter;
 
 };
 
@@ -443,20 +306,18 @@ struct Client
 /**
  * All the clients
  */
-static struct Client                    *clients_head;
-static struct Client                    *clients_tail;
+static struct MeshClient                *clients;
+static struct MeshClient                *clients_tail;
 
 /**
- * All the tunnels
+ * Tunnels known, indexed by MESH_TunnelID (MeshTunnel)
  */
-static struct MESH_tunnel               *tunnels_head;
-static struct MESH_tunnel               *tunnels_tail;
+struct GNUNET_CONTAINER_MultiHashMap    *tunnels;
 
 /**
- * All the paths (for future path optimization)
+ * Peers known, indexed by PeerIdentity (MeshPeerInfo)
  */
-// static struct Path                   *paths_head;
-// static struct Path                   *paths_tail;
+struct GNUNET_CONTAINER_MultiHashMap    *peers;
 
 /**
  * Handle to communicate with core
@@ -473,10 +334,379 @@ static struct GNUNET_DHT_Handle         *dht_handle;
  */
 static GNUNET_PEER_Id                   myid;
 
+/**
+ * Tunnel ID for the next created tunnel (global tunnel number)
+ */
+static MESH_TunnelNumber                next_tid;
+
+/******************************************************************************/
+/******************      GENERAL HELPER FUNCTIONS      ************************/
+/******************************************************************************/
+
+/**
+ * Retrieve the MeshPeerInfo stucture associated with the peer, create one
+ * and inster it in the appropiate structures if the peer is not known yet.
+ * @param peer Identity of the peer
+ * @return Existing or newly created peer info
+ */
+static struct MeshPeerInfo *
+get_peer_info (const struct GNUNET_PeerIdentity *peer)
+{
+    struct MeshPeerInfo *       peer_info;
+
+    peer_info = GNUNET_CONTAINER_multihashmap_get(peers,
+                                                  &peer->hashPubKey);
+    if (NULL == peer_info) {
+        peer_info = (struct MeshPeerInfo *)
+                    GNUNET_malloc(sizeof(struct MeshPeerInfo));
+        GNUNET_CONTAINER_multihashmap_put(peers,
+                            &peer->hashPubKey,
+                            peer_info,
+                            GNUNET_CONTAINER_MULTIHASHMAPOPTION_UNIQUE_ONLY);
+        peer_info->id = GNUNET_PEER_intern(peer);
+        peer_info->state = MESH_PEER_SEARCHING;
+    }
+
+    return peer_info;
+}
+
+/**
+ * Find the first peer whom to send a packet to go down this path
+ * @param path The path to use
+ * @return short id of the next peer, myid in case of local delivery,
+ * or 0 in case of error
+ */
+static GNUNET_PEER_Id
+get_first_hop (struct MeshPath *path)
+{
+    unsigned int        i;
+
+    if (NULL == path) return 0;
+    if (path->in_use == 0) return 0;
+
+    for (i = 0; i < path->length; i++) {
+        if (path->peers[i] == myid) {
+            if (i < path->length - 1) {
+                return path->peers[i+1];
+            } else {
+                return myid;
+            }
+        }
+    }
+    return 0;
+}
+
+
+/**
+ * Get the cost of the path.
+ * @param path The path to analyze
+ * @return Number of hops to reach destination, UINT_MAX in case the peer is not
+ * in the path
+ */
+static unsigned int
+get_path_cost(struct MeshPath *path)
+{
+    unsigned int        i;
+
+    if (NULL == path) return UINT_MAX;
+    for (i = 0; i < path->length; i++) {
+        if (path->peers[i] == myid) {
+            return path->length - i;
+        }
+    }
+    return UINT_MAX;
+}
+
+
+/**
+ * Add the path to the peer and update the path used to reach it in case this
+ * is the shortest.
+ * @param peer_info Destination peer to add the path to.
+ * @param path New path to add. Last peer must be the peer in arg 1.
+ */
+static void
+add_path_to_peer(struct MeshPeerInfo *peer_info, struct MeshPath *path)
+{
+    unsigned int        i;
+    unsigned int        new_cost;
+    unsigned int        best_cost;
+    struct MeshPath     *aux;
+    struct MeshPath     *best;
+
+    if (NULL == peer_info || NULL == path) return;
+
+    new_cost = get_path_cost(path);
+    best_cost = UINT_MAX;
+    best = NULL;
+    for (aux = peer_info->path; aux != NULL; aux = aux->next) {
+        if ((i = get_path_cost(aux)) < best_cost) {
+            best = aux;
+            best_cost = i;
+        }
+    }
+    if (best_cost < new_cost) {
+        path->in_use = 0;
+        GNUNET_CONTAINER_DLL_insert_tail(peer_info->path,
+                                         peer_info->path_tail,
+                                         path);
+    } else {
+        if (NULL != best) best->in_use = 0;
+        path->in_use = 1;
+        GNUNET_CONTAINER_DLL_insert(peer_info->path,
+                                    peer_info->path_tail,
+                                    path);
+    }
+    return;
+}
+
+
+/**
+ * Check if client has registered with the service and has not disconnected
+ * @param client the client to check
+ * @return non-NULL if client exists in the global DLL
+ */
+static struct MeshClient *
+retrieve_client (struct GNUNET_SERVER_Client *client)
+{
+    struct MeshClient       *c;
+
+    c = clients; 
+    while (NULL != c) {
+        if (c->handle == client) return c;
+        c = c->next;
+    }
+    return NULL;
+}
+
+
+/**
+ * Search for a tunnel among the tunnels for a client
+ * @param client the client whose tunnels to search in
+ * @param tid the local id of the tunnel
+ * @return tunnel handler, NULL if doesn't exist
+ */
+static struct MeshTunnel *
+retrieve_tunnel_by_local_id (struct MeshClient *c, MESH_TunnelNumber tid)
+{
+    GNUNET_HashCode hash;
+
+    GNUNET_CRYPTO_hash(&tid, sizeof(MESH_TunnelNumber), &hash);
+    return GNUNET_CONTAINER_multihashmap_get(c->tunnels, &hash);
+}
+
+/**
+ * Search for a tunnel by global ID using PEER_ID
+ * @param pi owner of the tunnel
+ * @param tid global tunnel number
+ * @return tunnel handler, NULL if doesn't exist
+ */
+static struct MeshTunnel *
+retrieve_tunnel_by_pi (GNUNET_PEER_Id pi, MESH_TunnelNumber tid)
+{
+    struct MESH_TunnelID        id;
+    GNUNET_HashCode             hash;
+
+    id.oid = pi;
+    id.tid = tid;
+
+    GNUNET_CRYPTO_hash(&id, sizeof(struct MESH_TunnelID), &hash);
+    return GNUNET_CONTAINER_multihashmap_get(tunnels, &hash);
+}
+
+
+
+/**
+ * Search for a tunnel by global ID using full PeerIdentities
+ * @param oid owner of the tunnel
+ * @param tid global tunnel number
+ * @return tunnel handler, NULL if doesn't exist
+ */
+static struct MeshTunnel *
+retrieve_tunnel (struct GNUNET_PeerIdentity *oid, MESH_TunnelNumber tid)
+{
+    GNUNET_PEER_Id              pi;
+
+    pi = GNUNET_PEER_intern(oid);
+    GNUNET_PEER_change_rc(pi, -1);
+    return retrieve_tunnel_by_pi(pi, tid);
+}
+
+
+/**
+ * Destroy the path and free any allocated resources linked to it
+ * @param t tunnel the path belongs to
+ * @param p the path to destroy
+ * @return GNUNET_OK on success
+ */
+static int
+destroy_path(struct MeshPath *p)
+{
+    GNUNET_PEER_decrement_rcs(p->peers, p->length);
+    GNUNET_free(p->peers);
+    GNUNET_free(p);
+    return GNUNET_OK;
+}
+
+#if LATER
+/**
+ * Destroy the peer_info and free any allocated resources linked to it
+ * @param t tunnel the path belongs to
+ * @param pi the peer_info to destroy
+ * @return GNUNET_OK on success
+ */
+static int
+destroy_peer_info(struct MeshPeerInfo *pi)
+{
+    GNUNET_HashCode                     hash;
+    struct GNUNET_PeerIdentity          id;
+
+    GNUNET_PEER_resolve(pi->id, &id);
+    GNUNET_PEER_change_rc(pi->id, -1);
+    GNUNET_CRYPTO_hash(&id, sizeof(struct GNUNET_PeerIdentity), &hash);
+
+    GNUNET_CONTAINER_multihashmap_remove(peers, &hash, pi);
+    GNUNET_free(pi);
+    return GNUNET_OK;
+}
+#endif
+
+
+/**
+ * Destroy the tunnel and free any allocated resources linked to it
+ * @param c client the tunnel belongs to
+ * @param t the tunnel to destroy
+ * @return GNUNET_OK on success
+ */
+static int
+destroy_tunnel(struct MeshTunnel  *t)
+{
+//     struct MeshPath         *path;
+    struct MeshClient           *c;
+    GNUNET_HashCode             hash;
+    int                         r;
+
+    if (NULL == t) return GNUNET_OK;
+
+    c = t->client;
+
+    GNUNET_CRYPTO_hash(&t->id, sizeof(struct MESH_TunnelID), &hash);
+    if(GNUNET_YES != GNUNET_CONTAINER_multihashmap_remove(tunnels, &hash, t)) {
+        r = GNUNET_SYSERR;
+    }
+
+    GNUNET_CRYPTO_hash(&t->local_tid, sizeof(MESH_TunnelNumber), &hash);
+    if(GNUNET_YES !=
+        GNUNET_CONTAINER_multihashmap_remove(c->tunnels, &hash, t))
+    {
+        r = GNUNET_SYSERR;
+    }
+    GNUNET_free(t);
+    return r;
+}
+
+/******************************************************************************/
+/****************      MESH NETWORK HANDLER HELPERS     ***********************/
+/******************************************************************************/
+
+/**
+ * Function called to notify a client about the socket
+ * being ready to queue more data.  "buf" will be
+ * NULL and "size" zero if the socket was closed for
+ * writing in the meantime.
+ *
+ * @param cls closure
+ * @param size number of bytes available in buf
+ * @param buf where the callee should write the message
+ * @return number of bytes written to buf
+ */
+static size_t
+send_core_create_path_for_peer (void *cls, size_t size, void *buf)
+{
+    struct MeshPeerInfo                 *peer_info = cls;
+    struct GNUNET_MESH_ManipulatePath   *msg;
+    struct MeshPath                     *p;
+    struct GNUNET_PeerIdentity          *peer_ptr;
+    struct GNUNET_PeerIdentity          id;
+    size_t                              size_needed;
+    int                                 i;
+
+    if (0 == size && NULL == buf) {
+        GNUNET_log(GNUNET_ERROR_TYPE_DEBUG, "Retransmitting create path\n");
+        GNUNET_PEER_resolve(get_first_hop(peer_info->path), &id);
+        GNUNET_CORE_notify_transmit_ready(core_handle,
+                            0,
+                            0,
+                            GNUNET_TIME_UNIT_FOREVER_REL,
+                            &id,
+                            sizeof(struct GNUNET_MESH_ManipulatePath)
+                            + (peer_info->path->length
+                            * sizeof (struct GNUNET_PeerIdentity)),
+                            &send_core_create_path_for_peer,
+                            peer_info);
+        return 0;
+    }
+    p = peer_info->path;
+    while (NULL != p) {
+        if (p->in_use) {
+            break;
+        }
+        p = p->next;
+    }
+    if (p == NULL) return 0; // TODO Notify ERROR Path not found
+
+    size_needed = sizeof(struct GNUNET_MESH_ManipulatePath)
+                  + p->length * sizeof(struct GNUNET_PeerIdentity);
+    if (size < size_needed) {
+        // TODO retry? cancel?
+        return 0;
+    }
+
+    msg = (struct GNUNET_MESH_ManipulatePath *) buf;
+    msg->header.size = htons(size_needed);
+    msg->header.type = htons(GNUNET_MESSAGE_TYPE_MESH_PATH_CREATE);
+
+    peer_ptr = (struct GNUNET_PeerIdentity *) &msg[1];
+    for (i = 0; i < p->length; i++) {
+        GNUNET_PEER_resolve(p->peers[i], peer_ptr++);
+    }
+
+    peer_info->state = MESH_PEER_WAITING; // TODO maybe already ready?
+
+    return size_needed;
+}
+
+#if LATER
+/**
+ * Send another peer a notification to destroy a tunnel
+ * @param cls The tunnel to destroy
+ * @param size Size in the buffer
+ * @param buf Memory where to put the data to transmit
+ * @return Size of data put in buffer
+ */
+static size_t
+send_p2p_tunnel_destroy(void *cls, size_t size, void *buf)
+{
+    struct MeshTunnel                   *t = cls;
+    struct MeshClient                   *c;
+    struct GNUNET_MESH_TunnelMessage    *msg;
+
+    c = t->client;
+    msg = buf;
+    msg->header.type = htons(GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_DESTROY); /*FIXME*/
+    msg->header.size = htons(sizeof(struct GNUNET_MESH_TunnelMessage));
+    msg->tunnel_id = htonl(t->id.tid);
+
+    destroy_tunnel(c, t);
+    return sizeof(struct GNUNET_MESH_TunnelMessage);
+}
+#endif
+
+
 /******************************************************************************/
 /********************      MESH NETWORK HANDLERS     **************************/
 /******************************************************************************/
 
+
 /**
  * Core handler for path creation
  * struct GNUNET_CORE_MessageHandler
@@ -496,14 +726,102 @@ handle_mesh_path_create (void *cls,
                               const struct GNUNET_TRANSPORT_ATS_Information
                               *atsi)
 {
-    /* Extract path */
-    /* Find origin & self */
-    /* Search for origin in local tunnels */
-    /* Create tunnel / add path */
-    /* Retransmit to next link in chain, if any (core_notify + callback) */
+    unsigned int                        own_pos;
+    uint16_t                            size;
+    uint16_t                            i;
+    MESH_TunnelNumber                   tid;
+    struct GNUNET_MESH_ManipulatePath   *msg;
+    struct GNUNET_PeerIdentity          *pi;
+    GNUNET_HashCode                     hash;
+    struct MeshPath                     *path;
+    struct MeshPeerInfo                 *peer_info;
+    struct MeshTunnel                   *t;
+
+
+    size = ntohs(message->size);
+    if (size < sizeof(struct GNUNET_MESH_ManipulatePath)) {
+        GNUNET_log(GNUNET_ERROR_TYPE_WARNING,
+                   "received create path message too short\n");
+        return GNUNET_OK;
+    }
+
+    size -= sizeof(struct GNUNET_MESH_ManipulatePath);
+    if (size < 2 * sizeof(struct GNUNET_PeerIdentity)) {
+        GNUNET_log(GNUNET_ERROR_TYPE_WARNING,
+                   "create path message lacks enough peers\n");
+        return GNUNET_OK;
+    }
+    if (size % sizeof(struct GNUNET_PeerIdentity)) {
+        GNUNET_log(GNUNET_ERROR_TYPE_WARNING,
+                   "create path message of wrong size\n");
+        return GNUNET_OK;
+    }
+    msg = (struct GNUNET_MESH_ManipulatePath *) message;
+    size /= sizeof(struct GNUNET_PeerIdentity);
+
+    tid = ntohl(msg->tid);
+    pi = (struct GNUNET_PeerIdentity *) &msg[1];
+    t = retrieve_tunnel(pi, tid);
+
+    if (NULL == t) {
+        t = GNUNET_malloc(sizeof(struct MeshTunnel));
+        t->id.oid = GNUNET_PEER_intern(pi);
+        t->id.tid = tid;
+        t->local_tid = 0;
+        t->client = NULL;
+        t->peers = GNUNET_CONTAINER_multihashmap_create(32);
+
+        GNUNET_CRYPTO_hash(&t->id, sizeof(struct MESH_TunnelID), &hash);
+        if (GNUNET_OK !=
+            GNUNET_CONTAINER_multihashmap_put(tunnels,
+                            &hash,
+                            t,
+                            GNUNET_CONTAINER_MULTIHASHMAPOPTION_UNIQUE_ONLY))
+        {
+            GNUNET_log(GNUNET_ERROR_TYPE_WARNING,
+                   "create path: could not store tunnel in hashmap\n");
+            return GNUNET_OK;
+        }
+
+    }
+    peer_info = GNUNET_CONTAINER_multihashmap_get(peers,
+                                                  &pi[size - 1].hashPubKey);
+    if (NULL == peer_info) {
+        peer_info = GNUNET_malloc(sizeof(struct MeshPeerInfo));
+        peer_info->id = GNUNET_PEER_intern(&pi[size - 1]);
+        peer_info->state = MESH_PEER_WAITING;
+        GNUNET_CONTAINER_multihashmap_put(peers,
+                            &pi[size - 1].hashPubKey,
+                            peer_info,
+                            GNUNET_CONTAINER_MULTIHASHMAPOPTION_UNIQUE_ONLY);
+    }
+
+    path = GNUNET_malloc(sizeof(struct MeshPath));
+    path->length = size;
+    path->peers = GNUNET_malloc(size * sizeof(GNUNET_PEER_Id));
+    own_pos = 0;
+    for (i = 0; i < size; i++) {
+        path->peers[i] = GNUNET_PEER_intern(&pi[i]);
+        if (path->peers[i] == myid) own_pos = i;
+    }
+    if (own_pos == 0) { /* cannot be self, must be 'not found' */
+        GNUNET_log(GNUNET_ERROR_TYPE_WARNING,
+                   "create path: self not found in path through self\n");
+        destroy_path(path);
+        /* FIXME destroy tunnel? leave for timeout? */
+        return 0;
+    }
+    if (own_pos == size - 1) { /* it is for us! */
+        destroy_path(path); /* not needed anymore */
+        /* TODO: send ack? new meesage type? */
+    } else {
+        add_path_to_peer(peer_info, path);
+        /* TODO: Retransmit to next link in chain, if any (core_notify + callback) */
+    }
     return GNUNET_OK;
 }
 
+
 /**
  * Core handler for mesh network traffic
  *
@@ -521,7 +839,7 @@ handle_mesh_network_traffic (void *cls,
                              const struct GNUNET_TRANSPORT_ATS_Information
                              *atsi)
 {
-    if(GNUNET_MESSAGE_TYPE_MESH_DATA_GO == ntohs(message->type)) {
+    if (GNUNET_MESSAGE_TYPE_MESH_DATA_GO == ntohs(message->type)) {
         /* Retransmit to next in path of tunnel identified by message */
         return GNUNET_OK;
     } else { /* GNUNET_MESSAGE_TYPE_MESH_DATA_BACK */
@@ -530,6 +848,7 @@ handle_mesh_network_traffic (void *cls,
     }
 }
 
+
 /**
  * Functions to handle messages from core
  */
@@ -543,23 +862,177 @@ static struct GNUNET_CORE_MessageHandler core_handlers[] = {
 
 
 /******************************************************************************/
-/*********************       MESH LOCAL HANDLES      **************************/
+/****************       MESH LOCAL HANDLER HELPERS      ***********************/
 /******************************************************************************/
 
 /**
- * Check if client has registered with the service and has not disconnected
- * @param client the client to check
- * @return non-NULL if client exists in the global DLL
+ * delete_tunnel_entry: iterator for deleting each tunnel that belongs to a
+ * client when the client disconnects.
+ * @param cls closure (client that is disconnecting)
+ * @param key the hash of the local tunnel id (used to access the hashmap)
+ * @param value the value stored at the key (tunnel to destroy)
+ * @return GNUNET_OK on success
  */
-struct Client *
-client_retrieve (struct GNUNET_SERVER_Client *client) {
-    struct Client       *c;
-    for (c = clients_head; c != clients_head; c = c->next) {
-        if(c->handle == client) return c;
+static int
+delete_tunnel_entry (void *cls, const GNUNET_HashCode * key, void *value) {
+    int r;
+    r = destroy_tunnel((struct MeshTunnel *) value);
+    return r;
+}
+
+#if LATER
+/**
+ * notify_client_connection_failure: notify a client that the connection to the
+ * requested remote peer is not possible (for instance, no route found)
+ * Function called when the socket is ready to queue more data. "buf" will be
+ * NULL and "size" zero if the socket was closed for writing in the meantime.
+ *
+ * @param cls closure
+ * @param size number of bytes available in buf
+ * @param buf where the callee should write the message
+ * @return number of bytes written to buf
+ */
+static size_t
+notify_client_connection_failure (void *cls, size_t size, void *buf)
+{
+    int                                 size_needed;
+    struct MeshPeerInfo                 *peer_info;
+    struct GNUNET_MESH_PeerControl      *msg;
+    struct GNUNET_PeerIdentity          id;
+
+    if (0 == size && NULL == buf) {
+        // TODO retry? cancel?
+        return 0;
     }
-    return NULL;
+
+    size_needed = sizeof(struct GNUNET_MESH_PeerControl);
+    peer_info = (struct MeshPeerInfo *) cls;
+    msg = (struct GNUNET_MESH_PeerControl *) buf;
+    msg->header.size = htons(sizeof(struct GNUNET_MESH_PeerControl));
+    msg->header.type = htons(GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_DISCONNECTED);
+//     msg->tunnel_id = htonl(peer_info->t->tid);
+    GNUNET_PEER_resolve(peer_info->id, &id);
+    memcpy(&msg->peer, &id, sizeof(struct GNUNET_PeerIdentity));
+
+    return size_needed;
+}
+#endif
+
+
+/**
+ * Send keepalive packets for a peer
+ *
+ * @param cls unused
+ * @param tc unused
+ */
+static void
+path_refresh (void *cls, const struct GNUNET_SCHEDULER_TaskContext *tc)
+{
+    struct MeshPeerInfo         *peer_info = cls;
+    struct GNUNET_PeerIdentity  id;
+
+    if (tc->reason == GNUNET_SCHEDULER_REASON_SHUTDOWN) return;
+    GNUNET_PEER_resolve(get_first_hop(peer_info->path), &id);
+    GNUNET_CORE_notify_transmit_ready(core_handle,
+                                0,
+                                0,
+                                GNUNET_TIME_UNIT_FOREVER_REL,
+                                &id,
+                                sizeof(struct GNUNET_MESH_ManipulatePath)
+                                + (peer_info->path->length
+                                * sizeof (struct GNUNET_PeerIdentity)),
+                                &send_core_create_path_for_peer,
+                                peer_info);
+
+    return;
 }
 
+
+/**
+ * Function to process paths received for a new peer addition. The recorded
+ * paths form the initial tunnel, which can be optimized later.
+ * Called on each result obtained for the DHT search.
+ *
+ * @param cls closure
+ * @param exp when will this value expire
+ * @param key key of the result
+ * @param get_path NULL-terminated array of pointers
+ *                 to the peers on reverse GET path (or NULL if not recorded)
+ * @param put_path NULL-terminated array of pointers
+ *                 to the peers on the PUT path (or NULL if not recorded)
+ * @param type type of the result
+ * @param size number of bytes in data
+ * @param data pointer to the result data
+ */
+static void
+dht_get_response_handler(void *cls,
+                        struct GNUNET_TIME_Absolute exp,
+                        const GNUNET_HashCode * key,
+                        const struct GNUNET_PeerIdentity * const *get_path,
+                        const struct GNUNET_PeerIdentity * const *put_path,
+                        enum GNUNET_BLOCK_Type type,
+                        size_t size,
+                        const void *data)
+{
+    struct MeshPeerInfo         *peer_info = cls;
+    struct MeshPath             *p;
+    struct GNUNET_PeerIdentity  pi;
+    int                         i;
+
+    if ((NULL == get_path || NULL == put_path) && NULL == peer_info->path) {
+        // Find ourselves some alternate initial path to the destination: retry
+        GNUNET_DHT_get_stop(peer_info->dhtget);
+        GNUNET_PEER_resolve(peer_info->id, &pi);
+        peer_info->dhtget = GNUNET_DHT_get_start(dht_handle,
+                                    GNUNET_TIME_UNIT_FOREVER_REL,
+                                    GNUNET_BLOCK_TYPE_ANY,
+                                    &pi.hashPubKey,
+                                    4,    /* replication level */
+                                    GNUNET_DHT_RO_RECORD_ROUTE,
+                                    NULL, /* bloom filter */
+                                    0,    /* mutator */
+                                    NULL, /* xquery */
+                                    0,    /* xquery bits */
+                                    dht_get_response_handler,
+                                    (void *)peer_info);
+    }
+
+    p = GNUNET_malloc(sizeof(struct MeshPath));
+    for (i = 0; get_path[i] != NULL; i++);
+    for (i--; i >= 0; i--) {
+        p->peers = GNUNET_realloc(p->peers,
+                                   sizeof(GNUNET_PEER_Id) * (p->length + 1));
+        p->peers[p->length] = GNUNET_PEER_intern(get_path[i]);
+        p->length++;
+    }
+    for (i = 0; put_path[i] != NULL; i++);
+    for (i--; i >= 0; i--) {
+        p->peers = GNUNET_realloc(p->peers,
+                                  sizeof(GNUNET_PEER_Id) * (p->length + 1));
+        p->peers[p->length] = GNUNET_PEER_intern(put_path[i]);
+        p->length++;
+    }
+    add_path_to_peer(peer_info, p);
+    GNUNET_CORE_notify_transmit_ready(core_handle,
+                                      0,
+                                      0,
+                                      GNUNET_TIME_UNIT_FOREVER_REL,
+                                      get_path[1],
+                                      sizeof(struct GNUNET_MESH_ManipulatePath)
+                                        + (p->length
+                                        * sizeof (struct GNUNET_PeerIdentity)),
+                                      &send_core_create_path_for_peer,
+                                      peer_info);
+    GNUNET_SCHEDULER_add_delayed(REFRESH_PATH_TIME, &path_refresh, peer_info);
+    return;
+}
+
+
+/******************************************************************************/
+/*********************       MESH LOCAL HANDLES      **************************/
+/******************************************************************************/
+
+
 /**
  * Handler for client disconnection
  *
@@ -570,34 +1043,38 @@ client_retrieve (struct GNUNET_SERVER_Client *client) {
 static void
 handle_client_disconnect (void *cls, struct GNUNET_SERVER_Client *client)
 {
-    struct Client       *c, *next;
-    struct MESH_tunnel  *t;
+    struct MeshClient   *c;
+    struct MeshClient   *next;
 
-    /* If there are no clients registered, something is wrong... or is it?
-     * FIXME: what happens if a client connects, doesn't send a MESH_Connect
-     * and disconnects? Does the service get a disconnect notification anyway?
-     */
-    GNUNET_assert(NULL != clients_head);
-    for (c = clients_head; c != clients_head; c = next) {
+    GNUNET_log(GNUNET_ERROR_TYPE_DEBUG,
+               "client disconnected\n");
+    c = clients;
+    while (NULL != c) {
         if (c->handle == client) {
-            GNUNET_CONTAINER_DLL_remove (clients_head, clients_tail, c);
-            while (NULL != (t = c->tunnels_head)) {
-                GNUNET_CONTAINER_DLL_remove (c->tunnels_head, c->tunnels_tail, t);
-                GNUNET_CONTAINER_DLL_remove (tunnels_head, tunnels_tail, t);
-                /* TODO free paths and other tunnel dynamic structures */
-                GNUNET_free (t);
-            }
-            GNUNET_free (c->messages_subscribed);
+            GNUNET_log(GNUNET_ERROR_TYPE_DEBUG,
+               " matching client found, cleaning\n");
+            GNUNET_CONTAINER_multihashmap_iterate(c->tunnels,
+                                                  &delete_tunnel_entry,
+                                                  c);
+            GNUNET_CONTAINER_multihashmap_destroy(c->tunnels);
+            if(0 != c->app_counter) GNUNET_free (c->apps);
+            if(0 != c->type_counter) GNUNET_free (c->types);
+            GNUNET_CONTAINER_DLL_remove(clients, clients_tail, c);
             next = c->next;
             GNUNET_free (c);
+            c = next;
         } else {
-            next = c->next;
+            GNUNET_log(GNUNET_ERROR_TYPE_DEBUG,
+               "   ... searching\n");
+            c = c->next;
         }
     }
-
+    GNUNET_log(GNUNET_ERROR_TYPE_DEBUG,
+               "   done!\n");
     return;
 }
 
+
 /**
  * Handler for new clients
  * 
@@ -610,36 +1087,54 @@ handle_local_new_client (void *cls,
                          struct GNUNET_SERVER_Client *client,
                          const struct GNUNET_MessageHeader *message)
 {
-    struct Client               *c;
-    unsigned int                payload_size;
+    struct GNUNET_MESH_ClientConnect    *cc_msg;
+    struct MeshClient                   *c;
+    unsigned int                        size;
+    uint16_t                            types;
+    uint16_t                            apps;
 
+    GNUNET_log(GNUNET_ERROR_TYPE_DEBUG, "new client connected\n");
     /* Check data sanity */
-    payload_size = message->size - sizeof(struct GNUNET_MessageHeader);
-    if (0 != payload_size % sizeof(GNUNET_MESH_ApplicationType)) {
+    size = ntohs(message->size) - sizeof(struct GNUNET_MESH_ClientConnect);
+    cc_msg = (struct GNUNET_MESH_ClientConnect *) message;
+    types = ntohs(cc_msg->types);
+    apps = ntohs(cc_msg->applications);
+    if (size !=
+        types * sizeof(uint16_t) + apps * sizeof(GNUNET_MESH_ApplicationType))
+    {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
 
     /* Create new client structure */
-    c = GNUNET_malloc(sizeof(struct Client));
+    c = GNUNET_malloc(sizeof(struct MeshClient));
     c->handle = client;
-    c->tunnels_head = NULL;
-    c->tunnels_tail = NULL;
-    if(payload_size != 0) {
-        c->messages_subscribed = GNUNET_malloc(payload_size);
-        memcpy(c->messages_subscribed, &message[1], payload_size);
-    } else {
-        c->messages_subscribed = NULL;
+    if (types != 0) {
+        c->type_counter = types;
+        c->types = GNUNET_malloc(types * sizeof(uint16_t));
+        memcpy(c->types, &message[1], types * sizeof(uint16_t));
+    }
+    if (apps != 0) {
+        c->app_counter = apps;
+        c->apps = GNUNET_malloc(apps * sizeof(GNUNET_MESH_ApplicationType));
+        memcpy(c->apps,
+               &message[1] + types * sizeof(uint16_t),
+               apps * sizeof(GNUNET_MESH_ApplicationType));
     }
-    c->subscription_counter = payload_size/sizeof(GNUNET_MESH_ApplicationType);
+    GNUNET_log(GNUNET_ERROR_TYPE_DEBUG,
+               " client has %u+%u subscriptions\n",
+               c->type_counter,
+               c->app_counter);
 
-    /* Insert new client in DLL */
-    GNUNET_CONTAINER_DLL_insert (clients_head, clients_tail, c);
+    GNUNET_CONTAINER_DLL_insert(clients, clients_tail, c);
+    c->tunnels = GNUNET_CONTAINER_multihashmap_create(32);
 
     GNUNET_SERVER_receive_done(client, GNUNET_OK);
+
 }
 
+
 /**
  * Handler for requests of new tunnels
  * 
@@ -652,62 +1147,73 @@ handle_local_tunnel_create (void *cls,
                             struct GNUNET_SERVER_Client *client,
                             const struct GNUNET_MessageHeader *message)
 {
-    struct GNUNET_MESH_TunnelMessage    *tunnel_msg;
-    struct MESH_tunnel                  *t;
-    struct Client                       *c;
+    struct GNUNET_MESH_TunnelMessage    *t_msg;
+    struct MeshTunnel                   *t;
+    struct MeshClient                   *c;
+    GNUNET_HashCode                     hash;
 
     /* Sanity check for client registration */
-    if(NULL == (c = client_retrieve(client))) {
+    if (NULL == (c = retrieve_client(client))) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
 
     /* Message sanity check */
-    if(sizeof(struct GNUNET_MESH_TunnelMessage) != ntohs(message->size)) {
+    if (sizeof(struct GNUNET_MESH_TunnelMessage) != ntohs(message->size)) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
 
-    tunnel_msg = (struct GNUNET_MESH_TunnelMessage *) message;
+    t_msg = (struct GNUNET_MESH_TunnelMessage *) message;
     /* Sanity check for tunnel numbering */
-    if(0 == (ntohl(tunnel_msg->tunnel_id) & 0x80000000)) {
+    if (0 == (ntohl(t_msg->tunnel_id) & GNUNET_MESH_LOCAL_TUNNEL_ID_MARK)) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
     /* Sanity check for duplicate tunnel IDs */
-    for (t = tunnels_head; t != tunnels_head; t = t->next) {
-        if(t->tid == ntohl(tunnel_msg->tunnel_id)) {
-            GNUNET_break(0);
-            GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
-            return;
-        }
+    if(NULL != retrieve_tunnel_by_local_id(c, ntohl(t_msg->tunnel_id))) {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
     }
-    /* FIXME: calloc? Is NULL != 0 on any platform? */
-    t = GNUNET_malloc(sizeof(struct MESH_tunnel));
-    t->tid = ntohl(tunnel_msg->tunnel_id);
-    t->oid = myid;
-    t->peers_ready = 0;
-    t->peers_total = 0;
-    t->peers_head = NULL;
-    t->peers_tail = NULL;
-    t->paths_head = NULL;
-    t->paths_tail = NULL;
-    t->in_head = NULL;
-    t->in_tail = NULL;
-    t->out_head = NULL;
-    t->out_tail = NULL;
+
+    t = GNUNET_malloc(sizeof(struct MeshTunnel));
+    while (NULL != retrieve_tunnel_by_pi(myid, next_tid))
+        next_tid = (next_tid + 1) % GNUNET_MESH_LOCAL_TUNNEL_ID_MARK;
+    t->id.tid = next_tid++;
+    t->id.oid = myid;
+    t->local_tid = ntohl(t_msg->tunnel_id);
     t->client = c;
+    t->peers = GNUNET_CONTAINER_multihashmap_create(32);
 
-    GNUNET_CONTAINER_DLL_insert(tunnels_head, tunnels_tail, t);
-    GNUNET_CONTAINER_DLL_insert(c->tunnels_head, c->tunnels_tail, t);
+    GNUNET_CRYPTO_hash(&t->local_tid, sizeof(MESH_TunnelNumber), &hash);
+    if (GNUNET_OK !=
+        GNUNET_CONTAINER_multihashmap_put(c->tunnels, &hash, t,
+                            GNUNET_CONTAINER_MULTIHASHMAPOPTION_UNIQUE_ONLY))
+    {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    GNUNET_CRYPTO_hash(&t->id, sizeof(struct MESH_TunnelID), &hash);
+    if (GNUNET_OK !=
+        GNUNET_CONTAINER_multihashmap_put(tunnels, &hash, t,
+                            GNUNET_CONTAINER_MULTIHASHMAPOPTION_UNIQUE_ONLY))
+    {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
 
     GNUNET_SERVER_receive_done(client, GNUNET_OK);
     return;
 }
 
+
 /**
  * Handler for requests of deleting tunnels
  * 
@@ -721,52 +1227,45 @@ handle_local_tunnel_destroy (void *cls,
                              const struct GNUNET_MessageHeader *message)
 {
     struct GNUNET_MESH_TunnelMessage    *tunnel_msg;
-    struct Client                       *c;
-    struct MESH_tunnel                  *t;
-    MESH_TunnelID                       tid;
-    struct PeerInfo                     *pi;
+    struct MeshClient                   *c;
+    struct MeshTunnel                   *t;
+    MESH_TunnelNumber                   tid;
+    GNUNET_HashCode                     hash;
+
 
     /* Sanity check for client registration */
-    if(NULL == (c = client_retrieve(client))) {
+    if (NULL == (c = retrieve_client(client))) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
     /* Message sanity check */
-    if(sizeof(struct GNUNET_MESH_TunnelMessage) != ntohs(message->size)) {
+    if (sizeof(struct GNUNET_MESH_TunnelMessage) != ntohs(message->size)) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
 
-    /* Tunnel exists? */
     tunnel_msg = (struct GNUNET_MESH_TunnelMessage *) message;
+
+    /* Retrieve tunnel */
     tid = ntohl(tunnel_msg->tunnel_id);
-    for (t = tunnels_head; t != tunnels_head; t = t->next) {
-        if(t->tid == tid) {
-            break;
-        }
-    }
-    if(t->tid != tid) {
-        GNUNET_break(0);
-        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
-        return;
-    }
 
-    GNUNET_CONTAINER_DLL_remove(tunnels_head, tunnels_tail, t);
-    GNUNET_CONTAINER_DLL_remove(c->tunnels_head, c->tunnels_tail, t);
+    /* Remove from local id hashmap */
+    GNUNET_CRYPTO_hash(&tid, sizeof(MESH_TunnelNumber), &hash);
+    t = GNUNET_CONTAINER_multihashmap_get(c->tunnels, &hash);
+    GNUNET_CONTAINER_multihashmap_remove(c->tunnels, &hash, t);
 
-    for(pi = t->peers_head; pi != t->peers_tail; pi = t->peers_head) {
-        GNUNET_PEER_change_rc(pi->id, -1);
-        GNUNET_CONTAINER_DLL_remove(t->peers_head, t->peers_tail, pi);
-        GNUNET_free(pi);
-    }
-    GNUNET_free(t);
+    /* Remove from global id hashmap */
+    GNUNET_CRYPTO_hash(&t->id, sizeof(struct MESH_TunnelID), &hash);
+    GNUNET_CONTAINER_multihashmap_remove(tunnels, &hash, t);
 
+//     notify_tunnel_destroy(t);
     GNUNET_SERVER_receive_done(client, GNUNET_OK);
     return;
 }
 
+
 /**
  * Handler for connection requests to new peers
  * 
@@ -780,14 +1279,14 @@ handle_local_connect_add (void *cls,
                           const struct GNUNET_MessageHeader *message)
 {
     struct GNUNET_MESH_PeerControl      *peer_msg;
-    struct Client                       *c;
-    struct MESH_tunnel                  *t;
-    MESH_TunnelID                       tid;
-    struct PeerInfo                     *peer_info;
+    struct MeshClient                   *c;
+    struct MeshTunnel                   *t;
+    MESH_TunnelNumber                   tid;
+    struct MeshPeerInfo                 *peer_info;
 
 
     /* Sanity check for client registration */
-    if(NULL == (c = client_retrieve(client))) {
+    if (NULL == (c = retrieve_client(client))) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
@@ -795,45 +1294,48 @@ handle_local_connect_add (void *cls,
 
     peer_msg = (struct GNUNET_MESH_PeerControl *)message;
     /* Sanity check for message size */
-    if(sizeof(struct GNUNET_MESH_PeerControl) != ntohs(peer_msg->header.size)) {
+    if (sizeof(struct GNUNET_MESH_PeerControl)
+        != ntohs(peer_msg->header.size))
+    {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
 
-    /* Does tunnel exist? */
+    /* Tunnel exists? */
     tid = ntohl(peer_msg->tunnel_id);
-    for(t = c->tunnels_head; t != c->tunnels_head; t = t->next) {
-        if(t->tid == tid) {
-            break;
-        }
-    }
-    if(NULL == t) {
+    t = retrieve_tunnel_by_local_id(c, tid);
+    if (NULL == t) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
-    } else {
-        if(t->tid != tid) {
-            GNUNET_break(0);
-            GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
-            return;
-        }
     }
 
     /* Does client own tunnel? */
-    if(t->client->handle != client) {
+    if (t->client->handle != client) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
 
-    /* Ok, add peer to tunnel */
-    peer_info = (struct PeerInfo *) GNUNET_malloc(sizeof(struct PeerInfo));
-    peer_info->id = GNUNET_PEER_intern(&peer_msg->peer);
-    peer_info->state = MESH_PEER_WAITING;
     t->peers_total++;
-    GNUNET_CONTAINER_DLL_insert(t->peers_head, t->peers_tail, peer_info);
-    /* TODO MESH SEARCH FOR PEER */
+    peer_info = get_peer_info(&peer_msg->peer);
+
+    /* Start DHT search if needed */
+    if(MESH_PEER_READY != peer_info->state && NULL == peer_info->dhtget) {
+        peer_info->dhtget = GNUNET_DHT_get_start(dht_handle,
+                                            GNUNET_TIME_UNIT_FOREVER_REL,
+                                            GNUNET_BLOCK_TYPE_ANY,
+                                            &peer_msg->peer.hashPubKey,
+                                            4,    /* replication level */
+                                            GNUNET_DHT_RO_RECORD_ROUTE,
+                                            NULL, /* bloom filter */
+                                            0,    /* mutator */
+                                            NULL, /* xquery */
+                                            0,    /* xquery bits */
+                                            dht_get_response_handler,
+                                            (void *)peer_info);
+    }
 
     GNUNET_SERVER_receive_done(client, GNUNET_OK);
     return;
@@ -852,12 +1354,52 @@ handle_local_connect_del (void *cls,
                           struct GNUNET_SERVER_Client *client,
                           const struct GNUNET_MessageHeader *message)
 {
+    struct GNUNET_MESH_PeerControl      *peer_msg;
+    struct MeshClient                   *c;
+    struct MeshTunnel                   *t;
+    MESH_TunnelNumber                   tid;
+    GNUNET_PEER_Id                      peer_id;
+
     /* Sanity check for client registration */
-    if(NULL == client_retrieve(client)) {
+    if (NULL == (c = retrieve_client(client))) {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+    peer_msg = (struct GNUNET_MESH_PeerControl *)message;
+    /* Sanity check for message size */
+    if (sizeof(struct GNUNET_MESH_PeerControl)
+        != ntohs(peer_msg->header.size))
+    {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    /* Tunnel exists? */
+    tid = ntohl(peer_msg->tunnel_id);
+    t = retrieve_tunnel_by_local_id(c, tid);
+    if (NULL == t) {
+            GNUNET_break(0);
+            GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+            return;
+        }
+
+    /* Does client own tunnel? */
+    if (t->client->handle != client) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
+
+    /* Ok, delete peer from tunnel */
+    peer_id = GNUNET_PEER_intern(&peer_msg->peer);
+
+    /* FIXME Delete paths */
+    /* FIXME Delete peer info */
+
+    GNUNET_PEER_change_rc(peer_id, -1);
+
     GNUNET_SERVER_receive_done(client, GNUNET_OK);
     return;
 }
@@ -875,12 +1417,49 @@ handle_local_connect_by_type (void *cls,
                               struct GNUNET_SERVER_Client *client,
                               const struct GNUNET_MessageHeader *message)
 {
+    struct GNUNET_MESH_ConnectPeerByType        *connect_msg;
+    MESH_TunnelNumber                           tid;
+    GNUNET_MESH_ApplicationType                 application;
+    struct MeshClient                           *c;
+    struct MeshTunnel                           *t;
+
     /* Sanity check for client registration */
-    if(NULL == client_retrieve(client)) {
+    if (NULL == (c = retrieve_client(client))) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
+
+    connect_msg = (struct GNUNET_MESH_ConnectPeerByType *)message;
+    /* Sanity check for message size */
+    if (sizeof(struct GNUNET_MESH_PeerControl) !=
+            ntohs(connect_msg->header.size))
+    {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    /* Tunnel exists? */
+    tid = ntohl(connect_msg->tunnel_id);
+    t = retrieve_tunnel_by_local_id(c, tid);
+    if (NULL == t) {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    /* Does client own tunnel? */
+    if (t->client->handle != client) {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    /* Ok, lets find a peer offering the service */
+    application = ntohl(connect_msg->type);
+    application++; // FIXME silence warnings
+
     GNUNET_SERVER_receive_done(client, GNUNET_OK);
     return;
 }
@@ -898,12 +1477,45 @@ handle_local_network_traffic (void *cls,
                          struct GNUNET_SERVER_Client *client,
                          const struct GNUNET_MessageHeader *message)
 {
+    struct MeshClient                           *c;
+    struct MeshTunnel                           *t;
+    struct GNUNET_MESH_Data                     *data_msg;
+    MESH_TunnelNumber                           tid;
+
     /* Sanity check for client registration */
-    if(NULL == client_retrieve(client)) {
+    if (NULL == (c = retrieve_client(client))) {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+    data_msg = (struct GNUNET_MESH_Data *)message;
+    /* Sanity check for message size */
+    if (sizeof(struct GNUNET_MESH_PeerControl) !=
+            ntohs(data_msg->header.size))
+    {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    /* Tunnel exists? */
+    tid = ntohl(data_msg->tunnel_id);
+    t = retrieve_tunnel_by_local_id(c, tid);
+    if (NULL == t) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
+
+    /* Does client own tunnel? */
+    if (t->client->handle != client) {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    /* TODO */
+
     GNUNET_SERVER_receive_done(client, GNUNET_OK);
     return;
 }
@@ -920,12 +1532,45 @@ handle_local_network_traffic_bcast (void *cls,
                                     struct GNUNET_SERVER_Client *client,
                                     const struct GNUNET_MessageHeader *message)
 {
+    struct MeshClient                           *c;
+    struct MeshTunnel                           *t;
+    struct GNUNET_MESH_DataBroadcast            *data_msg;
+    MESH_TunnelNumber                           tid;
+
     /* Sanity check for client registration */
-    if(NULL == client_retrieve(client)) {
+    if (NULL == (c = retrieve_client(client))) {
         GNUNET_break(0);
         GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
         return;
     }
+    data_msg = (struct GNUNET_MESH_DataBroadcast *)message;
+    /* Sanity check for message size */
+    if (sizeof(struct GNUNET_MESH_PeerControl)
+        != ntohs(data_msg->header.size))
+    {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    /* Tunnel exists? */
+    tid = ntohl(data_msg->tunnel_id);
+    t = retrieve_tunnel_by_local_id(c, tid);
+    if (NULL == t) {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    /* Does client own tunnel? */
+    if (t->client->handle != client) {
+        GNUNET_break(0);
+        GNUNET_SERVER_receive_done(client, GNUNET_SYSERR);
+        return;
+    }
+
+    /*  TODO */
+
     GNUNET_SERVER_receive_done(client, GNUNET_OK);
     return;
 }
@@ -936,13 +1581,17 @@ handle_local_network_traffic_bcast (void *cls,
 static struct GNUNET_SERVER_MessageHandler plugin_handlers[] = {
   {&handle_local_new_client, NULL, GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT, 0},
   {&handle_local_tunnel_create, NULL,
-   GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_CREATE, 0},
+   GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_CREATE,
+   sizeof(struct GNUNET_MESH_TunnelMessage)},
   {&handle_local_tunnel_destroy, NULL,
-   GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_DESTROY, 0},
+   GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_DESTROY,
+   sizeof(struct GNUNET_MESH_TunnelMessage)},
   {&handle_local_connect_add, NULL,
-   GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT_PEER_ADD, 0},
+   GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT_PEER_ADD,
+   sizeof(struct GNUNET_MESH_PeerControl)},
   {&handle_local_connect_del, NULL,
-   GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT_PEER_DEL, 0},
+   GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT_PEER_DEL,
+   sizeof(struct GNUNET_MESH_PeerControl)},
   {&handle_local_connect_by_type, NULL,
    GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT_PEER_BY_TYPE,
    sizeof(struct GNUNET_MESH_ConnectPeerByType)},
@@ -968,6 +1617,8 @@ core_init (void *cls,
            const struct GNUNET_PeerIdentity *identity,
            const struct GNUNET_CRYPTO_RsaPublicKeyBinaryEncoded *publicKey)
 {
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "Core init\n");
     core_handle = server;
     myid = GNUNET_PEER_intern(identity);
     return;
@@ -985,6 +1636,23 @@ core_connect (void *cls,
               const struct GNUNET_PeerIdentity *peer,
               const struct GNUNET_TRANSPORT_ATS_Information *atsi)
 {
+//     GNUNET_PEER_Id              pid;
+    struct MeshPeerInfo         *peer_info;
+    struct MeshPath             *path;
+
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "Peer connected\n");
+    peer_info = get_peer_info(peer);
+    if (myid == peer_info->id) {
+        GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "     (self)\n");
+    }
+    path = GNUNET_malloc(sizeof(struct MeshPath));
+    path->length = 2;
+    path->peers = GNUNET_malloc(sizeof(GNUNET_PEER_Id) * 2);
+    path->peers[0] = myid;
+    path->peers[1] = peer_info->id;
+    add_path_to_peer(peer_info, path);
     return;
 }
 
@@ -999,15 +1667,48 @@ core_disconnect (void *cls,
                 const struct
                 GNUNET_PeerIdentity *peer)
 {
+    GNUNET_PEER_Id      pid;
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "Peer disconnected\n");
+    pid = GNUNET_PEER_intern(peer);
+    GNUNET_PEER_change_rc(pid, -1);
+    if (myid == pid) {
+        GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "     (self)\n");
+    }
     return;
 }
 
+
 /******************************************************************************/
 /************************      MAIN FUNCTIONS      ****************************/
 /******************************************************************************/
 
 /**
- * Process mesh requests. FIXME NON FUNCTIONAL, SKELETON
+ * Task run during shutdown.
+ *
+ * @param cls unused
+ * @param tc unused
+ */
+static void
+shutdown_task (void *cls, const struct GNUNET_SCHEDULER_TaskContext *tc)
+{
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "shutting down\n");
+    if (core_handle != NULL) {
+        GNUNET_CORE_disconnect (core_handle);
+        core_handle = NULL;
+    }
+    if (dht_handle != NULL) {
+        GNUNET_DHT_disconnect (dht_handle);
+        dht_handle = NULL;
+    }
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "shut down\n");
+}
+
+/**
+ * Process mesh requests.
  *
  * @param cls closure
  * @param server the initialized server
@@ -1018,11 +1719,12 @@ run (void *cls,
      struct GNUNET_SERVER_Handle *server,
      const struct GNUNET_CONFIGURATION_Handle *c)
 {
-
-  GNUNET_SERVER_add_handlers (server, plugin_handlers);
-  GNUNET_SERVER_disconnect_notify (server, &handle_client_disconnect, NULL);
-  core_handle = GNUNET_CORE_connect (c,                 /* Main configuration */
-                            32,                                 /* queue size */
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "starting to run\n");
+    GNUNET_SERVER_add_handlers (server, plugin_handlers);
+    GNUNET_SERVER_disconnect_notify (server, &handle_client_disconnect, NULL);
+    core_handle = GNUNET_CORE_connect (c,               /* Main configuration */
+                            1,                                  /* queue size */
                             NULL,         /* Closure passed to MESH functions */
                             &core_init,      /* Call core_init once connected */
                             &core_connect,                 /* Handle connects */
@@ -1033,15 +1735,26 @@ run (void *cls,
                             NULL, /* Don't notify about all outbound messages */
                             GNUNET_NO,    /* For header-only out notification */
                             core_handlers);        /* Register these handlers */
+    if (core_handle == NULL) {
+        GNUNET_break(0);
+    }
+    dht_handle = GNUNET_DHT_connect(c, 64);
+    if (dht_handle == NULL) {
+        GNUNET_break(0);
+    }
+    next_tid = 0;
+
+    tunnels = GNUNET_CONTAINER_multihashmap_create(32);
+    peers = GNUNET_CONTAINER_multihashmap_create(32);
+    clients = NULL;
+    clients_tail = NULL;
 
-  if (core_handle == NULL) {
-      GNUNET_break(0);
-  }
-  
-  dht_handle = GNUNET_DHT_connect(c, 100); /* FIXME ht len correct size? */
-  if (dht_handle == NULL) {
-      GNUNET_break(0);
-  }
+    /* Scheduled the task to clean up when shutdown is called */
+    GNUNET_SCHEDULER_add_delayed (GNUNET_TIME_UNIT_FOREVER_REL,
+                                  &shutdown_task, NULL);
+
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "end if run()\n");
 }
 
 /**
@@ -1062,5 +1775,7 @@ main (int argc, char *const *argv)
                                "mesh",
                                GNUNET_SERVICE_OPTION_NONE,
                                &run, NULL)) ? 0 : 1;
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "end of main()\n");
     return ret;
-    }
+}