Allowed to destroy NULL paths
[oweals/gnunet.git] / src / mesh / mesh_api_new.c
index 5c11ab6651738131a756ae6ec92d8ad552779f63..64f880e062ef3335d588a84886ce9ef80af02366 100644 (file)
@@ -1,4 +1,3 @@
-
 /*
      This file is part of GNUnet.
      (C) 2011 Christian Grothoff (and other contributing authors)
  * @brief mesh api: client implementation of mesh service
  * @author Bartlomiej Polot
  *
- * TODO:
- * - handle reconnect (service crash/disconnect) properly
- * - callbacks to client missing on certain events
- * - processing messages from service is incomplete
- *
  * STRUCTURE:
  * - CONSTANTS
  * - DATA STRUCTURES
@@ -51,6 +45,9 @@ extern "C"
 #include "mesh.h"
 #include "mesh_protocol.h"
 
+#define MESH_API_DEBUG GNUNET_YES
+
+#define LOG(kind,...) GNUNET_log_from (kind, "mesh-api",__VA_ARGS__)
 
 /******************************************************************************/
 /************************      DATA STRUCTURES     ****************************/
@@ -77,13 +74,6 @@ struct GNUNET_MESH_TransmitHandle
      */
   struct GNUNET_MESH_Tunnel *tunnel;
 
-    /**
-     * Data itself, currently points to the end of this struct if
-     * we have a message already, NULL if the message is to be
-     * obtained from the callback.
-     */
-  const struct GNUNET_MessageHeader *data;
-
     /**
      * Callback to obtain the message to transmit, or NULL if we
      * got the message in 'data'.  Notice that messages built
@@ -117,7 +107,7 @@ struct GNUNET_MESH_TransmitHandle
   uint32_t priority;
 
     /**
-     * Target of the message, 0 for broadcast.  This field
+     * Target of the message, 0 for multicast.  This field
      * is only valid if 'notify' is non-NULL.
      */
   GNUNET_PEER_Id target;
@@ -160,7 +150,12 @@ struct GNUNET_MESH_Handle
   struct GNUNET_MESH_Tunnel *tunnels_tail;
 
     /**
-     * Callback for tunnel disconnection
+     * Callback for inbound tunnel creation
+     */
+  GNUNET_MESH_InboundTunnelNotificationHandler *new_tunnel;
+
+    /**
+     * Callback for inbound tunnel disconnection
      */
   GNUNET_MESH_TunnelEndHandler *cleaner;
 
@@ -198,6 +193,16 @@ struct GNUNET_MESH_Handle
      * Number of packets queued
      */
   unsigned int npackets;
+
+  /**
+   * Configuration given by the client, in case of reconnection
+   */
+  const struct GNUNET_CONFIGURATION_Handle *cfg;
+
+  /**
+   * Time to the next reconnect in case one reconnect fails
+   */
+  struct GNUNET_TIME_Relative reconnect_time;
 };
 
 
@@ -221,10 +226,6 @@ struct GNUNET_MESH_Peer
    */
   int connected;
 
-    /**
-     * Task to cancel the connection request for this peer
-     */
-  GNUNET_SCHEDULER_TaskIdentifier cancel;
 };
 
 
@@ -243,17 +244,12 @@ struct GNUNET_MESH_Tunnel
     /**
      * Callback to execute when peers connect to the tunnel
      */
-  GNUNET_MESH_TunnelConnectHandler connect_handler;
+  GNUNET_MESH_PeerConnectHandler connect_handler;
 
     /**
-     * Callback to execute when peers disconnect to the tunnel
+     * Callback to execute when peers disconnect from the tunnel
      */
-  GNUNET_MESH_TunnelDisconnectHandler disconnect_handler;
-
-    /**
-     * All peers added to the tunnel
-     */
-  struct GNUNET_MESH_Peer **peers;
+  GNUNET_MESH_PeerDisconnectHandler disconnect_handler;
 
     /**
      * Closure for the connect/disconnect handlers
@@ -276,6 +272,21 @@ struct GNUNET_MESH_Tunnel
   GNUNET_PEER_Id owner;
 
     /**
+     * All peers added to the tunnel
+     */
+  struct GNUNET_MESH_Peer **peers;
+
+  /**
+   * List of application types that have been requested for this tunnel
+   */
+  GNUNET_MESH_ApplicationType *apps;
+
+  /**
+   * Any data the caller wants to put in here
+   */
+  void *ctx;
+
+  /**
      * Number of peers added to the tunnel
      */
   unsigned int npeers;
@@ -284,6 +295,12 @@ struct GNUNET_MESH_Tunnel
      * Number of packets queued in this tunnel
      */
   unsigned int npackets;
+
+    /**
+     * Number of applications requested this tunnel
+     */
+  unsigned int napps;
+
 };
 
 
@@ -314,43 +331,164 @@ retrieve_tunnel (struct GNUNET_MESH_Handle *h, MESH_TunnelNumber tid)
 
 
 /**
- * Notify client that the transmission has timed out
- * @param cls closure
- * @param tc task context
+ * Create a new tunnel and insert it in the tunnel list of the mesh handle
+ * @param h Mesh handle
+ * @param tid desired tid of the tunnel, 0 to assign one automatically
+ * @return handle to the created tunnel
+ */
+static struct GNUNET_MESH_Tunnel *
+create_tunnel (struct GNUNET_MESH_Handle *h, MESH_TunnelNumber tid)
+{
+  struct GNUNET_MESH_Tunnel *t;
+
+  t = GNUNET_malloc (sizeof (struct GNUNET_MESH_Tunnel));
+  GNUNET_CONTAINER_DLL_insert (h->tunnels_head, h->tunnels_tail, t);
+  t->mesh = h;
+  if (0 == tid)
+  {
+    t->tid = h->next_tid;
+    while (NULL != retrieve_tunnel (h, h->next_tid))
+    {
+      h->next_tid++;
+      h->next_tid &= ~GNUNET_MESH_LOCAL_TUNNEL_ID_SERV;
+      h->next_tid |= GNUNET_MESH_LOCAL_TUNNEL_ID_CLI;
+    }
+  }
+  else
+  {
+    t->tid = tid;
+  }
+  return t;
+}
+
+
+/**
+ * Destroy the specified tunnel.
+ * - Destroys all peers, calling the disconnect callback on each if needed
+ * - Cancels all outgoing traffic for that tunnel, calling respective notifys
+ * - Calls cleaner if tunnel was inbound
+ * - Frees all memory used
+ * @param tid ID of the wanted tunnel
+ * @return handle to the required tunnel or NULL if not found
  */
 static void
