Added preliminary Symbian support.

[silc.git] / lib / silccore / silcpacket.c

diff --git a/lib/silccore/silcpacket.c b/lib/silccore/silcpacket.c
index e2bf31258f997f8cfe85df825dba98a6e1d59c0e..6de9eb089a91860680de499226a1a3189e2077bd 100644 (file)
--- a/lib/silccore/silcpacket.c
+++ b/lib/silccore/silcpacket.c
@@ -25,24 +25,35 @@
 
 /************************** Types and definitions ***************************/
 
 
 /************************** Types and definitions ***************************/
 

+/* Per scheduler (which usually means per thread) data.  We put per scheduler
+   data here for accessing without locking.  SILC Schedule dictates that
+   tasks are dispatched in one thread, hence the per scheduler context. */
+typedef struct {
+  SilcSchedule schedule;                /* The scheduler */
+  SilcPacketEngine engine;              /* Packet engine */
+  SilcBufferStruct inbuf;               /* Data input buffer */
+  SilcUInt32 stream_count;              /* Number of streams using this */
+} *SilcPacketEngineContext;
+

 /* Packet engine */
 struct SilcPacketEngineStruct {
 /* Packet engine */
 struct SilcPacketEngineStruct {

+  SilcMutex lock;                       /* Engine lock */

   SilcRng rng;                          /* RNG for engine */
   SilcRng rng;                          /* RNG for engine */

+  SilcHashTable contexts;               /* Per scheduler contexts */

   SilcPacketCallbacks *callbacks;       /* Packet callbacks */
   void *callback_context;               /* Context for callbacks */
   SilcList streams;                     /* All streams in engine */
   SilcList packet_pool;                 /* Free list for received packets */
   SilcPacketCallbacks *callbacks;       /* Packet callbacks */
   void *callback_context;               /* Context for callbacks */
   SilcList streams;                     /* All streams in engine */
   SilcList packet_pool;                 /* Free list for received packets */

-  SilcMutex lock;                       /* Engine lock */

   SilcHashTable udp_remote;             /* UDP remote streams, or NULL */
   SilcHashTable udp_remote;             /* UDP remote streams, or NULL */

-  SilcBool local_is_router;
+  unsigned int local_is_router    : 1;

 };
 
 /* Packet processor context */
 typedef struct SilcPacketProcessStruct {
 };
 
 /* Packet processor context */
 typedef struct SilcPacketProcessStruct {

-  SilcInt32 priority;                   /* Priority */

   SilcPacketType *types;                /* Packets to process */
   SilcPacketCallbacks *callbacks;       /* Callbacks or NULL */
   void *callback_context;
   SilcPacketType *types;                /* Packets to process */
   SilcPacketCallbacks *callbacks;       /* Callbacks or NULL */
   void *callback_context;

+  SilcInt32 priority;                   /* Priority */

 } *SilcPacketProcess;
 
 /* UDP remote stream tuple */
 } *SilcPacketProcess;
 
 /* UDP remote stream tuple */


@@ -54,13 +65,12 @@ typedef struct {
 /* Packet stream */
 struct SilcPacketStreamStruct {
   struct SilcPacketStreamStruct *next;
 /* Packet stream */
 struct SilcPacketStreamStruct {
   struct SilcPacketStreamStruct *next;

-  SilcPacketEngine engine;              /* Packet engine */
+  SilcPacketEngineContext sc;           /* Per scheduler context */

   SilcStream stream;                    /* Underlaying stream */
   SilcMutex lock;                       /* Stream lock */
   SilcDList process;                    /* Packet processors, or NULL */
   SilcPacketRemoteUDP remote_udp;       /* UDP remote stream tuple, or NULL */
   void *stream_context;                         /* Stream context */
   SilcStream stream;                    /* Underlaying stream */
   SilcMutex lock;                       /* Stream lock */
   SilcDList process;                    /* Packet processors, or NULL */
   SilcPacketRemoteUDP remote_udp;       /* UDP remote stream tuple, or NULL */
   void *stream_context;                         /* Stream context */

-  SilcBufferStruct inbuf;               /* In buffer */

   SilcBufferStruct outbuf;              /* Out buffer */
   SilcCipher send_key[2];               /* Sending key */
   SilcHmac send_hmac[2];                /* Sending HMAC */
   SilcBufferStruct outbuf;              /* Out buffer */
   SilcCipher send_key[2];               /* Sending key */
   SilcHmac send_hmac[2];                /* Sending HMAC */


@@ -138,21 +148,34 @@ do {                                                                         \
 /* EOS callback */
 #define SILC_PACKET_CALLBACK_EOS(s)                                    \
 do {                                                                   \
 /* EOS callback */
 #define SILC_PACKET_CALLBACK_EOS(s)                                    \
 do {                                                                   \

-  (s)->engine->callbacks->eos((s)->engine, s,                          \
-                             (s)->engine->callback_context,            \
-                             (s)->stream_context);                     \
+  (s)->sc->engine->callbacks->eos((s)->sc->engine, s,                  \
+                                 (s)->sc->engine->callback_context,    \
+                                 (s)->stream_context);                 \

 } while(0)
 
 /* Error callback */
 #define SILC_PACKET_CALLBACK_ERROR(s, err)                             \
 do {                                                                   \
 } while(0)
 
 /* Error callback */
 #define SILC_PACKET_CALLBACK_ERROR(s, err)                             \
 do {                                                                   \

-  (s)->engine->callbacks->error((s)->engine, s, err,                   \
-                               (s)->engine->callback_context,          \
-                               (s)->stream_context);                   \
+  (s)->sc->engine->callbacks->error((s)->sc->engine, s, err,           \
+                                   (s)->sc->engine->callback_context,  \
+                                   (s)->stream_context);               \

 } while(0)
 
 } while(0)
 

-static void silc_packet_dispatch(SilcPacket packet);
+static SilcBool silc_packet_dispatch(SilcPacket packet);

 static void silc_packet_read_process(SilcPacketStream stream);
 static void silc_packet_read_process(SilcPacketStream stream);

+static inline SilcBool silc_packet_send_raw(SilcPacketStream stream,
+                                           SilcPacketType type,
+                                           SilcPacketFlags flags,
+                                           SilcIdType src_id_type,
+                                           unsigned char *src_id,
+                                           SilcUInt32 src_id_len,
+                                           SilcIdType dst_id_type,
+                                           unsigned char *dst_id,
+                                           SilcUInt32 dst_id_len,
+                                           const unsigned char *data,
+                                           SilcUInt32 data_len,
+                                           SilcCipher cipher,
+                                           SilcHmac hmac);

 
 /************************ Static utility functions **************************/
 
 
 /************************ Static utility functions **************************/
 


@@ -166,14 +189,18 @@ SILC_TASK_CALLBACK(silc_packet_stream_inject_packet)
   SILC_LOG_DEBUG(("Injecting packet %p to stream %p", packet, packet->stream));
 
   silc_mutex_lock(stream->lock);
   SILC_LOG_DEBUG(("Injecting packet %p to stream %p", packet, packet->stream));
 
   silc_mutex_lock(stream->lock);

-  silc_packet_dispatch(packet);
+  if (!stream->destroyed)
+    silc_packet_dispatch(packet);

   silc_mutex_unlock(stream->lock);
   silc_mutex_unlock(stream->lock);

+  silc_packet_stream_unref(stream);

 }
 
 /* Write data to the stream.  Must be called with ps->lock locked.  Unlocks
 }
 
 /* Write data to the stream.  Must be called with ps->lock locked.  Unlocks

-   the lock inside this function. */
+   the lock inside this function, unless no_unlock is TRUE.  Unlocks always
+   in case it returns FALSE. */

 
 

-static inline SilcBool silc_packet_stream_write(SilcPacketStream ps)
+static inline SilcBool silc_packet_stream_write(SilcPacketStream ps,
+                                               SilcBool no_unlock)

 {
   SilcStream stream;
   SilcBool connected;
 {
   SilcStream stream;
   SilcBool connected;


@@ -191,17 +218,17 @@ static inline SilcBool silc_packet_stream_write(SilcPacketStream ps)
        i = silc_net_udp_send(stream, ps->remote_udp->remote_ip,
                              ps->remote_udp->remote_port,
                              ps->outbuf.data, silc_buffer_len(&ps->outbuf));
        i = silc_net_udp_send(stream, ps->remote_udp->remote_ip,
                              ps->remote_udp->remote_port,
                              ps->outbuf.data, silc_buffer_len(&ps->outbuf));

-       if (i == -2) {
+       if (silc_unlikely(i == -2)) {

          /* Error */
          silc_buffer_reset(&ps->outbuf);
          /* Error */
          silc_buffer_reset(&ps->outbuf);

-         silc_mutex_unlock(ps->lock);

          SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_WRITE);
          return FALSE;
        }
 
          SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_WRITE);
          return FALSE;
        }
 

-       if (i == -1) {
+       if (silc_unlikely(i == -1)) {

          /* Cannot write now, write later. */
          /* Cannot write now, write later. */

-         silc_mutex_unlock(ps->lock);
+         if (!no_unlock)
+           silc_mutex_unlock(ps->lock);

          return TRUE;
        }
 
          return TRUE;
        }
 


@@ -210,7 +237,8 @@ static inline SilcBool silc_packet_stream_write(SilcPacketStream ps)
       }
 
       silc_buffer_reset(&ps->outbuf);
       }
 
       silc_buffer_reset(&ps->outbuf);

-      silc_mutex_unlock(ps->lock);
+      if (!no_unlock)
+       silc_mutex_unlock(ps->lock);

 
       return TRUE;
     }
 
       return TRUE;
     }


@@ -220,7 +248,7 @@ static inline SilcBool silc_packet_stream_write(SilcPacketStream ps)
   while (silc_buffer_len(&ps->outbuf) > 0) {
     i = silc_stream_write(stream, ps->outbuf.data,
                          silc_buffer_len(&ps->outbuf));
   while (silc_buffer_len(&ps->outbuf) > 0) {
     i = silc_stream_write(stream, ps->outbuf.data,
                          silc_buffer_len(&ps->outbuf));

-    if (i == 0) {
+    if (silc_unlikely(i == 0)) {

       /* EOS */
       silc_buffer_reset(&ps->outbuf);
       silc_mutex_unlock(ps->lock);
       /* EOS */
       silc_buffer_reset(&ps->outbuf);
       silc_mutex_unlock(ps->lock);


@@ -228,7 +256,7 @@ static inline SilcBool silc_packet_stream_write(SilcPacketStream ps)
       return FALSE;
     }
 
       return FALSE;
     }
 

-    if (i == -2) {
+    if (silc_unlikely(i == -2)) {

       /* Error */
       silc_buffer_reset(&ps->outbuf);
       silc_mutex_unlock(ps->lock);
       /* Error */
       silc_buffer_reset(&ps->outbuf);
       silc_mutex_unlock(ps->lock);


@@ -236,9 +264,10 @@ static inline SilcBool silc_packet_stream_write(SilcPacketStream ps)
       return FALSE;
     }
 
       return FALSE;
     }
 

-    if (i == -1) {
+    if (silc_unlikely(i == -1)) {

       /* Cannot write now, write later. */
       /* Cannot write now, write later. */

-      silc_mutex_unlock(ps->lock);
+      if (!no_unlock)
+       silc_mutex_unlock(ps->lock);

       return TRUE;
     }
 
       return TRUE;
     }
 


@@ -247,12 +276,13 @@ static inline SilcBool silc_packet_stream_write(SilcPacketStream ps)
   }
 
   silc_buffer_reset(&ps->outbuf);
   }
 
   silc_buffer_reset(&ps->outbuf);

