Added IV Included support to allow packet send/receive on UDP.
[silc.git] / lib / silccore / silcpacket.c
index 5f3c55d092ba0c94c75f41ba55c1d2488bc38fcd..5c584d190f2fec507d9e421858d5f45ee9c05ca7 100644 (file)
@@ -51,7 +51,6 @@ struct SilcPacketStreamStruct {
   SilcStream stream;                    /* Underlaying stream */
   SilcMutex lock;                       /* Stream lock */
   SilcDList process;                    /* Packet processors, it set */
-  SilcHashTable streamers;              /* Valid if streamers exist */
   void *stream_context;                         /* Stream context */
   SilcBufferStruct inbuf;               /* In buffer */
   SilcBufferStruct outbuf;              /* Out buffer */
@@ -70,6 +69,7 @@ struct SilcPacketStreamStruct {
   SilcUInt8 refcnt;                     /* Reference counter */
   unsigned int is_router   : 1;                 /* Set if router stream */
   unsigned int destroyed   : 1;                 /* Set if destroyed */
+  unsigned int iv_included : 1;          /* Set if IV included */
 };
 
 /* Initial size of stream buffers */
@@ -78,10 +78,9 @@ struct SilcPacketStreamStruct {
 /* Header length without source and destination ID's. */
 #define SILC_PACKET_HEADER_LEN 10
 
-/* Minimum length of SILC Packet Header. This much is decrypted always
-   when packet is received to be able to get all the relevant data out
-   from the header. */
+/* Minimum length of SILC Packet Header. */
 #define SILC_PACKET_MIN_HEADER_LEN 16
+#define SILC_PACKET_MIN_HEADER_LEN_IV 32
 
 /* Maximum padding length */
 #define SILC_PACKET_MAX_PADLEN 128
@@ -92,9 +91,6 @@ struct SilcPacketStreamStruct {
 /* Minimum packet length */
 #define SILC_PACKET_MIN_LEN (SILC_PACKET_HEADER_LEN + 1)
 
-
-/* Macros */
-
 /* Returns true length of the packet. */
 #define SILC_PACKET_LENGTH(__packetdata, __ret_truelen, __ret_paddedlen) \
 do {                                                                    \
@@ -211,17 +207,15 @@ static void silc_packet_stream_io(SilcStream stream, SilcStreamStatus status,
     break;
 
   case SILC_STREAM_CAN_READ:
-    /* Packet receiving can only happen in one thread for one SilcPacketStream,
-       so locking is not required in packet receiving procedure. */
-    silc_mutex_unlock(ps->lock);
-
     SILC_LOG_DEBUG(("Reading data from 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_PACKET_DEFAULT_SIZE * 2)) {
+       silc_mutex_unlock(ps->lock);
        return;
+      }
 
     /* Read data from stream */
     ret = silc_stream_read(ps->stream, ps->inbuf.tail,
@@ -230,6 +224,7 @@ static void silc_packet_stream_io(SilcStream stream, SilcStreamStatus status,
     if (ret == 0) {
       /* EOS */
       silc_buffer_reset(&ps->inbuf);
+      silc_mutex_unlock(ps->lock);
       SILC_PACKET_CALLBACK_EOS(ps);
       return;
     }
@@ -237,6 +232,7 @@ static void silc_packet_stream_io(SilcStream stream, SilcStreamStatus status,
     if (ret == -2) {
       /* Error */
       silc_buffer_reset(&ps->inbuf);
+      silc_mutex_unlock(ps->lock);
       SILC_PACKET_CALLBACK_ERROR(ps, SILC_PACKET_ERR_READ);
       return;
     }
@@ -244,15 +240,15 @@ static void silc_packet_stream_io(SilcStream stream, SilcStreamStatus status,
     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;
     }
 
-    /* Read some data */
-    silc_buffer_pull_tail(&ps->inbuf, ret);
-
     /* Now process the data */
+    silc_buffer_pull_tail(&ps->inbuf, ret);
     silc_packet_read_process(ps);
 
+    silc_mutex_unlock(ps->lock);
     break;
 
   default:
@@ -446,13 +442,16 @@ void silc_packet_stream_destroy(SilcPacketStream stream)
   /* Clear and free buffers */
   silc_buffer_clear(&stream->inbuf);
   silc_buffer_clear(&stream->outbuf);
-  silc_free(silc_buffer_steal(&stream->inbuf, NULL));
-  silc_free(silc_buffer_steal(&stream->outbuf, NULL));
-
-  silc_dlist_uninit(stream->process);
+  silc_buffer_purge(&stream->inbuf);
+  silc_buffer_purge(&stream->outbuf);
 
   /* XXX */
 
+  /* Destroy the underlaying stream */
+  silc_stream_destroy(stream->stream);
+
+  silc_dlist_uninit(stream->process);
+  silc_mutex_free(stream->lock);
   silc_free(stream);
 }
 
@@ -463,6 +462,12 @@ void silc_packet_stream_set_router(SilcPacketStream stream)
   stream->is_router = TRUE;
 }
 
+/* Mark to include IV in ciphertext */
+
+void silc_packet_stream_set_iv_included(SilcPacketStream stream)
+{
+  stream->iv_included = TRUE;
+}
 
 /* Links `callbacks' to `stream' for specified packet types */
 
@@ -494,8 +499,10 @@ static SilcBool silc_packet_stream_link_va(SilcPacketStream stream,
 
   if (!stream->process) {
     stream->process = silc_dlist_init();
-    if (!stream->process)
+    if (!stream->process) {
+      silc_mutex_unlock(stream->lock);
       return FALSE;
+    }
   }
 
   /* According to priority set the procesor to correct position.  First
@@ -621,14 +628,20 @@ SilcPacketEngine silc_packet_get_engine(SilcPacketStream stream)
 
 void silc_packet_set_context(SilcPacketStream stream, void *stream_context)
 {
+  silc_mutex_lock(stream->lock);
   stream->stream_context = stream_context;
+  silc_mutex_unlock(stream->lock);
 }
 
 /* Return application context from packet stream */
 
 void *silc_packet_get_context(SilcPacketStream stream)
 {
-  return stream->stream_context;
+  void *context;
+  silc_mutex_lock(stream->lock);
+  context = stream->stream_context;
+  silc_mutex_unlock(stream->lock);
+  return context;
 }
 
 /* Return underlaying stream */
@@ -644,8 +657,10 @@ void silc_packet_set_ciphers(SilcPacketStream stream, SilcCipher send,
                             SilcCipher receive)
 {
   SILC_LOG_DEBUG(("Setting new ciphers to packet stream"));
+  silc_mutex_lock(stream->lock);
   stream->send_key = send;
   stream->receive_key = receive;
+  silc_mutex_unlock(stream->lock);
 }
 
 /* Return current ciphers from packet stream */
@@ -656,11 +671,15 @@ SilcBool silc_packet_get_ciphers(SilcPacketStream stream, SilcCipher *send,
   if (!stream->send_key && !stream->receive_key)
     return FALSE;
 
+  silc_mutex_lock(stream->lock);
+
   if (send)
     *send = stream->send_key;
   if (receive)
     *receive = stream->receive_key;
 
+  silc_mutex_unlock(stream->lock);
+
   return TRUE;
 }
 
@@ -670,8 +689,10 @@ 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);
   stream->send_hmac = send;
   stream->receive_hmac = receive;
+  silc_mutex_unlock(stream->lock);
 }
 
 /* Return current HMACs from packet stream */
@@ -682,11 +703,15 @@ SilcBool silc_packet_get_hmacs(SilcPacketStream stream, SilcHmac *send,
   if (!stream->send_hmac && !stream->receive_hmac)
     return FALSE;
 
+  silc_mutex_lock(stream->lock);
+
   if (send)
     *send = stream->send_hmac;
   if (receive)
     *receive = stream->receive_hmac;
 
+  silc_mutex_unlock(stream->lock);
+
   return TRUE;
 }
 
@@ -704,28 +729,40 @@ SilcBool silc_packet_set_ids(SilcPacketStream stream,
 
   SILC_LOG_DEBUG(("Setting new IDs to packet stream"));
 
+  silc_mutex_lock(stream->lock);
+
   if (src_id) {
     silc_free(stream->src_id);
-    if (!silc_id_id2str(src_id, src_id_type, tmp, sizeof(tmp), &len))
+    if (!silc_id_id2str(src_id, src_id_type, tmp, sizeof(tmp), &len)) {
+      silc_mutex_unlock(stream->lock);
       return FALSE;
+    }
     stream->src_id = silc_memdup(tmp, len);
-    if (!stream->src_id)
+    if (!stream->src_id) {
+      silc_mutex_unlock(stream->lock);
       return FALSE;
+    }
     stream->src_id_type = src_id_type;
     stream->src_id_len = len;
   }
 
   if (dst_id) {
     silc_free(stream->dst_id);
-    if (!silc_id_id2str(dst_id, dst_id_type, tmp, sizeof(tmp), &len))
+    if (!silc_id_id2str(dst_id, dst_id_type, tmp, sizeof(tmp), &len)) {
+      silc_mutex_unlock(stream->lock);
       return FALSE;
+    }
     stream->dst_id = silc_memdup(tmp, len);
-    if (!stream->dst_id)
+    if (!stream->dst_id) {
+      silc_mutex_unlock(stream->lock);
       return FALSE;
+    }
     stream->dst_id_type = dst_id_type;
     stream->dst_id_len = len;
   }
 
+  silc_mutex_unlock(stream->lock);
+
   return TRUE;
 }
 
@@ -754,24 +791,6 @@ void silc_packet_free(SilcPacket packet)
   silc_mutex_unlock(stream->engine->lock);
 }
 
-/* Creates streamer */
-
-SilcStream silc_packet_streamer_create(SilcPacketStream stream,
-                                      SilcPacketType packet_type,
-                                      SilcPacketFlags packet_flags)
-{
-  /* XXX TODO */
-  return NULL;
-}
-
-/* Destroyes streamer */
-
-void silc_packet_streamer_destroy(SilcStream stream)
-{
-
-}
-
-
 /****************************** Packet Sending ******************************/
 
 /* Prepare outgoing data buffer for packet sending.  Returns the
@@ -820,9 +839,9 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
                                     SilcCipher cipher,
                                     SilcHmac hmac)
 {
-  unsigned char tmppad[SILC_PACKET_MAX_PADLEN];
+  unsigned char tmppad[SILC_PACKET_MAX_PADLEN], iv[32], psn[4];
   int block_len = (cipher ? silc_cipher_get_block_len(cipher) : 0);
-  int i, enclen, truelen, padlen;
+  int i, enclen, truelen, padlen, ivlen = 0, psnlen = 0;
   SilcBufferStruct packet;
 
   SILC_LOG_DEBUG(("Sending packet %s (%d) flags %d, src %d dst %d,"
@@ -837,6 +856,13 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
   enclen = truelen = (data_len + SILC_PACKET_HEADER_LEN +
                      src_id_len + dst_id_len);
 
+  /* If IV is included, the IV and sequence number is added to packet */
+  if (stream->iv_included && cipher) {
+    ivlen = block_len;
+    psnlen = sizeof(psn);
+    memcpy(iv, silc_cipher_get_iv(cipher), block_len);
+  }
+
   /* We automatically figure out the packet structure from the packet
      type and flags, and calculate correct length.  Private messages with
      private keys and channel messages are special packets as their
@@ -846,21 +872,21 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
       type == SILC_PACKET_CHANNEL_MESSAGE) {
 
     /* Padding is calculated from header + IDs */
-    SILC_PACKET_PADLEN((SILC_PACKET_HEADER_LEN +
-                       src_id_len +
-                       dst_id_len), block_len, padlen);
+    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 + padlen;
+    enclen = (SILC_PACKET_HEADER_LEN + src_id_len + dst_id_len +
+             padlen + psnlen);
   } else {
 
     /* Padding is calculated from true length of the packet */
     if (flags & SILC_PACKET_FLAG_LONG_PAD)
-      SILC_PACKET_PADLEN_MAX(truelen, block_len, padlen);
+      SILC_PACKET_PADLEN_MAX(truelen + psnlen, block_len, padlen);
     else
-      SILC_PACKET_PADLEN(truelen, block_len, padlen);
+      SILC_PACKET_PADLEN(truelen + psnlen, block_len, padlen);
 
-    enclen += padlen;
+    enclen += padlen + psnlen;
   }
 
   /* Remove implementation specific flags */
@@ -873,14 +899,19 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
   silc_mutex_lock(stream->lock);
 
   /* Get packet pointer from the outgoing buffer */
-  if (!silc_packet_send_prepare(stream, truelen + padlen, hmac, &packet)) {
+  if (!silc_packet_send_prepare(stream, truelen + padlen + ivlen + psnlen,
+                               hmac, &packet)) {
     silc_mutex_unlock(stream->lock);
     return FALSE;
   }
 
+  SILC_PUT32_MSB(stream->send_psn, psn);
+
   /* Create the packet.  This creates the SILC header, adds padding, and
      the actual packet data. */
   i = silc_buffer_format(&packet,
+                        SILC_STR_UI_XNSTRING(iv, ivlen),
+                        SILC_STR_UI_XNSTRING(psn, psnlen),
                         SILC_STR_UI_SHORT(truelen),
                         SILC_STR_UI_CHAR(flags),
                         SILC_STR_UI_CHAR(type),
@@ -901,13 +932,13 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
   }
 
   SILC_LOG_HEXDUMP(("Assembled packet, len %d", silc_buffer_len(&packet)),
-                  packet.data, silc_buffer_len(&packet));
+                  silc_buffer_data(&packet), silc_buffer_len(&packet));
 
   /* Encrypt the packet */
   if (cipher) {
     SILC_LOG_DEBUG(("Encrypting packet"));
-    if (!silc_cipher_encrypt(cipher, packet.data, packet.data,
-                            enclen, NULL)) {
+    if (!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;
@@ -916,14 +947,12 @@ static SilcBool silc_packet_send_raw(SilcPacketStream stream,
 
   /* Compute HMAC */
   if (hmac) {
-    unsigned char psn[4];
     SilcUInt32 mac_len;
 
     /* MAC is computed from the entire encrypted packet data, and put
        to the end of the packet. */
     silc_hmac_init(hmac);
-    SILC_PUT32_MSB(stream->send_psn, psn);
-    silc_hmac_update(hmac, psn, 4);
+    silc_hmac_update(hmac, psn, sizeof(psn));
     silc_hmac_update(hmac, packet.data, silc_buffer_len(&packet));
     silc_hmac_final(hmac, packet.tail, &mac_len);
     silc_buffer_pull_tail(&packet, mac_len);
@@ -1005,15 +1034,15 @@ SilcBool silc_packet_send_ext(SilcPacketStream stream,
       return FALSE;
 
   return silc_packet_send_raw(stream, type, flags,
-                             src_id_type,
-                             src_id_data,
-                             src_id_len,
-                             dst_id_type,
-                             dst_id_data,
-                             dst_id_len,
+                             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,
-                             hmac);
+                             cipher ? cipher : stream->send_key,
+                             hmac ? hmac : stream->send_hmac);
 }
 
 
@@ -1025,6 +1054,7 @@ static SilcBool silc_packet_check_mac(SilcHmac hmac,
                                      const unsigned char *data,
                                      SilcUInt32 data_len,
                                      const unsigned char *packet_mac,
+                                     const unsigned char *packet_seq,
                                      SilcUInt32 sequence)
 {
   /* Check MAC */
@@ -1036,8 +1066,13 @@ static SilcBool silc_packet_check_mac(SilcHmac hmac,
 
     /* Compute HMAC of packet */
     silc_hmac_init(hmac);
-    SILC_PUT32_MSB(sequence, psn);
-    silc_hmac_update(hmac, psn, 4);
+
+    if (!packet_seq) {
+      SILC_PUT32_MSB(sequence, psn);
+      silc_hmac_update(hmac, psn, 4);
+    } else
+      silc_hmac_update(hmac, packet_seq, 4);
+
     silc_hmac_update(hmac, data, data_len);
     silc_hmac_final(hmac, mac, &mac_len);
 
@@ -1171,7 +1206,7 @@ static SilcBool silc_packet_parse(SilcPacket packet)
   return TRUE;
 }
 
-/* Dispatch packet to application */
+/* Dispatch packet to application.  Called with stream->lock locked. */
 
 static void silc_packet_dispatch(SilcPacket packet)
 {
@@ -1182,7 +1217,9 @@ static void silc_packet_dispatch(SilcPacket packet)
 
   /* Parse the packet */
   if (!silc_packet_parse(packet)) {
+    silc_mutex_unlock(packet->stream->lock);
     SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_MALFORMED);
+    silc_mutex_lock(packet->stream->lock);
     silc_packet_free(packet);
     return;
   }
@@ -1192,11 +1229,13 @@ static void silc_packet_dispatch(SilcPacket packet)
   if (!stream->process) {
     /* Send to default processor as no others exist */
     SILC_LOG_DEBUG(("Dispatching packet to default callbacks"));
+    silc_mutex_unlock(packet->stream->lock);
     if (!stream->engine->callbacks->
        packet_receive(stream->engine, stream, packet,
                       stream->engine->callback_context,
                       stream->stream_context))
       silc_packet_free(packet);
+    silc_mutex_lock(packet->stream->lock);
     return;
   }
 
@@ -1208,67 +1247,86 @@ static void silc_packet_dispatch(SilcPacket packet)
     if (!default_sent && p->priority <= 0) {
       SILC_LOG_DEBUG(("Dispatching packet to default callbacks"));
       default_sent = TRUE;
+      silc_mutex_unlock(packet->stream->lock);
       if (stream->engine->callbacks->
          packet_receive(stream->engine, stream, packet,
                         stream->engine->callback_context,
                         stream->stream_context)) {
+       silc_mutex_lock(packet->stream->lock);
        return;
       }
+      silc_mutex_lock(packet->stream->lock);
     }
 
     /* Send to processor */
     if (!p->types) {
       /* Send all packet types */
       SILC_LOG_DEBUG(("Dispatching packet to %p callbacks", p->callbacks));
+      silc_mutex_unlock(packet->stream->lock);
       if (p->callbacks->packet_receive(stream->engine, stream, packet,
                                       p->callback_context,
-                                      stream->stream_context))
+                                      stream->stream_context)) {
+       silc_mutex_lock(packet->stream->lock);
        return;
+      }
+      silc_mutex_lock(packet->stream->lock);
     } else {
       /* Send specific types */
-      for (pt = p->types; *pt; pt++)
-       if (*pt == packet->type) {
-         SILC_LOG_DEBUG(("Dispatching packet to %p callbacks",
-                         p->callbacks));
-         if (p->callbacks->packet_receive(stream->engine, stream, packet,
-                                          p->callback_context,
-                                          stream->stream_context))
-           return;
-         break;
+      for (pt = p->types; *pt; pt++) {
+       if (*pt != packet->type)
+         continue;
+       SILC_LOG_DEBUG(("Dispatching packet to %p callbacks", p->callbacks));
+       silc_mutex_unlock(packet->stream->lock);
+       if (p->callbacks->packet_receive(stream->engine, stream, packet,
+                                        p->callback_context,
+                                        stream->stream_context)) {
+         silc_mutex_lock(packet->stream->lock);
+         return;
        }
+       silc_mutex_lock(packet->stream->lock);
+       break;
+      }
     }
   }
 
   if (!default_sent) {
     /* Send to default processor as it has not been sent yet */
     SILC_LOG_DEBUG(("Dispatching packet to default callbacks"));
+    silc_mutex_unlock(packet->stream->lock);
     if (stream->engine->callbacks->
        packet_receive(stream->engine, stream, packet,
                       stream->engine->callback_context,
-                      stream->stream_context))
+                      stream->stream_context)) {
+      silc_mutex_lock(packet->stream->lock);
       return;
+    }
+    silc_mutex_lock(packet->stream->lock);
   }
 
   /* If we got here, no one wanted the packet, so drop it */
   silc_packet_free(packet);
 }
 
-/* Process incoming data and parse packets. */
+/* Process incoming data and parse packets.  Called with stream->lock
+   locked. */
 
 static void silc_packet_read_process(SilcPacketStream stream)
 {
   SilcPacket packet;
   SilcUInt16 packetlen;
-  SilcUInt32 paddedlen, mac_len, block_len;
+  SilcUInt32 paddedlen, mac_len, block_len, ivlen, psnlen;
   unsigned char tmp[SILC_PACKET_MIN_HEADER_LEN], *header;
-  unsigned char iv[SILC_CIPHER_MAX_IV_SIZE];
+  unsigned char iv[SILC_CIPHER_MAX_IV_SIZE], *packet_seq = NULL;
   SilcBool normal = TRUE;
   int ret;
 
   /* Parse the packets from the data */
   while (silc_buffer_len(&stream->inbuf) > 0) {
+    ivlen = psnlen = 0;
 
-    if (silc_buffer_len(&stream->inbuf) < SILC_PACKET_MIN_HEADER_LEN) {
+    if (silc_buffer_len(&stream->inbuf) <
+       stream->iv_included ? SILC_PACKET_MIN_HEADER_LEN :
+       SILC_PACKET_MIN_HEADER_LEN_IV) {
       SILC_LOG_DEBUG(("Partial packet in queue, waiting for the rest"));
       return;
     }
@@ -1281,10 +1339,24 @@ static void silc_packet_read_process(SilcPacketStream stream)
     /* Decrypt first block of the packet to get the length field out */
     if (stream->receive_key) {
       block_len = silc_cipher_get_block_len(stream->receive_key);
-      memcpy(iv, silc_cipher_get_iv(stream->receive_key), block_len);
-      silc_cipher_decrypt(stream->receive_key, stream->inbuf.data,
+
+      if (stream->iv_included) {
+       /* IV is included in the ciphertext */
+       memcpy(iv, stream->inbuf.data, block_len);
+       ivlen = block_len;
+       psnlen = 4;
+      } else
+       memcpy(iv, silc_cipher_get_iv(stream->receive_key), block_len);
+
+      silc_cipher_decrypt(stream->receive_key, stream->inbuf.data + ivlen,
                          tmp, block_len, iv);
+
       header = tmp;
+      if (stream->iv_included) {
+       /* Take sequence number from packet */
+       packet_seq = header;
+       header += 4;
+      }
     } else {
       block_len = SILC_PACKET_MIN_HEADER_LEN;
       header = stream->inbuf.data;
@@ -1296,13 +1368,15 @@ static void silc_packet_read_process(SilcPacketStream stream)
     /* Sanity checks */
     if (packetlen < SILC_PACKET_MIN_LEN) {
       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);
       return;
     }
 
-    if (silc_buffer_len(&stream->inbuf) < paddedlen + mac_len) {
+    if (silc_buffer_len(&stream->inbuf) < paddedlen + ivlen + mac_len) {
       SILC_LOG_DEBUG(("Received partial packet, waiting for the rest "
                      "(%d bytes)",
                      paddedlen + mac_len - silc_buffer_len(&stream->inbuf)));
@@ -1312,9 +1386,12 @@ static void silc_packet_read_process(SilcPacketStream stream)
 
     /* Check MAC of the packet */
     if (!silc_packet_check_mac(stream->receive_hmac, stream->inbuf.data,
-                              paddedlen, stream->inbuf.data + paddedlen,
-                              stream->receive_psn)) {
+                              paddedlen + ivlen,
+                              stream->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_buffer_reset(&stream->inbuf);
       return;
@@ -1323,7 +1400,9 @@ static void silc_packet_read_process(SilcPacketStream stream)
     /* Get packet */
     packet = silc_packet_alloc(stream->engine);
     if (!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_buffer_reset(&stream->inbuf);
       return;
@@ -1335,7 +1414,9 @@ static void silc_packet_read_process(SilcPacketStream stream)
                               silc_buffer_truelen(&packet->buffer) +
                               (paddedlen -
                                silc_buffer_truelen(&packet->buffer)))) {
+       silc_mutex_unlock(stream->lock);
        SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_NO_MEMORY);
+       silc_mutex_lock(stream->lock);
        silc_packet_free(packet);
        memset(tmp, 0, sizeof(tmp));
        silc_buffer_reset(&stream->inbuf);
@@ -1364,21 +1445,24 @@ static void silc_packet_read_process(SilcPacketStream stream)
     }
 
     SILC_LOG_HEXDUMP(("Incoming packet (%d) len %d",
-                     stream->receive_psn, paddedlen + mac_len),
-                    stream->inbuf.data, paddedlen + mac_len);
+                     stream->receive_psn, paddedlen + ivlen + mac_len),
+                    stream->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);
-    silc_buffer_pull(&packet->buffer, block_len);
-    silc_buffer_put(&packet->buffer, stream->inbuf.data + block_len,
-                   paddedlen - block_len);
+    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 +
+                                     psnlen + (block_len - psnlen)),
+                   paddedlen - ivlen - psnlen - (block_len - psnlen));
     if (stream->receive_key) {
       silc_cipher_set_iv(stream->receive_key, iv);
       ret = silc_packet_decrypt(stream->receive_key, stream->receive_hmac,
                                stream->receive_psn, &packet->buffer, normal);
       if (ret < 0) {
+       silc_mutex_unlock(stream->lock);
        SILC_PACKET_CALLBACK_ERROR(stream, SILC_PACKET_ERR_DECRYPTION_FAILED);
+       silc_mutex_lock(stream->lock);
        silc_packet_free(packet);
        memset(tmp, 0, sizeof(tmp));
        return;
@@ -1421,7 +1505,7 @@ typedef struct {
   SilcMutex wait_lock;
   SilcCond wait_cond;
   SilcList packet_queue;
-  SilcBool waiting;
+  unsigned int stopped     : 1;
 } *SilcPacketWait;
 
 /* Packet wait receive callback */
@@ -1438,7 +1522,7 @@ silc_packet_wait_packet_receive(SilcPacketEngine engine,
   /* Signal the waiting thread for a new packet */
   silc_mutex_lock(pw->wait_lock);
 
-  if (!pw->waiting) {
+  if (pw->stopped) {
     silc_mutex_unlock(pw->wait_lock);
     return FALSE;
   }
@@ -1494,11 +1578,18 @@ void *silc_packet_wait_init(SilcPacketStream stream, ...)
 
 /* Uninitialize packet waiting */
 
-void silc_packet_wait_uninit(void *context, SilcPacketStream stream)
+void silc_packet_wait_uninit(void *waiter, SilcPacketStream stream)
 {
-  SilcPacketWait pw = context;
+  SilcPacketWait pw = waiter;
   SilcPacket packet;
 
+  /* Signal any threads to stop waiting */
+  silc_mutex_lock(pw->wait_lock);
+  pw->stopped = TRUE;
+  silc_cond_broadcast(pw->wait_cond);
+  silc_mutex_unlock(pw->wait_lock);
+
+  /* Re-acquire lock and free resources */
   silc_mutex_lock(pw->wait_lock);
   silc_packet_stream_unlink(stream, &silc_packet_wait_cbs, pw);
 
@@ -1508,7 +1599,6 @@ void silc_packet_wait_uninit(void *context, SilcPacketStream stream)
     silc_packet_free(packet);
 
   silc_mutex_unlock(pw->wait_lock);
-
   silc_cond_free(pw->wait_cond);
   silc_mutex_free(pw->wait_lock);
   silc_free(pw);
@@ -1516,17 +1606,21 @@ void silc_packet_wait_uninit(void *context, SilcPacketStream stream)
 
 /* Blocks thread until a packet has been received. */
 
-int silc_packet_wait(void *context, int timeout, SilcPacket *return_packet)
+int silc_packet_wait(void *waiter, int timeout, SilcPacket *return_packet)
 {
-  SilcPacketWait pw = context;
+  SilcPacketWait pw = waiter;
   SilcBool ret = FALSE;
 
   silc_mutex_lock(pw->wait_lock);
 
   /* Wait here until packet has arrived */
-  pw->waiting = TRUE;
-  while (silc_list_count(pw->packet_queue) == 0)
+  while (silc_list_count(pw->packet_queue) == 0) {
+    if (pw->stopped) {
+      silc_mutex_unlock(pw->wait_lock);
+      return -1;
+    }
     ret = silc_cond_timedwait(pw->wait_cond, pw->wait_lock, timeout);
+  }
 
   /* Return packet */
   silc_list_start(pw->packet_queue);