-timeout_transmission (void *cls, const struct GNUNET_SCHEDULER_TaskContext *tc)
+destroy_tunnel (struct GNUNET_MESH_Tunnel *t)
 {
-  struct GNUNET_MESH_TransmitHandle *th = cls;
-  struct GNUNET_MESH_Handle *mesh;
+  struct GNUNET_MESH_Handle *h;
+  struct GNUNET_PeerIdentity pi;
+  struct GNUNET_MESH_TransmitHandle *th;
+  struct GNUNET_MESH_TransmitHandle *aux;
+  unsigned int i;
 
-  mesh = th->tunnel->mesh;
-  GNUNET_CONTAINER_DLL_remove (mesh->th_head, mesh->th_tail, th);
-  if (th->notify != NULL)
-    th->notify (th->notify_cls, 0, NULL);
-  GNUNET_free (th);
-  if ((NULL == mesh->th_head) && (NULL != mesh->th))
+  if (NULL == t)
   {
-    /* queue empty, no point in asking for transmission */
-    GNUNET_CLIENT_notify_transmit_ready_cancel (mesh->th);
-    mesh->th = NULL;
+    GNUNET_break (0);
+    return;
   }
+  h = t->mesh;
+  th = h->th_head;
+  while (NULL != th)
+  {
+    if (th->tunnel == t)
+    {
+      aux = th->next;
+      GNUNET_CONTAINER_DLL_remove (h->th_head, h->th_tail, th);
+      if (NULL == h->th_head && NULL != h->th)
+      {
+        GNUNET_CLIENT_notify_transmit_ready_cancel (h->th);
+        h->th = NULL;
+      }
+      if (NULL != th->notify)
+        th->notify (th->notify_cls, 0, NULL);
+      if (GNUNET_SCHEDULER_NO_TASK != th->timeout_task)
+        GNUNET_SCHEDULER_cancel (th->timeout_task);
+      GNUNET_free (th);
+      th = aux;
+    }
+    else
+    {
+      th = th->next;
+    }
+  }
+  GNUNET_CONTAINER_DLL_remove (h->tunnels_head, h->tunnels_tail, t);
+  for (i = 0; i < t->npeers; i++)
+  {
+    if (NULL != t->disconnect_handler && t->peers[i]->connected)
+    {
+      GNUNET_PEER_resolve (t->peers[i]->id, &pi);
+      t->disconnect_handler (t->cls, &pi);
+    }
+    GNUNET_PEER_change_rc (t->peers[i]->id, -1);
+    GNUNET_free (t->peers[i]);
+  }
+  if (t->npeers > 0)
+    GNUNET_free (t->peers);
+  if (NULL != h->cleaner && 0 != t->owner)
+    h->cleaner (h->cls, t, t->ctx);
+  if (0 != t->owner)
+    GNUNET_PEER_change_rc (t->owner, -1);
+  if (0 != t->napps && t->apps)
+    GNUNET_free (t->apps);
+  GNUNET_free (t);
+  return;
 }
 
 
 /**
- * Notify client that the transmission has timed out
- * @param cls closure
- * @param tc task context
+ * Get the peer descriptor for the peer with id from the given tunnel
+ * @param t Tunnel handle
+ * @param id Short form ID of the wanted peer
+ * @return handle to the requested peer or NULL if not found
+ */
+static struct GNUNET_MESH_Peer *
+retrieve_peer (struct GNUNET_MESH_Tunnel *t, GNUNET_PEER_Id id)
+{
+  unsigned int i;
+
+  for (i = 0; i < t->npeers; i++)
+    if (t->peers[i]->id == id)
+      return t->peers[i];
+  return NULL;
+}
+
+
+/**
+ * Add a peer into a tunnel
+ * @param t Tunnel handle
+ * @param pi Full ID of the new peer
+ * @return handle to the newly created peer
+ */
+static struct GNUNET_MESH_Peer *
+add_peer_to_tunnel (struct GNUNET_MESH_Tunnel *t,
+                    const struct GNUNET_PeerIdentity *pi)
+{
+  struct GNUNET_MESH_Peer *p;
+  GNUNET_PEER_Id id;
+
+  if (0 != t->owner)
+  {
+    GNUNET_break (0);
+    return NULL;
+  }
+  id = GNUNET_PEER_intern (pi);
+
+  p = GNUNET_malloc (sizeof (struct GNUNET_MESH_Peer));
+  p->id = id;
+  p->t = t;
+  GNUNET_array_append (t->peers, t->npeers, p);
+  return p;
+}
+
+
+/**
+ * Remove a peer from a tunnel
+ * @param p Peer handle
  */
 static void
-timeout_peer_request (void *cls, const struct GNUNET_SCHEDULER_TaskContext *tc)
+remove_peer_from_tunnel (struct GNUNET_MESH_Peer *p)
 {
-  struct GNUNET_MESH_Peer *p = cls;
-  struct GNUNET_PeerIdentity id;
   unsigned int i;
 
-  GNUNET_assert (0 == p->connected);
   for (i = 0; i < p->t->npeers; i++)
   {
     if (p->t->peers[i] == p)
@@ -363,20 +501,40 @@ timeout_peer_request (void *cls, const struct GNUNET_SCHEDULER_TaskContext *tc)
   }
   p->t->peers[i] = p->t->peers[p->t->npeers - 1];
   GNUNET_array_grow (p->t->peers, p->t->npeers, p->t->npeers - 1);
+}
 
-  if (NULL != p->t->connect_handler)
-    p->t->connect_handler (p->t->cls, 0, NULL);
 
-  GNUNET_PEER_resolve (p->id, &id);
-  GNUNET_MESH_peer_request_connect_del (p->t, &id);
+/**
+ * Notify client that the transmission has timed out
+ * @param cls closure
+ * @param tc task context
+ */
+static void
+timeout_transmission (void *cls, const struct GNUNET_SCHEDULER_TaskContext *tc)
+{
+  struct GNUNET_MESH_TransmitHandle *th = cls;
+  struct GNUNET_MESH_Handle *mesh;
+
+  mesh = th->tunnel->mesh;
+  GNUNET_CONTAINER_DLL_remove (mesh->th_head, mesh->th_tail, th);
+  if (th->notify != NULL)
+    th->notify (th->notify_cls, 0, NULL);
+  GNUNET_free (th);
+  if ((NULL == mesh->th_head) && (NULL != mesh->th))
+  {
+    /* queue empty, no point in asking for transmission */
+    GNUNET_CLIENT_notify_transmit_ready_cancel (mesh->th);
+    mesh->th = NULL;
+  }
 }
 