-  silc_mutex_unlock(ps->lock);
+  if (!no_unlock)
+    silc_mutex_unlock(ps->lock);

 
   return TRUE;
 }
 
 
   return TRUE;
 }
 

-/* Reads data from stream.  Must be called with the ps->lock locked.  If this
+/* Reads data from stream.  Must be called with ps->lock locked.  If this

    returns FALSE the lock has been unlocked.  If this returns packet stream
    to `ret_ps' its lock has been acquired and `ps' lock has been unlocked.
    It is returned if the stream is UDP and remote UDP stream exists for
    returns FALSE the lock has been unlocked.  If this returns packet stream
    to `ret_ps' its lock has been acquired and `ps' lock has been unlocked.
    It is returned if the stream is UDP and remote UDP stream exists for


@@ -262,19 +292,12 @@ static inline SilcBool silc_packet_stream_read(SilcPacketStream ps,
                                               SilcPacketStream *ret_ps)
 {
   SilcStream stream;
                                               SilcPacketStream *ret_ps)
 {
   SilcStream stream;

+  SilcBuffer inbuf;

   SilcBool connected;
   int ret;
 
   stream = ps->stream;
   SilcBool connected;
   int ret;
 
   stream = ps->stream;

-
-  /* Make sure we have fair amount of free space in inbuf */
-  if (silc_buffer_taillen(&ps->inbuf) < SILC_PACKET_DEFAULT_SIZE)
-    if (!silc_buffer_realloc(&ps->inbuf, silc_buffer_truelen(&ps->inbuf) +
-                            SILC_PACKET_DEFAULT_SIZE * 2)) {
-      silc_mutex_unlock(ps->lock);
-      SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_NO_MEMORY);
-      return FALSE;
-    }
+  inbuf = &ps->sc->inbuf;

 
   if (silc_socket_stream_is_udp(stream, &connected)) {
     if (!connected) {
 
   if (silc_socket_stream_is_udp(stream, &connected)) {
     if (!connected) {


@@ -284,57 +307,42 @@ static inline SilcBool silc_packet_stream_read(SilcPacketStream ps,
       SilcPacketStream remote;
 
       ret = silc_net_udp_receive(stream, remote_ip, sizeof(remote_ip),
       SilcPacketStream remote;
 
       ret = silc_net_udp_receive(stream, remote_ip, sizeof(remote_ip),

-                                &remote_port, ps->inbuf.tail,
-                                silc_buffer_taillen(&ps->inbuf));
-      if (ret == -2) {
-       /* Error */
-       silc_buffer_reset(&ps->inbuf);
-       silc_mutex_unlock(ps->lock);
-       SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_READ);
-       return FALSE;
-      }
+                                &remote_port, inbuf->tail,
+                                silc_buffer_taillen(inbuf));

 
 

-      if (ret == -1) {
-       /* Cannot read now, do it later. */
-       silc_buffer_pull(&ps->inbuf, silc_buffer_len(&ps->inbuf));
+      if (silc_unlikely(ret < 0)) {

        silc_mutex_unlock(ps->lock);
        silc_mutex_unlock(ps->lock);

+       if (ret == -1) {
+         /* Cannot read now, do it later. */
+         silc_buffer_pull(inbuf, silc_buffer_len(inbuf));
+         return FALSE;
+       }
+
+       /* Error */
+       silc_buffer_reset(inbuf);
+       SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_READ);

        return FALSE;
       }
 
       /* See if remote packet stream exist for this sender */
        return FALSE;
       }
 
       /* See if remote packet stream exist for this sender */

-      snprintf(tuple, sizeof(tuple), "%d%s", remote_port, remote_ip);
-      silc_mutex_lock(ps->engine->lock);
-      if (silc_hash_table_find(ps->engine->udp_remote, tuple, NULL,
+      silc_snprintf(tuple, sizeof(tuple), "%d%s", remote_port, remote_ip);
+      silc_mutex_lock(ps->sc->engine->lock);
+      if (silc_hash_table_find(ps->sc->engine->udp_remote, tuple, NULL,

                               (void *)&remote)) {
                               (void *)&remote)) {

-       /* Found packet stream for this sender, copy the packet */
-       silc_mutex_unlock(ps->engine->lock);
-
-       SILC_LOG_DEBUG(("UDP packet from %s:%d for stream %p",
-                       remote_ip, remote_port, remote));
-
+       silc_mutex_unlock(ps->sc->engine->lock);
+       SILC_LOG_DEBUG(("UDP packet from %s:%d for stream %p", remote_ip,
+                       remote_port, remote));
+       silc_mutex_unlock(ps->lock);

        silc_mutex_lock(remote->lock);
        silc_mutex_lock(remote->lock);

-       if (ret > silc_buffer_taillen(&remote->inbuf))
-         if (!silc_buffer_realloc(&remote->inbuf, ret)) {
-           silc_mutex_unlock(remote->lock);
-           silc_mutex_unlock(ps->lock);
-           SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_NO_MEMORY);
-           return FALSE;
-         }
-
-       silc_buffer_put_tail(&remote->inbuf, ps->inbuf.tail, ret);
-       silc_buffer_pull_tail(&remote->inbuf, ret);

        *ret_ps = remote;
        *ret_ps = remote;

-
-       silc_buffer_reset(&ps->inbuf);
-       silc_mutex_unlock(ps->lock);

        return TRUE;
       }
        return TRUE;
       }

-      silc_mutex_unlock(ps->engine->lock);
+      silc_mutex_unlock(ps->sc->engine->lock);

 
       /* Unknown sender */
       if (!ps->remote_udp) {
        ps->remote_udp = silc_calloc(1, sizeof(*ps->remote_udp));
 
       /* Unknown sender */
       if (!ps->remote_udp) {
        ps->remote_udp = silc_calloc(1, sizeof(*ps->remote_udp));

-       if (!ps->remote_udp) {
+       if (silc_unlikely(!ps->remote_udp)) {

          silc_mutex_unlock(ps->lock);
          SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_NO_MEMORY);
          return FALSE;
          silc_mutex_unlock(ps->lock);
          SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_NO_MEMORY);
          return FALSE;


@@ -346,39 +354,35 @@ static inline SilcBool silc_packet_stream_read(SilcPacketStream ps,
       ps->remote_udp->remote_ip = strdup(remote_ip);
       ps->remote_udp->remote_port = remote_port;
 
       ps->remote_udp->remote_ip = strdup(remote_ip);
       ps->remote_udp->remote_port = remote_port;
 

-      silc_buffer_pull_tail(&ps->inbuf, ret);
+      silc_buffer_pull_tail(inbuf, ret);

       return TRUE;
     }
   }
 
   /* Read data from the stream */
       return TRUE;
     }
   }
 
   /* Read data from the stream */

-  ret = silc_stream_read(ps->stream, ps->inbuf.tail,
-                        silc_buffer_taillen(&ps->inbuf));
-
-  if (ret == 0) {
-    /* EOS */
-    silc_buffer_reset(&ps->inbuf);
+  ret = silc_stream_read(stream, inbuf->tail, silc_buffer_taillen(inbuf));
+  if (silc_unlikely(ret <= 0)) {

     silc_mutex_unlock(ps->lock);
     silc_mutex_unlock(ps->lock);

-    SILC_PACKET_CALLBACK_EOS(ps);
-    return FALSE;
-  }
+    if (ret == 0) {
+      /* EOS */
+      silc_buffer_reset(inbuf);
+      SILC_PACKET_CALLBACK_EOS(ps);
+      return FALSE;
+    }
+
+    if (ret == -1) {
+      /* Cannot read now, do it later. */
+      silc_buffer_pull(inbuf, silc_buffer_len(inbuf));
+      return FALSE;
+    }

 
 

-  if (ret == -2) {

     /* Error */
     /* Error */

-    silc_buffer_reset(&ps->inbuf);
-    silc_mutex_unlock(ps->lock);
+    silc_buffer_reset(inbuf);

     SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_READ);
     return FALSE;
   }
 
     SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_READ);
     return FALSE;
   }
 

-  if (ret == -1) {
-    /* Cannot read now, do it later. */
-    silc_buffer_pull(&ps->inbuf, silc_buffer_len(&ps->inbuf));
-    silc_mutex_unlock(ps->lock);
-    return FALSE;
-  }
-
-  silc_buffer_pull_tail(&ps->inbuf, ret);
+  silc_buffer_pull_tail(inbuf, ret);

   return TRUE;
 }
 
   return TRUE;
 }
 