+
 /**
  * Add a transmit handle to the transmission queue by priority and set the
  * timeout if needed.
  *
  * @param h mesh handle with the queue head and tail
- * @param q handle to the packet to be transmitted
+ * @param th handle to the packet to be transmitted
  */
 static void
 add_to_queue (struct GNUNET_MESH_Handle *h,
@@ -385,7 +543,7 @@ add_to_queue (struct GNUNET_MESH_Handle *h,
   struct GNUNET_MESH_TransmitHandle *p;
 
   p = h->th_head;
-  while ((NULL != p) && (th->priority < p->priority))
+  while ((NULL != p) && (th->priority <= p->priority))
     p = p->next;
   if (NULL == p)
     p = h->th_tail;
@@ -400,6 +558,182 @@ add_to_queue (struct GNUNET_MESH_Handle *h,
 }
 
 
+/**
+ * Auxiliary function to send an already constructed packet to the service.
+ * Takes care of creating a new queue element, copying the message and
+ * calling the tmt_rdy function if necessary.
+ * @param h mesh handle
+ * @param msg message to transmit
+ */
+static void
+send_packet (struct GNUNET_MESH_Handle *h,
+             const struct GNUNET_MessageHeader *msg);
+
+
+/**
+ * Reconnect callback: tries to reconnect again after a failer previous
+ * reconnecttion
+ * @param cls closure (mesh handle)
+ * @param tc task context
+ */
+static void
+reconnect_cbk (void *cls, const struct GNUNET_SCHEDULER_TaskContext *tc);
+
+
+/**
+ * Send a connect packet to the service with the applications and types
+ * requested by the user.
+ *
+ * @param h The mesh handle.
+ *
+ */
+static void
+send_connect (struct GNUNET_MESH_Handle *h)
+{
+  size_t size;
+
+  size = sizeof (struct GNUNET_MESH_ClientConnect);
+  size += h->n_applications * sizeof (GNUNET_MESH_ApplicationType);
+  size += h->n_handlers * sizeof (uint16_t);
+  {
+    char buf[size];
+    struct GNUNET_MESH_ClientConnect *msg;
+    GNUNET_MESH_ApplicationType *apps;
+    uint16_t napps;
+    uint16_t *types;
+    uint16_t ntypes;
+
+    /* build connection packet */
+    msg = (struct GNUNET_MESH_ClientConnect *) buf;
+    msg->header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT);
+    msg->header.size = htons (size);
+    apps = (GNUNET_MESH_ApplicationType *) &msg[1];
+    for (napps = 0; napps < h->n_applications; napps++)
+    {
+      apps[napps] = htonl (h->applications[napps]);
+      LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh:  app %u\n", h->applications[napps]);
+    }
+    types = (uint16_t *) & apps[napps];
+    for (ntypes = 0; ntypes < h->n_handlers; ntypes++)
+      types[ntypes] = htons (h->message_handlers[ntypes].type);
+    msg->applications = htons (napps);
+    msg->types = htons (ntypes);
+#if MESH_API_DEBUG
+    LOG (GNUNET_ERROR_TYPE_DEBUG,
+         "mesh: Sending %lu bytes long message %d types and %d apps\n",
+         ntohs (msg->header.size), ntypes, napps);
+#endif
+    send_packet (h, &msg->header);
+  }
+}
+
+
+/**
+ * Reconnect to the service, retransmit all infomation to try to restore the
+ * original state.
+ *
+ * @param h handle to the mesh
+ *
+ * @return GNUNET_YES in case of sucess, GNUNET_NO otherwise (service down...)
+ */
+static int
+reconnect (struct GNUNET_MESH_Handle *h)
+{
+  struct GNUNET_MESH_Tunnel *t;
+  unsigned int i;
+
+#if MESH_API_DEBUG
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: *****************************\n");
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: *******   RECONNECT   *******\n");
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: *****************************\n");
+#endif
+  h->in_receive = GNUNET_NO;
+  /* disconnect */
+  if (NULL != h->th)
+  {
+    GNUNET_CLIENT_notify_transmit_ready_cancel (h->th);
+  }
+  if (NULL != h->client)
+  {
+    GNUNET_CLIENT_disconnect (h->client, GNUNET_NO);
+  }
+
+  /* connect again */
+  h->client = GNUNET_CLIENT_connect ("mesh", h->cfg);
+  if (h->client == NULL)
+  {
+    GNUNET_SCHEDULER_add_delayed (h->reconnect_time, &reconnect_cbk, h);
+    h->reconnect_time =
+        GNUNET_TIME_relative_min (GNUNET_TIME_UNIT_HOURS,
+                                  GNUNET_TIME_relative_multiply
+                                  (h->reconnect_time, 2));
+    LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh:   Next retry in %sms\n",
+         GNUNET_TIME_relative_to_string (h->reconnect_time));
+    GNUNET_break (0);
+    return GNUNET_NO;
+  }
+  else
+  {
+    h->reconnect_time = GNUNET_TIME_UNIT_MILLISECONDS;
+  }
+  send_connect (h);
+  /* Rebuild all tunnels */
+  for (t = h->tunnels_head; NULL != t; t = t->next)
+  {
+    struct GNUNET_MESH_TunnelMessage tmsg;
+    struct GNUNET_MESH_PeerControl pmsg;
+
+    tmsg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_CREATE);
+    tmsg.header.size = htons (sizeof (struct GNUNET_MESH_TunnelMessage));
+    tmsg.tunnel_id = htonl (t->tid);
+    send_packet (h, &tmsg.header);
+
+    pmsg.header.size = htons (sizeof (struct GNUNET_MESH_PeerControl));
+    pmsg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_ADD);
+    pmsg.tunnel_id = htonl (t->tid);
+
+    /* Reconnect all peers */
+    for (i = 0; i < t->npeers; i++)
+    {
+      GNUNET_PEER_resolve (t->peers[i]->id, &pmsg.peer);
+      if (NULL != t->disconnect_handler && t->peers[i]->connected)
+        t->disconnect_handler (t->cls, &pmsg.peer);
+      /* If the tunnel was "by type", dont connect individual peers */
+      if (0 == t->napps)
+        send_packet (t->mesh, &pmsg.header);
+    }
+    /* Reconnect all types, if any  */
+    for (i = 0; i < t->napps; i++)
+    {
+      struct GNUNET_MESH_ConnectPeerByType msg;
+
+      msg.header.size = htons (sizeof (struct GNUNET_MESH_ConnectPeerByType));
+      msg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_ADD_BY_TYPE);
+      msg.tunnel_id = htonl (t->tid);
+      msg.type = htonl (t->apps[i]);
+      send_packet (t->mesh, &msg.header);
+    }
+  }
+  return GNUNET_YES;
+}
+
+/**
+ * Reconnect callback: tries to reconnect again after a failer previous
+ * reconnecttion
+ * @param cls closure (mesh handle)
+ * @param tc task context
+ */
+static void
+reconnect_cbk (void *cls, const struct GNUNET_SCHEDULER_TaskContext *tc)
+{
+  struct GNUNET_MESH_Handle *h = cls;
+
+  if (tc->reason == GNUNET_SCHEDULER_REASON_SHUTDOWN)
+    return;
+  reconnect (h);
+}
+
+
 /******************************************************************************/
 /***********************      RECEIVE HANDLERS     ****************************/
 /******************************************************************************/
@@ -411,25 +745,65 @@ add_to_queue (struct GNUNET_MESH_Handle *h,
  * @param msg   A message with the details of the new incoming tunnel
  */
 static void
-process_tunnel_create (struct GNUNET_MESH_Handle *h,
-                       const struct GNUNET_MESH_TunnelMessage *msg)
+process_tunnel_created (struct GNUNET_MESH_Handle *h,
+                        const struct GNUNET_MESH_TunnelNotification *msg)
 {
   struct GNUNET_MESH_Tunnel *t;
+  struct GNUNET_ATS_Information atsi;
   MESH_TunnelNumber tid;
 
   tid = ntohl (msg->tunnel_id);
-  if (tid >= GNUNET_MESH_LOCAL_TUNNEL_ID_MARK)
+  if (tid <= GNUNET_MESH_LOCAL_TUNNEL_ID_CLI)
   {
-    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
-                "MESH: received an incoming tunnel with tid in local range (%X)\n",
-                tid);
-    GNUNET_break_op (0);
-    return;                     //FIXME abort? reconnect?
+    GNUNET_break (0);
+    return;
   }
-  t = GNUNET_malloc (sizeof (struct GNUNET_MESH_Tunnel));
-  t->cls = h->cls;
+  t = create_tunnel (h, tid);
+  t->owner = GNUNET_PEER_intern (&msg->peer);
+  t->npeers = 1;
+  t->peers = GNUNET_malloc (sizeof (struct GNUNET_MESH_Peer *));
+  t->peers[0] = GNUNET_malloc (sizeof (struct GNUNET_MESH_Peer));
+  t->peers[0]->t = t;
+  t->peers[0]->connected = 1;
+  t->peers[0]->id = t->owner;
   t->mesh = h;
   t->tid = tid;
+  if (NULL != h->new_tunnel)
+  {
+    atsi.type = 0;
+    atsi.value = 0;
+    t->ctx = h->new_tunnel (h->cls, t, &msg->peer, &atsi);
+  }
+  return;
+}
+
+
+/**
+ * Process the tunnel destroy notification and free associated resources
+ *
+ * @param h     The mesh handle
+ * @param msg   A message with the details of the tunnel being destroyed
+ */
+static void
+process_tunnel_destroy (struct GNUNET_MESH_Handle *h,
+                        const struct GNUNET_MESH_TunnelMessage *msg)
+{
+  struct GNUNET_MESH_Tunnel *t;
+  MESH_TunnelNumber tid;
+
+  tid = ntohl (msg->tunnel_id);
+  t = retrieve_tunnel (h, tid);
+
+  if (NULL == t)
+  {
+    return;
+  }
+  if (0 == t->owner)
+  {
+    GNUNET_break (0);
+  }
+
+  destroy_tunnel (t);
   return;
 }
 
@@ -445,42 +819,57 @@ process_peer_event (struct GNUNET_MESH_Handle *h,
                     const struct GNUNET_MESH_PeerControl *msg)
 {
   struct GNUNET_MESH_Tunnel *t;
+  struct GNUNET_MESH_Peer *p;
+  struct GNUNET_ATS_Information atsi;
+  GNUNET_PEER_Id id;
   uint16_t size;
 
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: processig peer event\n");
   size = ntohs (msg->header.size);
   if (size != sizeof (struct GNUNET_MESH_PeerControl))
   {
-    GNUNET_break_op (0);
+    GNUNET_break (0);
     return;
   }
   t = retrieve_tunnel (h, ntohl (msg->tunnel_id));
   if (NULL == t)
   {
-    GNUNET_break_op (0);
+    GNUNET_break (0);
     return;
   }
-  if (GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_CONNECTED == msg->header.type)
+  id = GNUNET_PEER_search (&msg->peer);
+  if ((p = retrieve_peer (t, id)) == NULL)
+    p = add_peer_to_tunnel (t, &msg->peer);
+  if (GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_ADD == ntohs (msg->header.type))
   {
+    LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: adding peer\n");
     if (NULL != t->connect_handler)
     {
-      t->connect_handler (t->cls, &msg->peer, NULL);    /* FIXME atsi */
+      atsi.type = 0;
+      atsi.value = 0;
+      t->connect_handler (t->cls, &msg->peer, &atsi);
     }
+    p->connected = 1;
   }
   else
   {
-    if (NULL != t->disconnect_handler)
+    LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: removing peer\n");
+    if (NULL != t->disconnect_handler && p->connected)
     {
       t->disconnect_handler (t->cls, &msg->peer);
     }
+    remove_peer_from_tunnel (p);
+    GNUNET_free (p);
   }
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: processing peer event END\n");
 }
 
 
 /**
  * Process the incoming data packets
  *
- * @param h     The mesh handle
- * @param msh   A message encapsulating the data
+ * @param h         The mesh handle
+ * @param message   A message encapsulating the data
  */
 static void
 process_incoming_data (struct GNUNET_MESH_Handle *h,
@@ -493,17 +882,23 @@ process_incoming_data (struct GNUNET_MESH_Handle *h,
   struct GNUNET_MESH_Multicast *mcast;
   struct GNUNET_MESH_ToOrigin *to_orig;
   struct GNUNET_MESH_Tunnel *t;
+  unsigned int i;
   uint16_t type;
-  int i;
 
+  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh: Got a data message!\n");
   type = ntohs (message->type);
   switch (type)
   {
   case GNUNET_MESSAGE_TYPE_MESH_UNICAST:
     ucast = (struct GNUNET_MESH_Unicast *) message;
+
     t = retrieve_tunnel (h, ntohl (ucast->tid));
     payload = (struct GNUNET_MessageHeader *) &ucast[1];
     peer = &ucast->oid;
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "mesh:   on tunnel %s [%x]\n",
+                GNUNET_i2s (peer),
+                ntohl (ucast->tid));
     break;
   case GNUNET_MESSAGE_TYPE_MESH_MULTICAST:
     mcast = (struct GNUNET_MESH_Multicast *) message;
@@ -518,31 +913,39 @@ process_incoming_data (struct GNUNET_MESH_Handle *h,
     peer = &to_orig->sender;
     break;
   default:
-    GNUNET_break_op (0);
+    GNUNET_break (0);
     return;
   }
   if (NULL == t)
   {
-    GNUNET_break_op (0);
+    GNUNET_break (0);
     return;
   }
+  type = ntohs (payload->type);
   for (i = 0; i < h->n_handlers; i++)
   {
     handler = &h->message_handlers[i];
     if (handler->type == type)
     {
-      if (GNUNET_OK == handler->callback (h->cls, t, NULL,      /* FIXME ctx */
-                                          peer, payload, NULL))
-      {                         /* FIXME atsi */
-        GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
-                    "MESH: callback completed successfully\n");
+      struct GNUNET_ATS_Information atsi;
+
+      atsi.type = 0;
+      atsi.value = 0;
+      if (GNUNET_OK !=
+          handler->callback (h->cls, t, &t->ctx, peer, payload, &atsi))
+      {
+        LOG (GNUNET_ERROR_TYPE_DEBUG, "MESH: callback caused disconnection\n");
+        GNUNET_MESH_disconnect (h);
+        return;
       }
+#if MESH_API_DEBUG
       else
       {
-        GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
-                    "MESH: callback caused disconnection\n");
-        GNUNET_MESH_disconnect (h);
+        LOG (GNUNET_ERROR_TYPE_DEBUG,
+             "MESH: callback completed successfully\n");
+
       }
+#endif
     }
   }
 }