@@ -391,25 +395,12 @@ static void silc_packet_stream_io(SilcStream stream, SilcStreamStatus status,
 
   silc_mutex_lock(ps->lock);
 
 
   silc_mutex_lock(ps->lock);
 

-  if (ps->destroyed) {
+  if (silc_unlikely(ps->destroyed)) {

     silc_mutex_unlock(ps->lock);
     return;
   }
 
   switch (status) {
     silc_mutex_unlock(ps->lock);
     return;
   }
 
   switch (status) {

-
-  case SILC_STREAM_CAN_WRITE:
-    SILC_LOG_DEBUG(("Writing pending data to stream"));
-
-    if (!silc_buffer_headlen(&ps->outbuf)) {
-      silc_mutex_unlock(ps->lock);
-      return;
-    }
-
-    /* Write pending data to stream */
-    silc_packet_stream_write(ps);
-    break;
-

   case SILC_STREAM_CAN_READ:
     SILC_LOG_DEBUG(("Reading data from stream"));
 
   case SILC_STREAM_CAN_READ:
     SILC_LOG_DEBUG(("Reading data from stream"));
 


@@ -418,6 +409,7 @@ static void silc_packet_stream_io(SilcStream stream, SilcStreamStatus status,
       return;
 
     /* Now process the data */
       return;
 
     /* Now process the data */

+    silc_packet_stream_ref(ps);

     if (!remote) {
       silc_packet_read_process(ps);
       silc_mutex_unlock(ps->lock);
     if (!remote) {
       silc_packet_read_process(ps);
       silc_mutex_unlock(ps->lock);


@@ -425,6 +417,19 @@ static void silc_packet_stream_io(SilcStream stream, SilcStreamStatus status,
       silc_packet_read_process(remote);
       silc_mutex_unlock(remote->lock);
     }
       silc_packet_read_process(remote);
       silc_mutex_unlock(remote->lock);
     }

+    silc_packet_stream_unref(ps);
+    break;
+
+  case SILC_STREAM_CAN_WRITE:
+    SILC_LOG_DEBUG(("Writing pending data to stream"));
+
+    if (silc_unlikely(!silc_buffer_headlen(&ps->outbuf))) {
+      silc_mutex_unlock(ps->lock);
+      return;
+    }
+
+    /* Write pending data to stream */
+    silc_packet_stream_write(ps, FALSE);

     break;
 
   default:
     break;
 
   default:


@@ -452,13 +457,13 @@ static SilcPacket silc_packet_alloc(SilcPacketEngine engine)
     silc_mutex_unlock(engine->lock);
 
     packet = silc_calloc(1, sizeof(*packet));
     silc_mutex_unlock(engine->lock);
 
     packet = silc_calloc(1, sizeof(*packet));

-    if (!packet)
+    if (silc_unlikely(!packet))

       return NULL;
 
     SILC_LOG_DEBUG(("Allocating new packet %p", packet));
 
     tmp = silc_malloc(SILC_PACKET_DEFAULT_SIZE);
       return NULL;
 
     SILC_LOG_DEBUG(("Allocating new packet %p", packet));
 
     tmp = silc_malloc(SILC_PACKET_DEFAULT_SIZE);

-    if (!tmp) {
+    if (silc_unlikely(!tmp)) {

       silc_free(packet);
       return NULL;
     }
       silc_free(packet);
       return NULL;
     }


@@ -486,6 +491,17 @@ static void silc_packet_engine_hash_destr(void *key, void *context,
   silc_free(key);
 }
 
   silc_free(key);
 }
 

+/* Per scheduler context hash table destructor */
+
+static void silc_packet_engine_context_destr(void *key, void *context,
+                                            void *user_context)
+{
+  SilcPacketEngineContext sc = context;
+  silc_buffer_clear(&sc->inbuf);
+  silc_buffer_purge(&sc->inbuf);
+  silc_free(sc);
+}
+

 
 /******************************** Packet API ********************************/
 
 
 /******************************** Packet API ********************************/
 


@@ -512,6 +528,14 @@ silc_packet_engine_start(SilcRng rng, SilcBool router,
   if (!engine)
     return NULL;
 
   if (!engine)
     return NULL;
 

+  engine->contexts = silc_hash_table_alloc(0, silc_hash_ptr, NULL, NULL, NULL,
+                                          silc_packet_engine_context_destr,
+                                          engine, TRUE);
+  if (!engine->contexts) {
+    silc_free(engine);
+    return NULL;
+  }
+

   engine->rng = rng;
   engine->local_is_router = router;
   engine->callbacks = callbacks;
   engine->rng = rng;
   engine->local_is_router = router;
   engine->callbacks = callbacks;


@@ -576,19 +600,11 @@ SilcPacketStream silc_packet_stream_create(SilcPacketEngine engine,
   if (!ps)
     return NULL;
 
   if (!ps)
     return NULL;
 

-  ps->engine = engine;

   ps->stream = stream;
   silc_atomic_init8(&ps->refcnt, 1);
   silc_mutex_alloc(&ps->lock);
 
   ps->stream = stream;
   silc_atomic_init8(&ps->refcnt, 1);
   silc_mutex_alloc(&ps->lock);
 

-  /* Allocate buffers */
-  tmp = silc_malloc(SILC_PACKET_DEFAULT_SIZE);
-  if (!tmp) {
-    silc_packet_stream_destroy(ps);
-    return NULL;
-  }
-  silc_buffer_set(&ps->inbuf, tmp, SILC_PACKET_DEFAULT_SIZE);
-  silc_buffer_reset(&ps->inbuf);
+  /* Allocate out buffer */

   tmp = silc_malloc(SILC_PACKET_DEFAULT_SIZE);
   if (!tmp) {
     silc_packet_stream_destroy(ps);
   tmp = silc_malloc(SILC_PACKET_DEFAULT_SIZE);
   if (!tmp) {
     silc_packet_stream_destroy(ps);


@@ -604,13 +620,46 @@ SilcPacketStream silc_packet_stream_create(SilcPacketEngine engine,
     return NULL;
   }
 
     return NULL;
   }
 

-  /* Set IO notifier callback */
-  silc_stream_set_notifier(ps->stream, schedule, silc_packet_stream_io, ps);
-
-  /* Add to engine */

   silc_mutex_lock(engine->lock);
   silc_mutex_lock(engine->lock);

+
+  /* Add per scheduler context */
+  if (!silc_hash_table_find(engine->contexts, schedule, NULL,
+                           (void *)&ps->sc)) {
+    ps->sc = silc_calloc(1, sizeof(*ps->sc));
+    if (!ps->sc) {
+      silc_packet_stream_destroy(ps);
+      silc_mutex_unlock(engine->lock);
+      return NULL;
+    }
+    ps->sc->engine = engine;
+    ps->sc->schedule = schedule;
+
+    /* Allocate data input buffer */
+    tmp = silc_malloc(SILC_PACKET_DEFAULT_SIZE * 31);
+    if (!tmp) {
+      silc_free(ps->sc);
+      ps->sc = NULL;
+      silc_packet_stream_destroy(ps);
+      silc_mutex_unlock(engine->lock);
+      return NULL;
+    }
+    silc_buffer_set(&ps->sc->inbuf, tmp, SILC_PACKET_DEFAULT_SIZE * 31);
+    silc_buffer_reset(&ps->sc->inbuf);
+
+    /* Add to per scheduler context hash table */
+    if (!silc_hash_table_add(engine->contexts, schedule, ps->sc)) {
+      silc_buffer_purge(&ps->sc->inbuf);
+      silc_free(ps->sc);
+      ps->sc = NULL;
+      silc_packet_stream_destroy(ps);
+      silc_mutex_unlock(engine->lock);
+      return NULL;
+    }
+  }
+  ps->sc->stream_count++;
+
+  /* Add the packet stream to engine */

   silc_list_add(engine->streams, ps);
   silc_list_add(engine->streams, ps);

-  silc_mutex_unlock(engine->lock);

 
   /* If this is UDP stream, allocate UDP remote stream hash table */
   if (!engine->udp_remote && silc_socket_stream_is_udp(stream, NULL))
 
   /* If this is UDP stream, allocate UDP remote stream hash table */
   if (!engine->udp_remote && silc_socket_stream_is_udp(stream, NULL))


@@ -618,6 +667,10 @@ SilcPacketStream silc_packet_stream_create(SilcPacketEngine engine,
                                               silc_hash_string_compare, NULL,
                                               silc_packet_engine_hash_destr,
                                               NULL, TRUE);
                                               silc_hash_string_compare, NULL,
                                               silc_packet_engine_hash_destr,
                                               NULL, TRUE);

+  silc_mutex_unlock(engine->lock);
+
+  /* Set IO notifier callback.  This schedules this stream for I/O. */
+  silc_stream_set_notifier(ps->stream, schedule, silc_packet_stream_io, ps);

 
   return ps;
 }
 
   return ps;
 }


@@ -629,7 +682,7 @@ SilcPacketStream silc_packet_stream_add_remote(SilcPacketStream stream,
                                               SilcUInt16 remote_port,
                                               SilcPacket packet)
 {
                                               SilcUInt16 remote_port,
                                               SilcPacket packet)
 {

-  SilcPacketEngine engine = stream->engine;
+  SilcPacketEngine engine = stream->sc->engine;

   SilcPacketStream ps;
   char *tuple;
   void *tmp;
   SilcPacketStream ps;
   char *tuple;
   void *tmp;


@@ -648,8 +701,8 @@ SilcPacketStream silc_packet_stream_add_remote(SilcPacketStream stream,
   ps = silc_calloc(1, sizeof(*ps));
   if (!ps)
     return NULL;
   ps = silc_calloc(1, sizeof(*ps));
   if (!ps)
     return NULL;

+  ps->sc = stream->sc;

 
 

-  ps->engine = engine;

   silc_atomic_init8(&ps->refcnt, 1);
   silc_mutex_alloc(&ps->lock);
 
   silc_atomic_init8(&ps->refcnt, 1);
   silc_mutex_alloc(&ps->lock);
 


@@ -658,14 +711,7 @@ SilcPacketStream silc_packet_stream_add_remote(SilcPacketStream stream,
   ps->stream = (SilcStream)stream;
   ps->udp = TRUE;
 
   ps->stream = (SilcStream)stream;
   ps->udp = TRUE;
 

-  /* Allocate buffers */
-  tmp = silc_malloc(SILC_PACKET_DEFAULT_SIZE);
-  if (!tmp) {
-    silc_packet_stream_destroy(ps);
-    return NULL;
-  }
-  silc_buffer_set(&ps->inbuf, tmp, SILC_PACKET_DEFAULT_SIZE);
-  silc_buffer_reset(&ps->inbuf);
+  /* Allocate out buffer */

   tmp = silc_malloc(SILC_PACKET_DEFAULT_SIZE);
   if (!tmp) {
     silc_packet_stream_destroy(ps);
   tmp = silc_malloc(SILC_PACKET_DEFAULT_SIZE);
   if (!tmp) {
     silc_packet_stream_destroy(ps);


@@ -707,6 +753,7 @@ SilcPacketStream silc_packet_stream_add_remote(SilcPacketStream stream,
   if (packet) {
     /* Inject packet to the new stream */
     packet->stream = ps;
   if (packet) {
     /* Inject packet to the new stream */
     packet->stream = ps;

+    silc_packet_stream_ref(ps);

     silc_schedule_task_add_timeout(silc_stream_get_schedule(stream->stream),
                                   silc_packet_stream_inject_packet, packet,
                                   0, 0);
     silc_schedule_task_add_timeout(silc_stream_get_schedule(stream->stream),
                                   silc_packet_stream_inject_packet, packet,
                                   0, 0);


@@ -731,9 +778,17 @@ void silc_packet_stream_destroy(SilcPacketStream stream)
 
   if (!stream->udp) {
     /* Delete from engine */
 
   if (!stream->udp) {
     /* Delete from engine */

-    silc_mutex_lock(stream->engine->lock);
-    silc_list_del(stream->engine->streams, stream);
-    silc_mutex_unlock(stream->engine->lock);
+    silc_mutex_lock(stream->sc->engine->lock);
+    silc_list_del(stream->sc->engine->streams, stream);
+
+    /* Remove per scheduler context, if it is not used anymore */
+    if (stream->sc) {
+      stream->sc->stream_count--;
+      if (!stream->sc->stream_count)
+       silc_hash_table_del(stream->sc->engine->contexts,
+                           stream->sc->schedule);
+    }
+    silc_mutex_unlock(stream->sc->engine->lock);

 
     /* Destroy the underlaying stream */
     if (stream->stream)
 
     /* Destroy the underlaying stream */
     if (stream->stream)


@@ -741,11 +796,11 @@ void silc_packet_stream_destroy(SilcPacketStream stream)
   } else {
     /* Delete from UDP remote hash table */
     char tuple[64];
   } else {
     /* Delete from UDP remote hash table */
     char tuple[64];

-    snprintf(tuple, sizeof(tuple), "%d%s", stream->remote_udp->remote_port,
+    silc_snprintf(tuple, sizeof(tuple), "%d%s", stream->remote_udp->remote_port,

             stream->remote_udp->remote_ip);
             stream->remote_udp->remote_ip);

-    silc_mutex_lock(stream->engine->lock);
-    silc_hash_table_del(stream->engine->udp_remote, tuple);
-    silc_mutex_unlock(stream->engine->lock);
+    silc_mutex_lock(stream->sc->engine->lock);
+    silc_hash_table_del(stream->sc->engine->udp_remote, tuple);
+    silc_mutex_unlock(stream->sc->engine->lock);

 
     silc_free(stream->remote_udp->remote_ip);
     silc_free(stream->remote_udp);
 
     silc_free(stream->remote_udp->remote_ip);
     silc_free(stream->remote_udp);


@@ -755,15 +810,23 @@ void silc_packet_stream_destroy(SilcPacketStream stream)
   }
 
   /* Clear and free buffers */
   }
 
   /* Clear and free buffers */

-  silc_buffer_clear(&stream->inbuf);

   silc_buffer_clear(&stream->outbuf);
   silc_buffer_clear(&stream->outbuf);

-  silc_buffer_purge(&stream->inbuf);

   silc_buffer_purge(&stream->outbuf);
 
   silc_buffer_purge(&stream->outbuf);
 

+  if (stream->process) {
+    SilcPacketProcess p;
+    silc_dlist_start(stream->process);
+    while ((p = silc_dlist_get(stream->process))) {
+      silc_free(p->types);
+      silc_free(p);
+      silc_dlist_del(stream->process, p);
+    }
+    silc_dlist_uninit(stream->process);
+  }
+

   /* XXX */
 
   silc_atomic_uninit8(&stream->refcnt);
   /* XXX */
 
   silc_atomic_uninit8(&stream->refcnt);

-  silc_dlist_uninit(stream->process);

   silc_mutex_free(stream->lock);
   silc_free(stream);
 }
   silc_mutex_free(stream->lock);
   silc_free(stream);
 }


@@ -896,6 +959,7 @@ void silc_packet_stream_unlink(SilcPacketStream stream,
     if (p->callbacks == callbacks &&
        p->callback_context == callback_context) {
       silc_dlist_del(stream->process, p);
     if (p->callbacks == callbacks &&
        p->callback_context == callback_context) {
       silc_dlist_del(stream->process, p);

+      silc_free(p->types);

       silc_free(p);
       break;
     }
       silc_free(p);
       break;
     }


@@ -910,6 +974,13 @@ void silc_packet_stream_unlink(SilcPacketStream stream,
   silc_packet_stream_unref(stream);
 }
 
   silc_packet_stream_unref(stream);
 }
 

+/* Returns TRUE if stream is UDP stream */
+
+SilcBool silc_packet_stream_is_udp(SilcPacketStream stream)
+{
+  return stream->udp || silc_socket_stream_is_udp(stream->stream, NULL);
+}
+

 /* Return packet sender IP and port for UDP packet stream */
 
 SilcBool silc_packet_get_sender(SilcPacket packet,
 /* Return packet sender IP and port for UDP packet stream */
 
 SilcBool silc_packet_get_sender(SilcPacket packet,


@@ -944,7 +1015,7 @@ void silc_packet_stream_unref(SilcPacketStream stream)
 
 SilcPacketEngine silc_packet_get_engine(SilcPacketStream stream)
 {
 
 SilcPacketEngine silc_packet_get_engine(SilcPacketStream stream)
 {

-  return stream->engine;
+  return stream->sc->engine;

 }
 
 /* Set application context for packet stream */
 }
 
 /* Set application context for packet stream */


@@ -970,13 +1041,13 @@ void *silc_packet_get_context(SilcPacketStream stream)
 /* Change underlaying stream */
 
 void silc_packet_stream_set_stream(SilcPacketStream ps,
 /* Change underlaying stream */
 
 void silc_packet_stream_set_stream(SilcPacketStream ps,

-                                  SilcStream stream,
-                                  SilcSchedule schedule)
+                                  SilcStream stream)

 {
   if (ps->stream)
 {
   if (ps->stream)

-    silc_stream_set_notifier(ps->stream, schedule, NULL, NULL);
+    silc_stream_set_notifier(ps->stream, ps->sc->schedule, NULL, NULL);

   ps->stream = stream;
   ps->stream = stream;

-  silc_stream_set_notifier(ps->stream, schedule, silc_packet_stream_io, ps);
+  silc_stream_set_notifier(ps->stream, ps->sc->schedule, silc_packet_stream_io,
+                          ps);

 }
 
 /* Return underlaying stream */
 }
 
 /* Return underlaying stream */


@@ -986,104 +1057,97 @@ SilcStream silc_packet_stream_get_stream(SilcPacketStream stream)
   return stream->stream;
 }
 
   return stream->stream;
 }
 

-/* Set ciphers for packet stream */
+/* Set keys. */

 
 

-void silc_packet_set_ciphers(SilcPacketStream stream, SilcCipher send,
-                            SilcCipher receive)
+SilcBool silc_packet_set_keys(SilcPacketStream stream, SilcCipher send_key,
+                              SilcCipher receive_key, SilcHmac send_hmac,
+                              SilcHmac receive_hmac, SilcBool rekey)

 {
 {

-  SILC_LOG_DEBUG(("Setting new ciphers to packet stream"));
+  SILC_LOG_DEBUG(("Setting new keys to packet stream %p", stream));
+
+  /* If doing rekey, send REKEY_DONE packet */
+  if (rekey) {
+    /* This will take stream lock. */
+    if (!silc_packet_send_raw(stream, SILC_PACKET_REKEY_DONE, 0,
+                             stream->src_id_type, stream->src_id,
+                             stream->src_id_len, stream->dst_id_type,
+                             stream->dst_id, stream->dst_id_len,
+                             NULL, 0, stream->send_key[0],
+                             stream->send_hmac[0]))
+      return FALSE;

 
 

-  silc_mutex_lock(stream->lock);
+    /* Write the packet to the stream */
+    if (!silc_packet_stream_write(stream, TRUE))
+      return FALSE;
+  } else {
+    silc_mutex_lock(stream->lock);
+  }

 
 

-  /* In case IV Included is set, save the old key */
+  /* In case IV Included is set, save the old keys */

   if (stream->iv_included) {
   if (stream->iv_included) {

-    if (stream->send_key[1]) {
+    if (stream->send_key[1] && send_key) {

       silc_cipher_free(stream->send_key[1]);
       stream->send_key[1] = stream->send_key[0];
     }
       silc_cipher_free(stream->send_key[1]);
       stream->send_key[1] = stream->send_key[0];
     }

-    if (stream->receive_key[1]) {
+    if (stream->receive_key[1] && receive_key) {

       silc_cipher_free(stream->receive_key[1]);
       stream->receive_key[1] = stream->receive_key[0];
     }
       silc_cipher_free(stream->receive_key[1]);
       stream->receive_key[1] = stream->receive_key[0];
     }

-  } else {
-    if (stream->send_key[0])
-      silc_cipher_free(stream->send_key[0]);
-    if (stream->send_key[1])
-      silc_cipher_free(stream->receive_key[0]);
-  }
-
-  stream->send_key[0] = send;
-  stream->receive_key[0] = receive;
-
-  silc_mutex_unlock(stream->lock);
-}
-
-/* Return current ciphers from packet stream */
-
-SilcBool silc_packet_get_ciphers(SilcPacketStream stream, SilcCipher *send,
-                                SilcCipher *receive)
-{
-  if (!stream->send_key[0] && !stream->receive_key[0])
-    return FALSE;
-
-  silc_mutex_lock(stream->lock);
-
-  if (send)
-    *send = stream->send_key[0];
-  if (receive)
-    *receive = stream->receive_key[0];
-
-  silc_mutex_unlock(stream->lock);
-
-  return TRUE;
-}
-
-/* Set HMACs for packet stream */
-
-void silc_packet_set_hmacs(SilcPacketStream stream, SilcHmac send,
-                          SilcHmac receive)
-{
-  SILC_LOG_DEBUG(("Setting new HMACs to packet stream"));
-
-  silc_mutex_lock(stream->lock);
-
-  /* In case IV Included is set, save the old HMAC */
-  if (stream->iv_included) {
-    if (stream->send_hmac[1]) {
+    if (stream->send_hmac[1] && send_hmac) {

       silc_hmac_free(stream->send_hmac[1]);
       stream->send_hmac[1] = stream->send_hmac[0];
     }
       silc_hmac_free(stream->send_hmac[1]);
       stream->send_hmac[1] = stream->send_hmac[0];
     }

-    if (stream->receive_hmac[1]) {
+    if (stream->receive_hmac[1] && receive_hmac) {

       silc_hmac_free(stream->receive_hmac[1]);
       stream->receive_hmac[1] = stream->receive_hmac[0];
     }
   } else {
       silc_hmac_free(stream->receive_hmac[1]);
       stream->receive_hmac[1] = stream->receive_hmac[0];
     }
   } else {

-    if (stream->send_hmac[0])
+    if (stream->send_key[0] && send_key)
+      silc_cipher_free(stream->send_key[0]);
+    if (stream->send_key[1] && receive_key)
+      silc_cipher_free(stream->receive_key[0]);
+    if (stream->send_hmac[0] && send_hmac)

       silc_hmac_free(stream->send_hmac[0]);
       silc_hmac_free(stream->send_hmac[0]);

-    if (stream->receive_hmac[0])
+    if (stream->receive_hmac[0] && receive_hmac)

       silc_hmac_free(stream->receive_hmac[0]);
   }
 
       silc_hmac_free(stream->receive_hmac[0]);
   }
 

-  stream->send_hmac[0] = send;
-  stream->receive_hmac[0] = receive;
+  /* Set keys */
+  if (send_key)
+    stream->send_key[0] = send_key;
+  if (receive_key)
+    stream->receive_key[0] = receive_key;
+  if (send_hmac)
+    stream->send_hmac[0] = send_hmac;
+  if (receive_hmac)
+    stream->receive_hmac[0] = receive_hmac;

 
   silc_mutex_unlock(stream->lock);
 
   silc_mutex_unlock(stream->lock);

+  return TRUE;

 }
 
 }
 

-/* Return current HMACs from packet stream */
+/* Return current ciphers from packet stream */

 
 

-SilcBool silc_packet_get_hmacs(SilcPacketStream stream, SilcHmac *send,
-                              SilcHmac *receive)
+SilcBool silc_packet_get_keys(SilcPacketStream stream,
+                             SilcCipher *send_key,
+                             SilcCipher *receive_key,
+                             SilcHmac *send_hmac,
+                             SilcHmac *receive_hmac)

 {
 {

-  if (!stream->send_hmac[0] && !stream->receive_hmac[0])
+  if (!stream->send_key[0] && !stream->receive_key[0] &&
+      !stream->send_hmac[0] && !stream->receive_hmac[0])

     return FALSE;
 
   silc_mutex_lock(stream->lock);
 
     return FALSE;
 
   silc_mutex_lock(stream->lock);
 

-  if (send)
-    *send = stream->send_hmac[0];
-  if (receive)
-    *receive = stream->receive_hmac[0];
+  if (send_key)
+    *send_key = stream->send_key[0];
+  if (receive_key)
+    *receive_key = stream->receive_key[0];
+  if (send_hmac)
+    *send_hmac = stream->send_hmac[0];
+  if (receive_hmac)
+    *receive_hmac = stream->receive_hmac[0];

 
   silc_mutex_unlock(stream->lock);
 
 
   silc_mutex_unlock(stream->lock);
 


@@ -1169,14 +1233,14 @@ void silc_packet_free(SilcPacket packet)
   packet->src_id = packet->dst_id = NULL;
   silc_buffer_reset(&packet->buffer);
 
   packet->src_id = packet->dst_id = NULL;
   silc_buffer_reset(&packet->buffer);
 

-  silc_mutex_lock(stream->engine->lock);
+  silc_mutex_lock(stream->sc->engine->lock);

 
   /* Put the packet back to freelist */
 
   /* Put the packet back to freelist */

-  silc_list_add(stream->engine->packet_pool, packet);
-  if (silc_list_count(stream->engine->packet_pool) == 1)
-    silc_list_start(stream->engine->packet_pool);
+  silc_list_add(stream->sc->engine->packet_pool, packet);
+  if (silc_list_count(stream->sc->engine->packet_pool) == 1)
+    silc_list_start(stream->sc->engine->packet_pool);

 
 

-  silc_mutex_unlock(stream->engine->lock);
+  silc_mutex_unlock(stream->sc->engine->lock);

 }
 
 /****************************** Packet Sending ******************************/
 }
 
 /****************************** Packet Sending ******************************/


@@ -1184,10 +1248,10 @@ void silc_packet_free(SilcPacket packet)
 /* Prepare outgoing data buffer for packet sending.  Returns the
    pointer to that buffer into the `packet'. */
 
 /* Prepare outgoing data buffer for packet sending.  Returns the
    pointer to that buffer into the `packet'. */
 

-static SilcBool silc_packet_send_prepare(SilcPacketStream stream,
-                                        SilcUInt32 totlen,
-                                        SilcHmac hmac,
-                                        SilcBuffer packet)
+static inline SilcBool silc_packet_send_prepare(SilcPacketStream stream,
+                                               SilcUInt32 totlen,
+                                               SilcHmac hmac,
+                                               SilcBuffer packet)

 {
   unsigned char *oldptr;
   unsigned int mac_len = hmac ? silc_hmac_len(hmac) : 0;
 {
   unsigned char *oldptr;
   unsigned int mac_len = hmac ? silc_hmac_len(hmac) : 0;


@@ -1195,7 +1259,7 @@ static SilcBool silc_packet_send_prepare(SilcPacketStream stream,
   totlen += mac_len;
 
   /* Allocate more space if needed */
   totlen += mac_len;
 
   /* Allocate more space if needed */

-  if (silc_buffer_taillen(&stream->outbuf) < totlen) {
+  if (silc_unlikely(silc_buffer_taillen(&stream->outbuf) < totlen)) {

     if (!silc_buffer_realloc(&stream->outbuf,
                             silc_buffer_truelen(&stream->outbuf) + totlen))
       return FALSE;
     if (!silc_buffer_realloc(&stream->outbuf,
                             silc_buffer_truelen(&stream->outbuf) + totlen))
       return FALSE;


@@ -1211,25 +1275,62 @@ static SilcBool silc_packet_send_prepare(SilcPacketStream stream,
   return TRUE;
 }
 
   return TRUE;
 }
 

-/* Internal routine to send packet */
-
-static SilcBool silc_packet_send_raw(SilcPacketStream stream,
-                                    SilcPacketType type,
-                                    SilcPacketFlags flags,
-                                    SilcIdType src_id_type,
-                                    unsigned char *src_id,
-                                    SilcUInt32 src_id_len,
-                                    SilcIdType dst_id_type,
-                                    unsigned char *dst_id,
-                                    SilcUInt32 dst_id_len,
-                                    const unsigned char *data,
-                                    SilcUInt32 data_len,
-                                    SilcCipher cipher,
-                                    SilcHmac hmac)
+/* Increments counter when encrypting in counter mode. */
+
+static inline void silc_packet_send_ctr_increment(SilcPacketStream stream,
+                                                 SilcCipher cipher,
+                                                 unsigned char *ret_iv)
+{
+  unsigned char *iv = silc_cipher_get_iv(cipher);
+  SilcUInt32 pc;
+
+  /* Increment packet counter */
+  SILC_GET32_MSB(pc, iv + 8);
+  pc++;
+  SILC_PUT32_MSB(pc, iv + 8);
+
+  /* Reset block counter */
+  memset(iv + 12, 0, 4);
+
+  /* If IV Included flag, return the 64-bit IV for inclusion in packet */
+  if (stream->iv_included) {
+    /* Get new nonce */
+    ret_iv[0] = silc_rng_get_byte_fast(stream->sc->engine->rng);
+    ret_iv[1] = ret_iv[0] + iv[4];
+    ret_iv[2] = ret_iv[0] ^ ret_iv[1];
+    ret_iv[3] = ret_iv[0] + ret_iv[2];
+    SILC_PUT32_MSB(pc, ret_iv + 4);
+    SILC_LOG_HEXDUMP(("IV"), ret_iv, 8);
+
+    /* Set new nonce to counter block */
+    memcpy(iv + 4, ret_iv, 4);
+  }
+
+  SILC_LOG_HEXDUMP(("Counter Block"), iv, 16);
+}
+
+/* Internal routine to assemble outgoing packet.  Assembles and encryptes
+   the packet.  The silc_packet_stream_write needs to be called to send it
+   after this returns TRUE. */
+
+static inline SilcBool silc_packet_send_raw(SilcPacketStream stream,
+                                           SilcPacketType type,
+                                           SilcPacketFlags flags,
+                                           SilcIdType src_id_type,
+                                           unsigned char *src_id,
+                                           SilcUInt32 src_id_len,
+                                           SilcIdType dst_id_type,
+                                           unsigned char *dst_id,
+                                           SilcUInt32 dst_id_len,
+                                           const unsigned char *data,
+                                           SilcUInt32 data_len,
+                                           SilcCipher cipher,
+                                           SilcHmac hmac)

 {
   unsigned char tmppad[SILC_PACKET_MAX_PADLEN], iv[33], psn[4];
   int block_len = (cipher ? silc_cipher_get_block_len(cipher) : 0);
 {
   unsigned char tmppad[SILC_PACKET_MAX_PADLEN], iv[33], psn[4];
   int block_len = (cipher ? silc_cipher_get_block_len(cipher) : 0);

-  int i, enclen, truelen, padlen, ivlen = 0, psnlen = 0;
+  int i, enclen, truelen, padlen = 0, ivlen = 0, psnlen = 0;
+  SilcBool ctr;

   SilcBufferStruct packet;
 
   SILC_LOG_DEBUG(("Sending packet %s (%d) flags %d, src %d dst %d, "
   SilcBufferStruct packet;
 
   SILC_LOG_DEBUG(("Sending packet %s (%d) flags %d, src %d dst %d, "


@@ -1244,12 +1345,25 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
   enclen = truelen = (data_len + SILC_PACKET_HEADER_LEN +
                      src_id_len + dst_id_len);
 
   enclen = truelen = (data_len + SILC_PACKET_HEADER_LEN +
                      src_id_len + dst_id_len);
 

-  /* If IV is included, the SID, IV and sequence number is added to packet */
-  if (stream->iv_included && cipher) {
-    psnlen = sizeof(psn);
-    ivlen = block_len + 1;
-    iv[0] = stream->sid;
-    memcpy(iv + 1, silc_cipher_get_iv(cipher), block_len);
+  /* If using CTR mode, increment the counter */
+  ctr = (cipher && silc_cipher_get_mode(cipher) == SILC_CIPHER_MODE_CTR);
+  if (ctr) {
+    silc_packet_send_ctr_increment(stream, cipher, iv + 1);
+
+    /* If IV is included, the SID, IV and sequence number is added to packet */
+    if (stream->iv_included && cipher) {
+      psnlen = sizeof(psn);
+      ivlen = 8 + 1;
+      iv[0] = stream->sid;
+    }
+  } else {
+    /* If IV is included, the SID, IV and sequence number is added to packet */
+    if (stream->iv_included && cipher) {
+      psnlen = sizeof(psn);
+      ivlen = block_len + 1;
+      iv[0] = stream->sid;
+      memcpy(iv + 1, silc_cipher_get_iv(cipher), block_len);
+    }

   }
 
   /* We automatically figure out the packet structure from the packet
   }
 
   /* We automatically figure out the packet structure from the packet


@@ -1261,8 +1375,9 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
       type == SILC_PACKET_CHANNEL_MESSAGE) {
 
     /* Padding is calculated from header + IDs */
       type == SILC_PACKET_CHANNEL_MESSAGE) {
 
     /* Padding is calculated from header + IDs */

-    SILC_PACKET_PADLEN((SILC_PACKET_HEADER_LEN + src_id_len + dst_id_len +
-                       psnlen), block_len, padlen);
+    if (!ctr)
+      SILC_PACKET_PADLEN((SILC_PACKET_HEADER_LEN + src_id_len + dst_id_len +
+                         psnlen), block_len, padlen);

 
     /* Length to encrypt, header + IDs + padding. */
     enclen = (SILC_PACKET_HEADER_LEN + src_id_len + dst_id_len +
 
     /* Length to encrypt, header + IDs + padding. */
     enclen = (SILC_PACKET_HEADER_LEN + src_id_len + dst_id_len +


@@ -1272,7 +1387,7 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
     /* Padding is calculated from true length of the packet */
     if (flags & SILC_PACKET_FLAG_LONG_PAD)
       SILC_PACKET_PADLEN_MAX(truelen + psnlen, block_len, padlen);
     /* Padding is calculated from true length of the packet */
     if (flags & SILC_PACKET_FLAG_LONG_PAD)
       SILC_PACKET_PADLEN_MAX(truelen + psnlen, block_len, padlen);

-    else
+    else if (!ctr)

       SILC_PACKET_PADLEN(truelen + psnlen, block_len, padlen);
 
     enclen += padlen + psnlen;
       SILC_PACKET_PADLEN(truelen + psnlen, block_len, padlen);
 
     enclen += padlen + psnlen;


@@ -1283,13 +1398,13 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
 
   /* Get random padding */
   for (i = 0; i < padlen; i++) tmppad[i] =
 
   /* Get random padding */
   for (i = 0; i < padlen; i++) tmppad[i] =

-                                silc_rng_get_byte_fast(stream->engine->rng);
+    silc_rng_get_byte_fast(stream->sc->engine->rng);

 
   silc_mutex_lock(stream->lock);
 
   /* Get packet pointer from the outgoing buffer */
 
   silc_mutex_lock(stream->lock);
 
   /* Get packet pointer from the outgoing buffer */

-  if (!silc_packet_send_prepare(stream, truelen + padlen + ivlen + psnlen,
-                               hmac, &packet)) {
+  if (silc_unlikely(!silc_packet_send_prepare(stream, truelen + padlen + ivlen
+                                             + psnlen, hmac, &packet))) {

     silc_mutex_unlock(stream->lock);
     return FALSE;
   }
     silc_mutex_unlock(stream->lock);
     return FALSE;
   }


@@ -1315,7 +1430,7 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
                         SILC_STR_DATA(tmppad, padlen),
                         SILC_STR_DATA(data, data_len),
                         SILC_STR_END);
                         SILC_STR_DATA(tmppad, padlen),
                         SILC_STR_DATA(data, data_len),
                         SILC_STR_END);

-  if (i < 0) {
+  if (silc_unlikely(i < 0)) {

     silc_mutex_unlock(stream->lock);
     return FALSE;
   }
     silc_mutex_unlock(stream->lock);
     return FALSE;
   }


@@ -1324,10 +1439,11 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
                   silc_buffer_data(&packet), silc_buffer_len(&packet));
 
   /* Encrypt the packet */
                   silc_buffer_data(&packet), silc_buffer_len(&packet));
 
   /* Encrypt the packet */

-  if (cipher) {
+  if (silc_likely(cipher)) {

     SILC_LOG_DEBUG(("Encrypting packet"));
     SILC_LOG_DEBUG(("Encrypting packet"));

-    if (!silc_cipher_encrypt(cipher, packet.data + ivlen,
-                            packet.data + ivlen, enclen, NULL)) {
+    if (silc_unlikely(!silc_cipher_encrypt(cipher, packet.data + ivlen,
+                                          packet.data + ivlen, enclen,
+                                          NULL))) {

       SILC_LOG_ERROR(("Packet encryption failed"));
       silc_mutex_unlock(stream->lock);
       return FALSE;
       SILC_LOG_ERROR(("Packet encryption failed"));
       silc_mutex_unlock(stream->lock);
       return FALSE;


@@ -1335,7 +1451,7 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
   }
 
   /* Compute HMAC */
   }
 
   /* Compute HMAC */

-  if (hmac) {
+  if (silc_likely(hmac)) {

     SilcUInt32 mac_len;
 
     /* MAC is computed from the entire encrypted packet data, and put
     SilcUInt32 mac_len;
 
     /* MAC is computed from the entire encrypted packet data, and put


@@ -1348,8 +1464,7 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
     stream->send_psn++;
   }
 
     stream->send_psn++;
   }
 

-  /* Write the packet to the stream */
-  return silc_packet_stream_write(stream);
+  return TRUE;

 }
 
 /* Sends a packet */
 }
 
 /* Sends a packet */


@@ -1358,16 +1473,21 @@ SilcBool silc_packet_send(SilcPacketStream stream,
                          SilcPacketType type, SilcPacketFlags flags,
                          const unsigned char *data, SilcUInt32 data_len)
 {
                          SilcPacketType type, SilcPacketFlags flags,
                          const unsigned char *data, SilcUInt32 data_len)
 {

-  return silc_packet_send_raw(stream, type, flags,
-                             stream->src_id_type,
-                             stream->src_id,
-                             stream->src_id_len,
-                             stream->dst_id_type,
-                             stream->dst_id,
-                             stream->dst_id_len,
-                             data, data_len,
-                             stream->send_key[0],
-                             stream->send_hmac[0]);
+  SilcBool ret;
+
+  ret = silc_packet_send_raw(stream, type, flags,
+                            stream->src_id_type,
+                            stream->src_id,
+                            stream->src_id_len,
+                            stream->dst_id_type,
+                            stream->dst_id,
+                            stream->dst_id_len,
+                            data, data_len,
+                            stream->send_key[0],
+                            stream->send_hmac[0]);
+
+  /* Write the packet to the stream */
+  return ret ? silc_packet_stream_write(stream, FALSE) : FALSE;

 }
 
 /* Sends a packet, extended routine */
 }
 
 /* Sends a packet, extended routine */


@@ -1381,6 +1501,7 @@ SilcBool silc_packet_send_ext(SilcPacketStream stream,
 {
   unsigned char src_id_data[32], dst_id_data[32];
   SilcUInt32 src_id_len, dst_id_len;
 {
   unsigned char src_id_data[32], dst_id_data[32];
   SilcUInt32 src_id_len, dst_id_len;

+  SilcBool ret;

 
   if (src_id)
     if (!silc_id_id2str(src_id, src_id_type, src_id_data,
 
   if (src_id)
     if (!silc_id_id2str(src_id, src_id_type, src_id_data,


@@ -1391,16 +1512,19 @@ SilcBool silc_packet_send_ext(SilcPacketStream stream,
                        sizeof(dst_id_data), &dst_id_len))
       return FALSE;
 
                        sizeof(dst_id_data), &dst_id_len))
       return FALSE;
 

-  return silc_packet_send_raw(stream, type, flags,
-                             src_id ? src_id_type : stream->src_id_type,
-                             src_id ? src_id_data : stream->src_id,
-                             src_id ? src_id_len : stream->src_id_len,
-                             dst_id ? dst_id_type : stream->dst_id_type,
-                             dst_id ? dst_id_data : stream->dst_id,
-                             dst_id ? dst_id_len : stream->dst_id_len,
-                             data, data_len,
-                             cipher ? cipher : stream->send_key[0],
-                             hmac ? hmac : stream->send_hmac[0]);
+  ret = silc_packet_send_raw(stream, type, flags,
+                            src_id ? src_id_type : stream->src_id_type,
+                            src_id ? src_id_data : stream->src_id,
+                            src_id ? src_id_len : stream->src_id_len,
+                            dst_id ? dst_id_type : stream->dst_id_type,
+                            dst_id ? dst_id_data : stream->dst_id,
+                            dst_id ? dst_id_len : stream->dst_id_len,
+                            data, data_len,
+                            cipher ? cipher : stream->send_key[0],
+                            hmac ? hmac : stream->send_hmac[0]);
+
+  /* Write the packet to the stream */
+  return ret ? silc_packet_stream_write(stream, FALSE) : FALSE;

 }
 
 /* Sends packet after formatting the arguments to buffer */
 }
 
 /* Sends packet after formatting the arguments to buffer */


@@ -1471,7 +1595,7 @@ static inline SilcBool silc_packet_check_mac(SilcHmac hmac,
                                             SilcUInt32 sequence)
 {
   /* Check MAC */
                                             SilcUInt32 sequence)
 {
   /* Check MAC */

-  if (hmac) {
+  if (silc_likely(hmac)) {

     unsigned char mac[32], psn[4];
     SilcUInt32 mac_len;
 
     unsigned char mac[32], psn[4];
     SilcUInt32 mac_len;
 


@@ -1490,7 +1614,7 @@ static inline SilcBool silc_packet_check_mac(SilcHmac hmac,
     silc_hmac_final(hmac, mac, &mac_len);
 
     /* Compare the MAC's */
     silc_hmac_final(hmac, mac, &mac_len);
 
     /* Compare the MAC's */

-    if (memcmp(packet_mac, mac, mac_len)) {
+    if (silc_unlikely(memcmp(packet_mac, mac, mac_len))) {

       SILC_LOG_DEBUG(("MAC failed"));
       return FALSE;
     }
       SILC_LOG_DEBUG(("MAC failed"));
       return FALSE;
     }


@@ -1501,6 +1625,30 @@ static inline SilcBool silc_packet_check_mac(SilcHmac hmac,
   return TRUE;
 }
 
   return TRUE;
 }
 

+/* Increments/sets counter when decrypting in counter mode. */
+
+static inline void silc_packet_receive_ctr_increment(SilcPacketStream stream,
+                                                    unsigned char *iv,
+                                                    unsigned char *packet_iv)
+{
+  SilcUInt32 pc;
+
+  /* If IV Included flag, set the IV from packet to block counter. */
+  if (stream->iv_included) {
+    memcpy(iv + 4, packet_iv, 8);
+  } else {
+    /* Increment packet counter */
+    SILC_GET32_MSB(pc, iv + 8);
+    pc++;
+    SILC_PUT32_MSB(pc, iv + 8);
+  }
+
+  /* Reset block counter */
+  memset(iv + 12, 0, 4);
+
+  SILC_LOG_HEXDUMP(("Counter Block"), iv, 16);
+}
+

 /* Decrypts SILC packet.  Handles both normal and special packet decryption.
    Return 0 when packet is normal and 1 when it it special, -1 on error. */
 
 /* Decrypts SILC packet.  Handles both normal and special packet decryption.
    Return 0 when packet is normal and 1 when it it special, -1 on error. */
 


@@ -1509,18 +1657,19 @@ static inline int silc_packet_decrypt(SilcCipher cipher, SilcHmac hmac,
                                      SilcBool normal)
 {
   if (normal == TRUE) {
                                      SilcBool normal)
 {
   if (normal == TRUE) {

-    if (cipher) {
+    if (silc_likely(cipher)) {

       /* Decrypt rest of the packet */
       SILC_LOG_DEBUG(("Decrypting the packet"));
       /* Decrypt rest of the packet */
       SILC_LOG_DEBUG(("Decrypting the packet"));

-      if (!silc_cipher_decrypt(cipher, buffer->data, buffer->data,
-                              silc_buffer_len(buffer), NULL))
+      if (silc_unlikely(!silc_cipher_decrypt(cipher, buffer->data,
+                                            buffer->data,
+                                            silc_buffer_len(buffer), NULL)))

        return -1;
     }
     return 0;
 
   } else {
     /* Decrypt rest of the header plus padding */
        return -1;
     }
     return 0;
 
   } else {
     /* Decrypt rest of the header plus padding */

-    if (cipher) {
+    if (silc_likely(cipher)) {

       SilcUInt16 len;
       SilcUInt32 block_len = silc_cipher_get_block_len(cipher);
 
       SilcUInt16 len;
       SilcUInt32 block_len = silc_cipher_get_block_len(cipher);
 


@@ -1534,13 +1683,13 @@ static inline int silc_packet_decrypt(SilcCipher cipher, SilcHmac hmac,
             block_len);
       silc_buffer_pull(buffer, block_len);
 
             block_len);
       silc_buffer_pull(buffer, block_len);
 

-      if (len > silc_buffer_len(buffer)) {
+      if (silc_unlikely(len > silc_buffer_len(buffer))) {

        SILC_LOG_ERROR(("Garbage in header of packet, bad packet length, "
                        "packet dropped"));
        return -1;
       }
        SILC_LOG_ERROR(("Garbage in header of packet, bad packet length, "
                        "packet dropped"));
        return -1;
       }

-      if (!silc_cipher_decrypt(cipher, buffer->data, buffer->data,
-                              len, NULL))
+      if (silc_unlikely(!silc_cipher_decrypt(cipher, buffer->data,
+                                            buffer->data, len, NULL)))

        return -1;
     }
 
        return -1;
     }
 


@@ -1569,15 +1718,15 @@ static inline SilcBool silc_packet_parse(SilcPacket packet)
                             SILC_STR_UI_CHAR(&dst_id_len),
                             SILC_STR_UI_CHAR(&src_id_type),
                             SILC_STR_END);
                             SILC_STR_UI_CHAR(&dst_id_len),
                             SILC_STR_UI_CHAR(&src_id_type),
                             SILC_STR_END);

-  if (ret == -1) {
+  if (silc_unlikely(ret == -1)) {

     if (!packet->stream->udp &&
        !silc_socket_stream_is_udp(packet->stream->stream, NULL))
       SILC_LOG_ERROR(("Malformed packet header, packet dropped"));
     return FALSE;
   }
 
     if (!packet->stream->udp &&
        !silc_socket_stream_is_udp(packet->stream->stream, NULL))
       SILC_LOG_ERROR(("Malformed packet header, packet dropped"));
     return FALSE;
   }
 

-  if (src_id_len > SILC_PACKET_MAX_ID_LEN ||
-      dst_id_len > SILC_PACKET_MAX_ID_LEN) {
+  if (silc_unlikely(src_id_len > SILC_PACKET_MAX_ID_LEN ||
+                   dst_id_len > SILC_PACKET_MAX_ID_LEN)) {

     if (!packet->stream->udp &&
        !silc_socket_stream_is_udp(packet->stream->stream, NULL))
       SILC_LOG_ERROR(("Bad ID lengths in packet (%d and %d)",
     if (!packet->stream->udp &&
        !silc_socket_stream_is_udp(packet->stream->stream, NULL))
       SILC_LOG_ERROR(("Bad ID lengths in packet (%d and %d)",


@@ -1592,15 +1741,15 @@ static inline SilcBool silc_packet_parse(SilcPacket packet)
                             SILC_STR_DATA(&packet->dst_id, dst_id_len),
                             SILC_STR_OFFSET(padlen),
                             SILC_STR_END);
                             SILC_STR_DATA(&packet->dst_id, dst_id_len),
                             SILC_STR_OFFSET(padlen),
                             SILC_STR_END);

-  if (ret == -1) {
+  if (silc_unlikely(ret == -1)) {

     if (!packet->stream->udp &&
        !silc_socket_stream_is_udp(packet->stream->stream, NULL))
       SILC_LOG_ERROR(("Malformed packet header, packet dropped"));
     return FALSE;
   }
 
     if (!packet->stream->udp &&
        !silc_socket_stream_is_udp(packet->stream->stream, NULL))
       SILC_LOG_ERROR(("Malformed packet header, packet dropped"));
     return FALSE;
   }
 

-  if (src_id_type > SILC_ID_CHANNEL ||
-      dst_id_type > SILC_ID_CHANNEL) {
+  if (silc_unlikely(src_id_type > SILC_ID_CHANNEL ||
+                   dst_id_type > SILC_ID_CHANNEL)) {

     if (!packet->stream->udp &&
        !silc_socket_stream_is_udp(packet->stream->stream, NULL))
       SILC_LOG_ERROR(("Bad ID types in packet (%d and %d)",
     if (!packet->stream->udp &&
        !silc_socket_stream_is_udp(packet->stream->stream, NULL))
       SILC_LOG_ERROR(("Bad ID types in packet (%d and %d)",


@@ -1623,9 +1772,10 @@ static inline SilcBool silc_packet_parse(SilcPacket packet)
   return TRUE;
 }
 
   return TRUE;
 }
 

-/* Dispatch packet to application.  Called with stream->lock locked. */
+/* Dispatch packet to application.  Called with stream->lock locked.
+   Returns FALSE if the stream was destroyed while dispatching a packet. */

 
 

-static void silc_packet_dispatch(SilcPacket packet)
+static SilcBool silc_packet_dispatch(SilcPacket packet)

 {
   SilcPacketStream stream = packet->stream;
   SilcPacketProcess p;
 {
   SilcPacketStream stream = packet->stream;
   SilcPacketProcess p;


@@ -1634,17 +1784,17 @@ static void silc_packet_dispatch(SilcPacket packet)
 
   /* Dispatch packet to all packet processors that want it */
 
 
   /* Dispatch packet to all packet processors that want it */
 

-  if (!stream->process) {
+  if (silc_likely(!stream->process)) {

     /* Send to default processor as no others exist */
     SILC_LOG_DEBUG(("Dispatching packet to default callbacks"));
     silc_mutex_unlock(stream->lock);
     /* Send to default processor as no others exist */
     SILC_LOG_DEBUG(("Dispatching packet to default callbacks"));
     silc_mutex_unlock(stream->lock);

-    if (!stream->engine->callbacks->
-       packet_receive(stream->engine, stream, packet,
-                      stream->engine->callback_context,
-                      stream->stream_context))
+    if (silc_unlikely(!stream->sc->engine->callbacks->
+                     packet_receive(stream->sc->engine, stream, packet,
+                                    stream->sc->engine->callback_context,
+                                    stream->stream_context)))

       silc_packet_free(packet);
     silc_mutex_lock(stream->lock);
       silc_packet_free(packet);
     silc_mutex_lock(stream->lock);

-    return;
+    return stream->destroyed == FALSE;

   }
 
   silc_dlist_start(stream->process);
   }
 
   silc_dlist_start(stream->process);


@@ -1656,12 +1806,12 @@ static void silc_packet_dispatch(SilcPacket packet)
       SILC_LOG_DEBUG(("Dispatching packet to default callbacks"));
       default_sent = TRUE;
       silc_mutex_unlock(stream->lock);
       SILC_LOG_DEBUG(("Dispatching packet to default callbacks"));
       default_sent = TRUE;
       silc_mutex_unlock(stream->lock);

-      if (stream->engine->callbacks->
-         packet_receive(stream->engine, stream, packet,
-                        stream->engine->callback_context,
+      if (stream->sc->engine->callbacks->
+         packet_receive(stream->sc->engine, stream, packet,
+                        stream->sc->engine->callback_context,

                         stream->stream_context)) {
        silc_mutex_lock(stream->lock);
                         stream->stream_context)) {
        silc_mutex_lock(stream->lock);

-       return;
+       return stream->destroyed == FALSE;

       }
       silc_mutex_lock(stream->lock);
     }
       }
       silc_mutex_lock(stream->lock);
     }


@@ -1671,11 +1821,11 @@ static void silc_packet_dispatch(SilcPacket packet)
       /* Send all packet types */
       SILC_LOG_DEBUG(("Dispatching packet to %p callbacks", p->callbacks));
       silc_mutex_unlock(stream->lock);
       /* Send all packet types */
       SILC_LOG_DEBUG(("Dispatching packet to %p callbacks", p->callbacks));
       silc_mutex_unlock(stream->lock);

-      if (p->callbacks->packet_receive(stream->engine, stream, packet,
+      if (p->callbacks->packet_receive(stream->sc->engine, stream, packet,

                                       p->callback_context,
                                       stream->stream_context)) {
        silc_mutex_lock(stream->lock);
                                       p->callback_context,
                                       stream->stream_context)) {
        silc_mutex_lock(stream->lock);

-       return;
+       return stream->destroyed == FALSE;

       }
       silc_mutex_lock(stream->lock);
     } else {
       }
       silc_mutex_lock(stream->lock);
     } else {


@@ -1685,11 +1835,11 @@ static void silc_packet_dispatch(SilcPacket packet)
          continue;
        SILC_LOG_DEBUG(("Dispatching packet to %p callbacks", p->callbacks));
        silc_mutex_unlock(stream->lock);
          continue;
        SILC_LOG_DEBUG(("Dispatching packet to %p callbacks", p->callbacks));
        silc_mutex_unlock(stream->lock);

-       if (p->callbacks->packet_receive(stream->engine, stream, packet,
+       if (p->callbacks->packet_receive(stream->sc->engine, stream, packet,

                                         p->callback_context,
                                         stream->stream_context)) {
          silc_mutex_lock(stream->lock);
                                         p->callback_context,
                                         stream->stream_context)) {
          silc_mutex_lock(stream->lock);

-         return;
+         return stream->destroyed == FALSE;

        }
        silc_mutex_lock(stream->lock);
        break;
        }
        silc_mutex_lock(stream->lock);
        break;


@@ -1701,18 +1851,19 @@ static void silc_packet_dispatch(SilcPacket packet)
     /* Send to default processor as it has not been sent yet */
     SILC_LOG_DEBUG(("Dispatching packet to default callbacks"));
     silc_mutex_unlock(stream->lock);
     /* Send to default processor as it has not been sent yet */
     SILC_LOG_DEBUG(("Dispatching packet to default callbacks"));
     silc_mutex_unlock(stream->lock);

-    if (stream->engine->callbacks->
-       packet_receive(stream->engine, stream, packet,
-                      stream->engine->callback_context,
+    if (stream->sc->engine->callbacks->
+       packet_receive(stream->sc->engine, stream, packet,
+                      stream->sc->engine->callback_context,

                       stream->stream_context)) {
       silc_mutex_lock(stream->lock);
                       stream->stream_context)) {
       silc_mutex_lock(stream->lock);

-      return;
+      return stream->destroyed == FALSE;

     }
     silc_mutex_lock(stream->lock);
   }
 
   /* If we got here, no one wanted the packet, so drop it */
   silc_packet_free(packet);
     }
     silc_mutex_lock(stream->lock);
   }
 
   /* If we got here, no one wanted the packet, so drop it */
   silc_packet_free(packet);

+  return stream->destroyed == FALSE;

 }
 
 /* Process incoming data and parse packets.  Called with stream->lock
 }
 
 /* Process incoming data and parse packets.  Called with stream->lock


@@ -1720,6 +1871,7 @@ static void silc_packet_dispatch(SilcPacket packet)
 
 static void silc_packet_read_process(SilcPacketStream stream)
 {
 
 static void silc_packet_read_process(SilcPacketStream stream)
 {

+  SilcBuffer inbuf = &stream->sc->inbuf;

   SilcCipher cipher;
   SilcHmac hmac;
   SilcPacket packet;
   SilcCipher cipher;
   SilcHmac hmac;
   SilcPacket packet;


@@ -1732,33 +1884,42 @@ static void silc_packet_read_process(SilcPacketStream stream)
   int ret;
 
   /* Parse the packets from the data */
   int ret;
 
   /* Parse the packets from the data */

-  while (silc_buffer_len(&stream->inbuf) > 0) {
+  while (silc_buffer_len(inbuf) > 0) {

     ivlen = psnlen = 0;
     cipher = stream->receive_key[0];
     hmac = stream->receive_hmac[0];
     normal = FALSE;
 
     ivlen = psnlen = 0;
     cipher = stream->receive_key[0];
     hmac = stream->receive_hmac[0];
     normal = FALSE;
 

-    if (silc_buffer_len(&stream->inbuf) <
-       (stream->iv_included ? SILC_PACKET_MIN_HEADER_LEN_IV :
-        SILC_PACKET_MIN_HEADER_LEN)) {
+    if (silc_unlikely(silc_buffer_len(inbuf) <
+                     (stream->iv_included ? SILC_PACKET_MIN_HEADER_LEN_IV :
+                      SILC_PACKET_MIN_HEADER_LEN))) {

       SILC_LOG_DEBUG(("Partial packet in queue, waiting for the rest"));
       return;
     }
 
       SILC_LOG_DEBUG(("Partial packet in queue, waiting for the rest"));
       return;
     }
 

-    if (hmac)
+    if (silc_likely(hmac))

       mac_len = silc_hmac_len(hmac);
     else
       mac_len = 0;
 
     /* Decrypt first block of the packet to get the length field out */
       mac_len = silc_hmac_len(hmac);
     else
       mac_len = 0;
 
     /* Decrypt first block of the packet to get the length field out */

-    if (cipher) {
+    if (silc_likely(cipher)) {

       block_len = silc_cipher_get_block_len(cipher);
 
       if (stream->iv_included) {
        /* SID, IV and sequence number is included in the ciphertext */
       block_len = silc_cipher_get_block_len(cipher);
 
       if (stream->iv_included) {
        /* SID, IV and sequence number is included in the ciphertext */

-       sid = (SilcUInt8)stream->inbuf.data[0];
-       memcpy(iv, stream->inbuf.data + 1, block_len);
-       ivlen = block_len + 1;
+       sid = (SilcUInt8)inbuf->data[0];
+
+       if (silc_cipher_get_mode(cipher) == SILC_CIPHER_MODE_CTR) {
+         /* Set the CTR mode IV from packet to counter block */
+         memcpy(iv, silc_cipher_get_iv(cipher), block_len);
+         silc_packet_receive_ctr_increment(stream, iv, inbuf->data + 1);
+         ivlen = 8 + 1;
+       } else {
+         /* Get IV from packet */
+         memcpy(iv, inbuf->data + 1, block_len);
+         ivlen = block_len + 1;
+       }

        psnlen = 4;
 
        /* Check SID, and get correct decryption key */
        psnlen = 4;
 
        /* Check SID, and get correct decryption key */


@@ -1775,15 +1936,19 @@ static void silc_packet_read_process(SilcPacketStream stream)
            silc_mutex_unlock(stream->lock);
            SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_UNKNOWN_SID);
            silc_mutex_lock(stream->lock);
            silc_mutex_unlock(stream->lock);
            SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_UNKNOWN_SID);
            silc_mutex_lock(stream->lock);

-           silc_buffer_reset(&stream->inbuf);
+           silc_buffer_reset(inbuf);

            return;
          }
        }
       } else {
        memcpy(iv, silc_cipher_get_iv(cipher), block_len);
            return;
          }
        }
       } else {
        memcpy(iv, silc_cipher_get_iv(cipher), block_len);

+
+       /* If using CTR mode, increment the counter */
+       if (silc_cipher_get_mode(cipher) == SILC_CIPHER_MODE_CTR)
+         silc_packet_receive_ctr_increment(stream, iv, NULL);

       }
 
       }
 

-      silc_cipher_decrypt(cipher, stream->inbuf.data + ivlen, tmp,
+      silc_cipher_decrypt(cipher, inbuf->data + ivlen, tmp,

                          block_len, iv);
 
       header = tmp;
                          block_len, iv);
 
       header = tmp;


@@ -1793,60 +1958,62 @@ static void silc_packet_read_process(SilcPacketStream stream)
        header += 4;
       }
     } else {
        header += 4;
       }
     } else {

+      /* Unencrypted packet */

       block_len = SILC_PACKET_MIN_HEADER_LEN;
       block_len = SILC_PACKET_MIN_HEADER_LEN;

-      header = stream->inbuf.data;
+      header = inbuf->data;

     }
 
     /* Get packet length and full packet length with padding */
     SILC_PACKET_LENGTH(header, packetlen, paddedlen);
 
     /* Sanity checks */
     }
 
     /* Get packet length and full packet length with padding */
     SILC_PACKET_LENGTH(header, packetlen, paddedlen);
 
     /* Sanity checks */

-    if (packetlen < SILC_PACKET_MIN_LEN) {
+    if (silc_unlikely(packetlen < SILC_PACKET_MIN_LEN)) {

       if (!stream->udp && !silc_socket_stream_is_udp(stream->stream, NULL))
        SILC_LOG_ERROR(("Received too short packet"));
       silc_mutex_unlock(stream->lock);
       SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_MALFORMED);
       silc_mutex_lock(stream->lock);
       memset(tmp, 0, sizeof(tmp));
       if (!stream->udp && !silc_socket_stream_is_udp(stream->stream, NULL))
        SILC_LOG_ERROR(("Received too short packet"));
       silc_mutex_unlock(stream->lock);
       SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_MALFORMED);
       silc_mutex_lock(stream->lock);
       memset(tmp, 0, sizeof(tmp));

-      silc_buffer_reset(&stream->inbuf);
+      silc_buffer_reset(inbuf);

       return;
     }
 
       return;
     }
 

-    if (silc_buffer_len(&stream->inbuf) < paddedlen + ivlen + mac_len) {
+    if (silc_buffer_len(inbuf) < paddedlen + ivlen + mac_len) {

       SILC_LOG_DEBUG(("Received partial packet, waiting for the rest "
                      "(%d bytes)",
       SILC_LOG_DEBUG(("Received partial packet, waiting for the rest "
                      "(%d bytes)",

-                     paddedlen + mac_len - silc_buffer_len(&stream->inbuf)));
+                     paddedlen + mac_len - silc_buffer_len(inbuf)));

       memset(tmp, 0, sizeof(tmp));
       return;
     }
 
     /* Check MAC of the packet */
       memset(tmp, 0, sizeof(tmp));
       return;
     }
 
     /* Check MAC of the packet */

-    if (!silc_packet_check_mac(hmac, stream->inbuf.data,
-                              paddedlen + ivlen,
-                              stream->inbuf.data + ivlen + paddedlen,
-                              packet_seq, stream->receive_psn)) {
+    if (silc_unlikely(!silc_packet_check_mac(hmac, inbuf->data,
+                                            paddedlen + ivlen,
+                                            inbuf->data + ivlen +
+                                            paddedlen, packet_seq,
+                                            stream->receive_psn))) {

       silc_mutex_unlock(stream->lock);
       SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_MAC_FAILED);
       silc_mutex_lock(stream->lock);
       memset(tmp, 0, sizeof(tmp));
       silc_mutex_unlock(stream->lock);
       SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_MAC_FAILED);
       silc_mutex_lock(stream->lock);
       memset(tmp, 0, sizeof(tmp));

-      silc_buffer_reset(&stream->inbuf);
+      silc_buffer_reset(inbuf);

       return;
     }
 
     /* Get packet */
       return;
     }
 
     /* Get packet */

-    packet = silc_packet_alloc(stream->engine);
-    if (!packet) {
+    packet = silc_packet_alloc(stream->sc->engine);
+    if (silc_unlikely(!packet)) {

       silc_mutex_unlock(stream->lock);
       SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_NO_MEMORY);
       silc_mutex_lock(stream->lock);
       memset(tmp, 0, sizeof(tmp));
       silc_mutex_unlock(stream->lock);
       SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_NO_MEMORY);
       silc_mutex_lock(stream->lock);
       memset(tmp, 0, sizeof(tmp));

-      silc_buffer_reset(&stream->inbuf);
+      silc_buffer_reset(inbuf);

       return;
     }
     packet->stream = stream;
 
     /* Allocate more space to packet buffer, if needed */
       return;
     }
     packet->stream = stream;
 
     /* Allocate more space to packet buffer, if needed */

-    if (silc_buffer_truelen(&packet->buffer) < paddedlen) {
+    if (silc_unlikely(silc_buffer_truelen(&packet->buffer) < paddedlen)) {

       if (!silc_buffer_realloc(&packet->buffer,
                               silc_buffer_truelen(&packet->buffer) +
                               (paddedlen -
       if (!silc_buffer_realloc(&packet->buffer,
                               silc_buffer_truelen(&packet->buffer) +
                               (paddedlen -


@@ -1856,7 +2023,7 @@ static void silc_packet_read_process(SilcPacketStream stream)
        silc_mutex_lock(stream->lock);
        silc_packet_free(packet);
        memset(tmp, 0, sizeof(tmp));
        silc_mutex_lock(stream->lock);
        silc_packet_free(packet);
        memset(tmp, 0, sizeof(tmp));

-       silc_buffer_reset(&stream->inbuf);
+       silc_buffer_reset(inbuf);

        return;
       }
     }
        return;
       }
     }


@@ -1865,7 +2032,7 @@ static void silc_packet_read_process(SilcPacketStream stream)
     packet->flags = (SilcPacketFlags)header[2];
     packet->type = (SilcPacketType)header[3];
 
     packet->flags = (SilcPacketFlags)header[2];
     packet->type = (SilcPacketType)header[3];
 

-    if (stream->engine->local_is_router) {
+    if (stream->sc->engine->local_is_router) {

       if (packet->type == SILC_PACKET_PRIVATE_MESSAGE &&
          (packet->flags & SILC_PACKET_FLAG_PRIVMSG_KEY))
        normal = FALSE;
       if (packet->type == SILC_PACKET_PRIVATE_MESSAGE &&
          (packet->flags & SILC_PACKET_FLAG_PRIVMSG_KEY))
        normal = FALSE;


@@ -1883,20 +2050,20 @@ static void silc_packet_read_process(SilcPacketStream stream)
 
     SILC_LOG_HEXDUMP(("Incoming packet (%d) len %d",
                      stream->receive_psn, paddedlen + ivlen + mac_len),
 
     SILC_LOG_HEXDUMP(("Incoming packet (%d) len %d",
                      stream->receive_psn, paddedlen + ivlen + mac_len),

-                    stream->inbuf.data, paddedlen + ivlen + mac_len);
+                    inbuf->data, paddedlen + ivlen + mac_len);

 
     /* Put the decrypted part, and rest of the encrypted data, and decrypt */
     silc_buffer_pull_tail(&packet->buffer, paddedlen);
     silc_buffer_put(&packet->buffer, header, block_len - psnlen);
     silc_buffer_pull(&packet->buffer, block_len - psnlen);
 
     /* Put the decrypted part, and rest of the encrypted data, and decrypt */
     silc_buffer_pull_tail(&packet->buffer, paddedlen);
     silc_buffer_put(&packet->buffer, header, block_len - psnlen);
     silc_buffer_pull(&packet->buffer, block_len - psnlen);

-    silc_buffer_put(&packet->buffer, (stream->inbuf.data + ivlen +
+    silc_buffer_put(&packet->buffer, (inbuf->data + ivlen +

                                      psnlen + (block_len - psnlen)),
                    paddedlen - ivlen - psnlen - (block_len - psnlen));
                                      psnlen + (block_len - psnlen)),
                    paddedlen - ivlen - psnlen - (block_len - psnlen));

-    if (cipher) {
+    if (silc_likely(cipher)) {

       silc_cipher_set_iv(cipher, iv);
       ret = silc_packet_decrypt(cipher, hmac, stream->receive_psn,
                                &packet->buffer, normal);
       silc_cipher_set_iv(cipher, iv);
       ret = silc_packet_decrypt(cipher, hmac, stream->receive_psn,
                                &packet->buffer, normal);

-      if (ret < 0) {
+      if (silc_unlikely(ret < 0)) {

        silc_mutex_unlock(stream->lock);
        SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_DECRYPTION_FAILED);
        silc_mutex_lock(stream->lock);
        silc_mutex_unlock(stream->lock);
        SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_DECRYPTION_FAILED);
        silc_mutex_lock(stream->lock);


@@ -1910,10 +2077,10 @@ static void silc_packet_read_process(SilcPacketStream stream)
     silc_buffer_push(&packet->buffer, block_len);
 
     /* Pull the packet from inbuf thus we'll get the next one in the inbuf. */
     silc_buffer_push(&packet->buffer, block_len);
 
     /* Pull the packet from inbuf thus we'll get the next one in the inbuf. */

-    silc_buffer_pull(&stream->inbuf, paddedlen + mac_len);
+    silc_buffer_pull(inbuf, paddedlen + mac_len);

 
     /* Parse the packet */
 
     /* Parse the packet */

-    if (!silc_packet_parse(packet)) {
+    if (silc_unlikely(!silc_packet_parse(packet))) {

       silc_mutex_unlock(stream->lock);
       SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_MALFORMED);
       silc_mutex_lock(stream->lock);
       silc_mutex_unlock(stream->lock);
       SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_MALFORMED);
       silc_mutex_lock(stream->lock);


@@ -1923,10 +2090,11 @@ static void silc_packet_read_process(SilcPacketStream stream)
     }
 
     /* Dispatch the packet to application */
     }
 
     /* Dispatch the packet to application */

-    silc_packet_dispatch(packet);
+    if (!silc_packet_dispatch(packet))
+      break;

   }
 
   }
 

-  silc_buffer_reset(&stream->inbuf);
+  silc_buffer_reset(inbuf);

 }
 
 
 }
 
 


@@ -1968,7 +2136,7 @@ silc_packet_wait_packet_receive(SilcPacketEngine engine,
   /* Signal the waiting thread for a new packet */
   silc_mutex_lock(pw->wait_lock);
 
   /* Signal the waiting thread for a new packet */
   silc_mutex_lock(pw->wait_lock);
 

-  if (pw->stopped) {
+  if (silc_unlikely(pw->stopped)) {

     silc_mutex_unlock(pw->wait_lock);
     return FALSE;
   }
     silc_mutex_unlock(pw->wait_lock);
     return FALSE;
   }


@@ -2061,7 +2229,7 @@ int silc_packet_wait(void *waiter, int timeout, SilcPacket *return_packet)
 
   /* Wait here until packet has arrived */
   while (silc_list_count(pw->packet_queue) == 0) {
 
   /* Wait here until packet has arrived */
   while (silc_list_count(pw->packet_queue) == 0) {

-    if (pw->stopped) {
+    if (silc_unlikely(pw->stopped)) {

       silc_mutex_unlock(pw->wait_lock);
       return -1;
     }
       silc_mutex_unlock(pw->wait_lock);
       return -1;
     }