@@ -559,24 +962,26 @@ msg_received (void *cls, const struct GNUNET_MessageHeader *msg)
 {
   struct GNUNET_MESH_Handle *h = cls;
 
-  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh: received a message from MESH\n");
   if (msg == NULL)
   {
-    GNUNET_break (0);
-    h->in_receive = GNUNET_NO;
-    // rather: do_reconnect () -- and set 'in_receive' to NO there...
-    // FIXME: service disconnect, handle!
+    reconnect (h);
     return;
   }
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: received a message type %hu from MESH\n",
+       ntohs (msg->type));
   switch (ntohs (msg->type))
   {
     /* Notify of a new incoming tunnel */
   case GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_CREATE:
-    process_tunnel_create (h, (struct GNUNET_MESH_TunnelMessage *) msg);
+    process_tunnel_created (h, (struct GNUNET_MESH_TunnelNotification *) msg);
+    break;
+    /* Notify of a tunnel disconnection */
+  case GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_DESTROY:
+    process_tunnel_destroy (h, (struct GNUNET_MESH_TunnelMessage *) msg);
     break;
     /* Notify of a new peer or a peer disconnect in the tunnel */
-  case GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_CONNECTED:
-  case GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_DISCONNECTED:
+  case GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_ADD:
+  case GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_DEL:
     process_peer_event (h, (struct GNUNET_MESH_PeerControl *) msg);
     break;
     /* Notify of a new data packet in the tunnel */
@@ -587,11 +992,11 @@ msg_received (void *cls, const struct GNUNET_MessageHeader *msg)
     break;
     /* We shouldn't get any other packages, log and ignore */
   default:
-    GNUNET_log (GNUNET_ERROR_TYPE_WARNING,
-                "MESH: unsolicited message form service (type %d)\n",
-                ntohs (msg->type));
+    LOG (GNUNET_ERROR_TYPE_WARNING,
+         "MESH: unsolicited message form service (type %d)\n",
+         ntohs (msg->type));
   }
-  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh: message processed\n");
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: message processed\n");
   GNUNET_CLIENT_receive (h->client, &msg_received, h,
                          GNUNET_TIME_UNIT_FOREVER_REL);
 }
@@ -612,71 +1017,104 @@ msg_received (void *cls, const struct GNUNET_MessageHeader *msg)
  * @return number of bytes written to buf
  */
 static size_t
-send_raw (void *cls, size_t size, void *buf)
+send_callback (void *cls, size_t size, void *buf)
 {
   struct GNUNET_MESH_Handle *h = cls;
   struct GNUNET_MESH_TransmitHandle *th;
   char *cbuf = buf;
-  size_t ret;
+  size_t tsize;
   size_t psize;
 
-  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh: Send packet() Buffer %u\n", size);
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: Send packet() Buffer %u\n", size);
   h->th = NULL;
   if ((0 == size) || (NULL == buf))
   {
-    // FIXME: disconnect, reconnect, retry?
-    // do_reconnect ();
+    reconnect (h);
     return 0;
   }
-  ret = 0;
+  tsize = 0;
   while ((NULL != (th = h->th_head)) && (size >= th->size))
   {
-    GNUNET_log_from (GNUNET_ERROR_TYPE_DEBUG, "mesh-api", "type: %u\n",
-                     ntohs (th->data->type));
-    if (NULL == th->data)
+    if (NULL != th->notify)
     {
-      GNUNET_assert (NULL != th->notify);
-      if (th->target == 0)
+      if (th->tunnel->tid >= GNUNET_MESH_LOCAL_TUNNEL_ID_SERV)
+      {
+        /* traffic to origin */
+        struct GNUNET_MESH_ToOrigin to;
+        struct GNUNET_MessageHeader *mh;
+
+        GNUNET_assert (size >= th->size);
+        mh = (struct GNUNET_MessageHeader *) &cbuf[sizeof (to)];
+        psize =
+            th->notify (th->notify_cls, size - sizeof (to), mh);
+        GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+              "mesh:   to origin, type %u\n",
+              ntohs (mh->type));
+        if (psize > 0)
+        {
+          psize += sizeof (to);
+          GNUNET_assert (size >= psize);
+          to.header.size = htons (psize);
+          to.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_TO_ORIGIN);
+          to.tid = htonl (th->tunnel->tid);
+          memset (&to.oid, 0, sizeof (struct GNUNET_PeerIdentity));
+          memset (&to.sender, 0, sizeof (struct GNUNET_PeerIdentity));
+          memcpy (cbuf, &to, sizeof (to));
+        }
+      }
+      else if (th->target == 0)
       {
         /* multicast */
         struct GNUNET_MESH_Multicast mc;
+        struct GNUNET_MessageHeader *mh;
 
-        GNUNET_assert (size >= sizeof (mc) + th->size);
+        GNUNET_assert (size >= th->size);
+        mh = (struct GNUNET_MessageHeader *) &cbuf[sizeof (mc)];
         psize =
-            th->notify (th->notify_cls, size - sizeof (mc), &cbuf[sizeof (mc)]);
+            th->notify (th->notify_cls, size - sizeof (mc), mh);
+        GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                    "mesh:   multicast, type %u\n",
+                    ntohs (mh->type));
         if (psize > 0)
         {
-          mc.header.size = htons (sizeof (mc) + th->size);
+          psize += sizeof (mc);
+          GNUNET_assert (size >= psize);
+          mc.header.size = htons (psize);
           mc.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_MULTICAST);
           mc.tid = htonl (th->tunnel->tid);
-          memset (&mc.oid, 0, sizeof (struct GNUNET_PeerIdentity));     /* myself */
+          memset (&mc.oid, 0, sizeof (struct GNUNET_PeerIdentity));
           memcpy (cbuf, &mc, sizeof (mc));
-          psize = th->size + sizeof (mc);
         }
       }
       else
       {
         /* unicast */
         struct GNUNET_MESH_Unicast uc;
+        struct GNUNET_MessageHeader *mh;
 
-        GNUNET_assert (size >= sizeof (uc) + th->size);
+        GNUNET_assert (size >= th->size);
+        mh = (struct GNUNET_MessageHeader *) &cbuf[sizeof (uc)];
         psize =
-            th->notify (th->notify_cls, size - sizeof (uc), &cbuf[sizeof (uc)]);
+            th->notify (th->notify_cls, size - sizeof (uc), mh);
+        GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+              "mesh:   unicast, type %u\n",
+              ntohs (mh->type));
         if (psize > 0)
         {
-          uc.header.size = htons (sizeof (uc) + th->size);
+          psize += sizeof (uc);
+          GNUNET_assert (size >= psize);
+          uc.header.size = htons (psize);
           uc.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_UNICAST);
           uc.tid = htonl (th->tunnel->tid);
-          memset (&uc.oid, 0, sizeof (struct GNUNET_PeerIdentity));     /* myself */
+          memset (&uc.oid, 0, sizeof (struct GNUNET_PeerIdentity));
           GNUNET_PEER_resolve (th->target, &uc.destination);
           memcpy (cbuf, &uc, sizeof (uc));
-          psize = th->size + sizeof (uc);
         }
       }
     }
     else
     {
-      memcpy (cbuf, th->data, th->size);
+      memcpy (cbuf, &th[1], th->size);
       psize = th->size;
     }
     if (th->timeout_task != GNUNET_SCHEDULER_NO_TASK)
@@ -685,25 +1123,25 @@ send_raw (void *cls, size_t size, void *buf)
     GNUNET_free (th);
     cbuf += psize;
     size -= psize;
-    ret += psize;
+    tsize += psize;
   }
-  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh:   size: %u\n", ret);
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh:   total size: %u\n", tsize);
   if (NULL != (th = h->th_head))
   {
-    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh:   next size: %u\n", th->size);
+    LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh:   next size: %u\n", th->size);
     h->th =
         GNUNET_CLIENT_notify_transmit_ready (h->client, th->size,
                                              GNUNET_TIME_UNIT_FOREVER_REL,
-                                             GNUNET_YES, &send_raw, h);
+                                             GNUNET_YES, &send_callback, h);
   }
-  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh: Send packet() END\n");
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: Send packet() END\n");
   if (GNUNET_NO == h->in_receive)
   {
     h->in_receive = GNUNET_YES;
     GNUNET_CLIENT_receive (h->client, &msg_received, h,
                            GNUNET_TIME_UNIT_FOREVER_REL);
   }
-  return ret;
+  return tsize;
 }
 
 
@@ -726,7 +1164,6 @@ send_packet (struct GNUNET_MESH_Handle *h,
   th->priority = UINT32_MAX;
   th->timeout = GNUNET_TIME_UNIT_FOREVER_ABS;
   th->size = msize;
-  th->data = (void *) &th[1];
   memcpy (&th[1], msg, msize);
   add_to_queue (h, th);
   if (NULL != h->th)
@@ -734,7 +1171,7 @@ send_packet (struct GNUNET_MESH_Handle *h,
   h->th =
       GNUNET_CLIENT_notify_transmit_ready (h->client, msize,
                                            GNUNET_TIME_UNIT_FOREVER_REL,
-                                           GNUNET_YES, &send_raw, h);
+                                           GNUNET_YES, &send_callback, h);
 }
 
 
@@ -746,38 +1183,36 @@ send_packet (struct GNUNET_MESH_Handle *h,
  * Connect to the mesh service.
  *
  * @param cfg configuration to use
- * @param cls closure for the various callbacks that follow
- *            (including handlers in the handlers array)
  * @param queue_size size of the data message queue, shared among all tunnels
  *                   (each tunnel is guaranteed to accept at least one message,
  *                    no matter what is the status of other tunnels)
+ * @param cls closure for the various callbacks that follow
+ *            (including handlers in the handlers array)
+ * @param new_tunnel function called when an *inbound* tunnel is created
  * @param cleaner function called when an *inbound* tunnel is destroyed
  * @param handlers callbacks for messages we care about, NULL-terminated
- *                 note that the mesh is allowed to drop notifications about
- *                 inbound messages if the client does not process them fast
- *                 enough (for this notification type, a bounded queue is used)
- * @param stypes Application Types the client claims to offer
- * @return handle to the mesh service
- *         NULL on error (in this case, init is never called)
+ *                note that the mesh is allowed to drop notifications about
+ *                inbound messages if the client does not process them fast
+ *                enough (for this notification type, a bounded queue is used)
+ * @param stypes list of the applications that this client claims to provide
+ * @return handle to the mesh service NULL on error
+ *         (in this case, init is never called)
  */
 struct GNUNET_MESH_Handle *
 GNUNET_MESH_connect (const struct GNUNET_CONFIGURATION_Handle *cfg,
                      unsigned int queue_size, void *cls,
+                     GNUNET_MESH_InboundTunnelNotificationHandler new_tunnel,
                      GNUNET_MESH_TunnelEndHandler cleaner,
                      const struct GNUNET_MESH_MessageHandler *handlers,
                      const GNUNET_MESH_ApplicationType *stypes)
 {
   struct GNUNET_MESH_Handle *h;
-  struct GNUNET_MESH_ClientConnect *msg;
-  GNUNET_MESH_ApplicationType *apps;
-  uint16_t napps;
-  uint16_t *types;
-  uint16_t ntypes;
-  size_t size;
 
-  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh: GNUNET_MESH_connect()\n");
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: GNUNET_MESH_connect()\n");
   h = GNUNET_malloc (sizeof (struct GNUNET_MESH_Handle));
+  h->cfg = cfg;
   h->max_queue_size = queue_size;
+  h->new_tunnel = new_tunnel;
   h->cleaner = cleaner;
   h->client = GNUNET_CLIENT_connect ("mesh", cfg);
   if (h->client == NULL)
@@ -787,38 +1222,17 @@ GNUNET_MESH_connect (const struct GNUNET_CONFIGURATION_Handle *cfg,
     return NULL;
   }
   h->cls = cls;
-  h->message_handlers = handlers;
+  /* FIXME memdup? */
   h->applications = stypes;
-  h->next_tid = GNUNET_MESH_LOCAL_TUNNEL_ID_MARK;
+  h->message_handlers = handlers;
+  h->next_tid = GNUNET_MESH_LOCAL_TUNNEL_ID_CLI;
+  h->reconnect_time = GNUNET_TIME_UNIT_MILLISECONDS;
 
   /* count handlers and apps, calculate size */
-  for (h->n_handlers = 0; handlers[h->n_handlers].type; h->n_handlers++) ;
   for (h->n_applications = 0; stypes[h->n_applications]; h->n_applications++) ;
-  size = sizeof (struct GNUNET_MESH_ClientConnect);
-  size += h->n_handlers * sizeof (uint16_t);
-  size += h->n_applications * sizeof (GNUNET_MESH_ApplicationType);
-
-  {
-    char buf[size];
-
-    /* build connection packet */
-    msg = (struct GNUNET_MESH_ClientConnect *) buf;
-    msg->header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT);
-    msg->header.size = htons (size);
-    types = (uint16_t *) & msg[1];
-    for (ntypes = 0; ntypes < h->n_handlers; ntypes++)
-      types[ntypes] = h->message_handlers[ntypes].type;
-    apps = (GNUNET_MESH_ApplicationType *) &types[ntypes];
-    for (napps = 0; napps < h->n_applications; napps++)
-      apps[napps] = h->applications[napps];
-    msg->applications = htons (napps);
-    msg->types = htons (ntypes);
-    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
-                "mesh: Sending %lu bytes long message %d types and %d apps\n",
-                ntohs (msg->header.size), ntypes, napps);
-    send_packet (h, &msg->header);
-  }
-  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh: GNUNET_MESH_connect() END\n");
+  for (h->n_handlers = 0; handlers[h->n_handlers].type; h->n_handlers++) ;
+  send_connect (h);
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: GNUNET_MESH_connect() END\n");
   return h;
 }
 
@@ -831,6 +1245,16 @@ GNUNET_MESH_connect (const struct GNUNET_CONFIGURATION_Handle *cfg,
 void
 GNUNET_MESH_disconnect (struct GNUNET_MESH_Handle *handle)
 {
+  struct GNUNET_MESH_Tunnel *t;
+  struct GNUNET_MESH_Tunnel *aux;
+
+  t = handle->tunnels_head;
+  while (NULL != t)
+  {
+    aux = t->next;
+    destroy_tunnel (t);
+    t = aux;
+  }
   if (NULL != handle->th)
   {
     GNUNET_CLIENT_notify_transmit_ready_cancel (handle->th);
@@ -848,27 +1272,26 @@ GNUNET_MESH_disconnect (struct GNUNET_MESH_Handle *handle)
  * and to broadcast).
  *
  * @param h mesh handle
+ * @param tunnel_ctx client's tunnel context to associate with the tunnel
  * @param connect_handler function to call when peers are actually connected
  * @param disconnect_handler function to call when peers are disconnected
  * @param handler_cls closure for connect/disconnect handlers
  */
 struct GNUNET_MESH_Tunnel *
-GNUNET_MESH_tunnel_create (struct GNUNET_MESH_Handle *h,
-                           GNUNET_MESH_TunnelConnectHandler connect_handler,
-                           GNUNET_MESH_TunnelDisconnectHandler
-                           disconnect_handler, void *handler_cls)
+GNUNET_MESH_tunnel_create (struct GNUNET_MESH_Handle *h, void *tunnel_ctx,
+                           GNUNET_MESH_PeerConnectHandler connect_handler,
+                           GNUNET_MESH_PeerDisconnectHandler disconnect_handler,
+                           void *handler_cls)
 {
   struct GNUNET_MESH_Tunnel *t;
   struct GNUNET_MESH_TunnelMessage msg;
 
-  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh: Creating new tunnel\n");
-  t = GNUNET_malloc (sizeof (struct GNUNET_MESH_Tunnel));
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: Creating new tunnel\n");
+  t = create_tunnel (h, 0);
   t->connect_handler = connect_handler;
   t->disconnect_handler = disconnect_handler;
   t->cls = handler_cls;
-  t->mesh = h;
-  t->tid = h->next_tid++;
-  h->next_tid |= GNUNET_MESH_LOCAL_TUNNEL_ID_MARK;      // keep in range
+  t->ctx = tunnel_ctx;
   msg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_CREATE);
   msg.header.size = htons (sizeof (struct GNUNET_MESH_TunnelMessage));
   msg.tunnel_id = htonl (t->tid);
@@ -883,18 +1306,21 @@ GNUNET_MESH_tunnel_create (struct GNUNET_MESH_Handle *h,
  * @param tun tunnel handle
  */
 void
-GNUNET_MESH_tunnel_destroy (struct GNUNET_MESH_Tunnel *tun)
+GNUNET_MESH_tunnel_destroy (struct GNUNET_MESH_Tunnel *tunnel)
 {
   struct GNUNET_MESH_Handle *h;
   struct GNUNET_MESH_TunnelMessage msg;
 
-  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG, "mesh: Destroying tunnel\n");
-  h = tun->mesh;
+  LOG (GNUNET_ERROR_TYPE_DEBUG, "mesh: Destroying tunnel\n");
+  h = tunnel->mesh;
+
+  if (0 != tunnel->owner)
+    GNUNET_PEER_change_rc (tunnel->owner, -1);
 
   msg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_TUNNEL_DESTROY);
   msg.header.size = htons (sizeof (struct GNUNET_MESH_TunnelMessage));
-  msg.tunnel_id = htonl (tun->tid);
-  GNUNET_free (tun);
+  msg.tunnel_id = htonl (tunnel->tid);
+  destroy_tunnel (tunnel);
   send_packet (h, &msg.header);
 }
 
@@ -903,19 +1329,16 @@ GNUNET_MESH_tunnel_destroy (struct GNUNET_MESH_Tunnel *tun)
  * Request that a peer should be added to the tunnel.  The existing
  * connect handler will be called ONCE with either success or failure.
  * This function should NOT be called again with the same peer before the
- * connect handler is called
+ * connect handler is called.
  *
  * @param tunnel handle to existing tunnel
- * @param timeout how long to try to establish a connection
  * @param peer peer to add
  */
 void
 GNUNET_MESH_peer_request_connect_add (struct GNUNET_MESH_Tunnel *tunnel,
-                                      struct GNUNET_TIME_Relative timeout,
                                       const struct GNUNET_PeerIdentity *peer)
 {
   struct GNUNET_MESH_PeerControl msg;
-  struct GNUNET_MESH_Peer *p;
   GNUNET_PEER_Id peer_id;
   unsigned int i;
 
@@ -924,24 +1347,21 @@ GNUNET_MESH_peer_request_connect_add (struct GNUNET_MESH_Tunnel *tunnel,
   {
     if (tunnel->peers[i]->id == peer_id)
     {
+      /* Peer already exists in tunnel */
       GNUNET_PEER_change_rc (peer_id, -1);
-      /* FIXME: peer was already in the tunnel */
+      GNUNET_break (0);
       return;
     }
   }
-  p = GNUNET_malloc (sizeof (struct GNUNET_MESH_Peer));
-  p->id = peer_id;
-  p->t = tunnel;
-  p->cancel = GNUNET_SCHEDULER_add_delayed (timeout, &timeout_peer_request, p);
-  GNUNET_array_append (tunnel->peers, tunnel->npeers, p);
+  if (NULL == add_peer_to_tunnel (tunnel, peer))
+    return;
 
   msg.header.size = htons (sizeof (struct GNUNET_MESH_PeerControl));
-  msg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT_PEER_ADD);
+  msg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_ADD);
   msg.tunnel_id = htonl (tunnel->tid);
   msg.peer = *peer;
   send_packet (tunnel->mesh, &msg.header);
-//   tunnel->connect_handler (tunnel->cls, peer, NULL); FIXME call this later
-//   TODO: remember timeout
+
   return;
 }
 
@@ -976,14 +1396,14 @@ GNUNET_MESH_peer_request_connect_del (struct GNUNET_MESH_Tunnel *tunnel,
     return;
   }
   if (NULL != tunnel->disconnect_handler && tunnel->peers[i]->connected == 1)
-      tunnel->disconnect_handler (tunnel->cls, peer);
+    tunnel->disconnect_handler (tunnel->cls, peer);
   GNUNET_PEER_change_rc (peer_id, -1);
   GNUNET_free (tunnel->peers[i]);
   tunnel->peers[i] = tunnel->peers[tunnel->npeers - 1];
   GNUNET_array_grow (tunnel->peers, tunnel->npeers, tunnel->npeers - 1);
 
   msg.header.size = htons (sizeof (struct GNUNET_MESH_PeerControl));
-  msg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT_PEER_DEL);
+  msg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_DEL);
   msg.tunnel_id = htonl (tunnel->tid);
   memcpy (&msg.peer, peer, sizeof (struct GNUNET_PeerIdentity));
   send_packet (tunnel->mesh, &msg.header);
@@ -995,20 +1415,19 @@ GNUNET_MESH_peer_request_connect_del (struct GNUNET_MESH_Tunnel *tunnel,
  * message type.
  *
  * @param tunnel handle to existing tunnel
- * @param timeout how long to try to establish a connection
  * @param app_type application type that must be supported by the peer (MESH
  *                 should discover peer in proximity handling this type)
  */
 void
 GNUNET_MESH_peer_request_connect_by_type (struct GNUNET_MESH_Tunnel *tunnel,
-                                          struct GNUNET_TIME_Relative timeout,
                                           GNUNET_MESH_ApplicationType app_type)
 {
   struct GNUNET_MESH_ConnectPeerByType msg;
 
-  /* FIXME: remember request connect by type for reconnect! */
+  GNUNET_array_append (tunnel->apps, tunnel->napps, app_type);
+
   msg.header.size = htons (sizeof (struct GNUNET_MESH_ConnectPeerByType));
-  msg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_CONNECT_PEER_BY_TYPE);
+  msg.header.type = htons (GNUNET_MESSAGE_TYPE_MESH_LOCAL_PEER_ADD_BY_TYPE);
   msg.tunnel_id = htonl (tunnel->tid);
   msg.type = htonl (app_type);
   send_packet (tunnel->mesh, &msg.header);
@@ -1047,11 +1466,53 @@ GNUNET_MESH_notify_transmit_ready (struct GNUNET_MESH_Tunnel *tunnel, int cork,
                                    void *notify_cls)
 {
   struct GNUNET_MESH_TransmitHandle *th;
+  struct GNUNET_MESH_TransmitHandle *least_priority_th;
+  uint32_t least_priority;
   size_t overhead;
 
+#if MESH_API_DEBUG
+  GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+              "mesh: mesh notify transmit ready called\n");
+  if (NULL != target)
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "mesh:     target %s\n",
+                GNUNET_i2s (target));
+  else
+    GNUNET_log (GNUNET_ERROR_TYPE_DEBUG,
+                "mesh:     target multicast\n");
+#endif
+  GNUNET_assert (NULL != notify);
   if (tunnel->mesh->npackets >= tunnel->mesh->max_queue_size &&
       tunnel->npackets > 0)
-    return NULL;                /* queue full */
+  {
+    /* queue full */
+    if (0 == priority)
+      return NULL;
+    th = tunnel->mesh->th_tail;
+    least_priority = priority;
+    least_priority_th = NULL;
+    while (NULL != th)
+    {
+      if (th->priority < least_priority && th->tunnel->npackets > 1)
+      {
+        least_priority_th = th;
+        least_priority = th->priority;
+      }
+      th = th->prev;
+    }
+    if (NULL == least_priority_th)
+      return NULL;
+    /* Can't be a control message */
+    GNUNET_assert (NULL != least_priority_th->notify);
+    least_priority_th->notify (notify_cls, 0, NULL);
+    least_priority_th->tunnel->npackets--;
+    tunnel->mesh->npackets--;
+    GNUNET_CONTAINER_DLL_remove (tunnel->mesh->th_head, tunnel->mesh->th_tail,
+                                 least_priority_th);
+    if (GNUNET_SCHEDULER_NO_TASK != least_priority_th->timeout_task)
+      GNUNET_SCHEDULER_cancel (least_priority_th->timeout_task);
+    GNUNET_free (least_priority_th);
+  }
   tunnel->npackets++;
   tunnel->mesh->npackets++;
   th = GNUNET_malloc (sizeof (struct GNUNET_MESH_TransmitHandle));
@@ -1059,14 +1520,25 @@ GNUNET_MESH_notify_transmit_ready (struct GNUNET_MESH_Tunnel *tunnel, int cork,
   th->priority = priority;
   th->timeout = GNUNET_TIME_relative_to_absolute (maxdelay);
   th->target = GNUNET_PEER_intern (target);
-  overhead =
-      (NULL ==
-       target) ? sizeof (struct GNUNET_MESH_Multicast) : sizeof (struct
-                                                                 GNUNET_MESH_Unicast);
+  if (tunnel->tid >= GNUNET_MESH_LOCAL_TUNNEL_ID_SERV)
+    overhead = sizeof (struct GNUNET_MESH_ToOrigin);
+  else if (NULL == target)
+    overhead = sizeof (struct GNUNET_MESH_Multicast);
+  else
+    overhead = sizeof (struct GNUNET_MESH_Unicast);
   th->size = notify_size + overhead;
   th->notify = notify;
   th->notify_cls = notify_cls;
   add_to_queue (tunnel->mesh, th);
+  if (NULL != tunnel->mesh->th)
+    return th;
+  tunnel->mesh->th =
+      GNUNET_CLIENT_notify_transmit_ready (tunnel->mesh->client,
+                                           th->size,
+                                           GNUNET_TIME_UNIT_FOREVER_REL,
+                                           GNUNET_YES,
+                                           &send_callback,
+                                           tunnel->mesh);
   return th;
 }