diff --git a/programs/ssl/ssl_server2.c b/programs/ssl/ssl_server2.c
index 5d6fe40..5319c7e 100644
--- a/programs/ssl/ssl_server2.c
+++ b/programs/ssl/ssl_server2.c
@@ -29,27 +29,12 @@
 #if defined(POLARSSL_PLATFORM_C)
 #include "polarssl/platform.h"
 #else
-#define polarssl_printf     printf
-#define polarssl_fprintf    fprintf
-#define polarssl_malloc     malloc
-#define polarssl_free       free
-#endif
-
-#if !defined(POLARSSL_ENTROPY_C) ||  \
-    !defined(POLARSSL_SSL_TLS_C) || !defined(POLARSSL_SSL_SRV_C) || \
-    !defined(POLARSSL_NET_C) || !defined(POLARSSL_CTR_DRBG_C)
 #include <stdio.h>
-int main( int argc, char *argv[] )
-{
-    ((void) argc);
-    ((void) argv);
-
-    polarssl_printf("POLARSSL_ENTROPY_C and/or "
-           "POLARSSL_SSL_TLS_C and/or POLARSSL_SSL_SRV_C and/or "
-           "POLARSSL_NET_C and/or POLARSSL_CTR_DRBG_C not defined.\n");
-    return( 0 );
-}
-#else
+#define polarssl_free       free
+#define polarssl_malloc     malloc
+#define polarssl_fprintf    fprintf
+#define polarssl_printf     printf
+#endif
 
 #if defined(POLARSSL_SSL_SERVER_NAME_INDICATION) && defined(POLARSSL_FS_IO)
 #define POLARSSL_SNI
@@ -59,14 +44,9 @@
 #include <windows.h>
 #endif
 
-#include <string.h>
-#include <stdlib.h>
-#include <stdio.h>
-
-#if !defined(_WIN32)
-#include <signal.h>
-#endif
-
+#if defined(POLARSSL_ENTROPY_C) && \
+    defined(POLARSSL_SSL_TLS_C) && defined(POLARSSL_SSL_SRV_C) && \
+    defined(POLARSSL_NET_C) && defined(POLARSSL_CTR_DRBG_C)
 #include "polarssl/net.h"
 #include "polarssl/ssl.h"
 #include "polarssl/entropy.h"
@@ -76,6 +56,15 @@
 #include "polarssl/error.h"
 #include "polarssl/debug.h"
 
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#endif
+
+#if !defined(_WIN32)
+#include <signal.h>
+#endif
+
 #if defined(POLARSSL_SSL_CACHE_C)
 #include "polarssl/ssl_cache.h"
 #endif
@@ -155,102 +144,6 @@
  */
 #define IO_BUF_LEN      200
 
-/*
- * global options
- */
-struct options
-{
-    const char *server_addr;    /* address on which the ssl service runs    */
-    int server_port;            /* port on which the ssl service runs       */
-    int debug_level;            /* level of debugging                       */
-    int nbio;                   /* should I/O be blocking?                  */
-    uint32_t read_timeout;      /* timeout on ssl_read() in milliseconds    */
-    const char *ca_file;        /* the file with the CA certificate(s)      */
-    const char *ca_path;        /* the path with the CA certificate(s) reside */
-    const char *crt_file;       /* the file with the server certificate     */
-    const char *key_file;       /* the file with the server key             */
-    const char *crt_file2;      /* the file with the 2nd server certificate */
-    const char *key_file2;      /* the file with the 2nd server key         */
-    const char *psk;            /* the pre-shared key                       */
-    const char *psk_identity;   /* the pre-shared key identity              */
-    char *psk_list;             /* list of PSK id/key pairs for callback    */
-    int force_ciphersuite[2];   /* protocol/ciphersuite to use, or all      */
-    const char *version_suites; /* per-version ciphersuites                 */
-    int renegotiation;          /* enable / disable renegotiation           */
-    int allow_legacy;           /* allow legacy renegotiation               */
-    int renegotiate;            /* attempt renegotiation?                   */
-    int renego_delay;           /* delay before enforcing renegotiation     */
-    int renego_period;          /* period for automatic renegotiation       */
-    int exchanges;              /* number of data exchanges                 */
-    int min_version;            /* minimum protocol version accepted        */
-    int max_version;            /* maximum protocol version accepted        */
-    int arc4;                   /* flag for arc4 suites support             */
-    int auth_mode;              /* verify mode for connection               */
-    unsigned char mfl_code;     /* code for maximum fragment length         */
-    int trunc_hmac;             /* accept truncated hmac?                   */
-    int tickets;                /* enable / disable session tickets         */
-    int ticket_timeout;         /* session ticket lifetime                  */
-    int cache_max;              /* max number of session cache entries      */
-    int cache_timeout;          /* expiration delay of session cache entries */
-    char *sni;                  /* string describing sni information        */
-    const char *alpn_string;    /* ALPN supported protocols                 */
-    const char *dhm_file;       /* the file with the DH parameters          */
-    int extended_ms;            /* allow negotiation of extended MS?        */
-    int etm;                    /* allow negotiation of encrypt-then-MAC?   */
-    int transport;              /* TLS or DTLS?                             */
-    int cookies;                /* Use cookies for DTLS? -1 to break them   */
-    int anti_replay;            /* Use anti-replay for DTLS? -1 for default */
-    uint32_t hs_to_min;         /* Initial value of DTLS handshake timer    */
-    uint32_t hs_to_max;         /* Max value of DTLS handshake timer        */
-    int badmac_limit;           /* Limit of records with bad MAC            */
-} opt;
-
-static void my_debug( void *ctx, int level, const char *str )
-{
-    ((void) level);
-
-    polarssl_fprintf( (FILE *) ctx, "%s", str );
-    fflush(  (FILE *) ctx  );
-}
-
-/*
- * Test recv/send functions that make sure each try returns
- * WANT_READ/WANT_WRITE at least once before sucesseding
- */
-static int my_recv( void *ctx, unsigned char *buf, size_t len )
-{
-    static int first_try = 1;
-    int ret;
-
-    if( first_try )
-    {
-        first_try = 0;
-        return( POLARSSL_ERR_NET_WANT_READ );
-    }
-
-    ret = net_recv( ctx, buf, len );
-    if( ret != POLARSSL_ERR_NET_WANT_READ )
-        first_try = 1; /* Next call will be a new operation */
-    return( ret );
-}
-
-static int my_send( void *ctx, const unsigned char *buf, size_t len )
-{
-    static int first_try = 1;
-    int ret;
-
-    if( first_try )
-    {
-        first_try = 0;
-        return( POLARSSL_ERR_NET_WANT_WRITE );
-    }
-
-    ret = net_send( ctx, buf, len );
-    if( ret != POLARSSL_ERR_NET_WANT_WRITE )
-        first_try = 1; /* Next call will be a new operation */
-    return( ret );
-}
-
 #if defined(POLARSSL_X509_CRT_PARSE_C)
 #if defined(POLARSSL_FS_IO)
 #define USAGE_IO \
@@ -434,6 +327,114 @@
     "    force_ciphersuite=<name>    default: all enabled\n"            \
     " acceptable ciphersuite names:\n"
 
+#if !defined(POLARSSL_ENTROPY_C) || \
+    !defined(POLARSSL_SSL_TLS_C) || !defined(POLARSSL_SSL_SRV_C) || \
+    !defined(POLARSSL_NET_C) || !defined(POLARSSL_CTR_DRBG_C)
+#include <stdio.h>
+int main( void )
+{
+    polarssl_printf("POLARSSL_ENTROPY_C and/or "
+           "POLARSSL_SSL_TLS_C and/or POLARSSL_SSL_SRV_C and/or "
+           "POLARSSL_NET_C and/or POLARSSL_CTR_DRBG_C not defined.\n");
+    return( 0 );
+}
+#else
+/*
+ * global options
+ */
+struct options
+{
+    const char *server_addr;    /* address on which the ssl service runs    */
+    int server_port;            /* port on which the ssl service runs       */
+    int debug_level;            /* level of debugging                       */
+    int nbio;                   /* should I/O be blocking?                  */
+    uint32_t read_timeout;      /* timeout on ssl_read() in milliseconds    */
+    const char *ca_file;        /* the file with the CA certificate(s)      */
+    const char *ca_path;        /* the path with the CA certificate(s) reside */
+    const char *crt_file;       /* the file with the server certificate     */
+    const char *key_file;       /* the file with the server key             */
+    const char *crt_file2;      /* the file with the 2nd server certificate */
+    const char *key_file2;      /* the file with the 2nd server key         */
+    const char *psk;            /* the pre-shared key                       */
+    const char *psk_identity;   /* the pre-shared key identity              */
+    char *psk_list;             /* list of PSK id/key pairs for callback    */
+    int force_ciphersuite[2];   /* protocol/ciphersuite to use, or all      */
+    const char *version_suites; /* per-version ciphersuites                 */
+    int renegotiation;          /* enable / disable renegotiation           */
+    int allow_legacy;           /* allow legacy renegotiation               */
+    int renegotiate;            /* attempt renegotiation?                   */
+    int renego_delay;           /* delay before enforcing renegotiation     */
+    int renego_period;          /* period for automatic renegotiation       */
+    int exchanges;              /* number of data exchanges                 */
+    int min_version;            /* minimum protocol version accepted        */
+    int max_version;            /* maximum protocol version accepted        */
+    int arc4;                   /* flag for arc4 suites support             */
+    int auth_mode;              /* verify mode for connection               */
+    unsigned char mfl_code;     /* code for maximum fragment length         */
+    int trunc_hmac;             /* accept truncated hmac?                   */
+    int tickets;                /* enable / disable session tickets         */
+    int ticket_timeout;         /* session ticket lifetime                  */
+    int cache_max;              /* max number of session cache entries      */
+    int cache_timeout;          /* expiration delay of session cache entries */
+    char *sni;                  /* string describing sni information        */
+    const char *alpn_string;    /* ALPN supported protocols                 */
+    const char *dhm_file;       /* the file with the DH parameters          */
+    int extended_ms;            /* allow negotiation of extended MS?        */
+    int etm;                    /* allow negotiation of encrypt-then-MAC?   */
+    int transport;              /* TLS or DTLS?                             */
+    int cookies;                /* Use cookies for DTLS? -1 to break them   */
+    int anti_replay;            /* Use anti-replay for DTLS? -1 for default */
+    uint32_t hs_to_min;         /* Initial value of DTLS handshake timer    */
+    uint32_t hs_to_max;         /* Max value of DTLS handshake timer        */
+    int badmac_limit;           /* Limit of records with bad MAC            */
+} opt;
+
+static void my_debug( void *ctx, int level, const char *str )
+{
+    ((void) level);
+
+    polarssl_fprintf( (FILE *) ctx, "%s", str );
+    fflush(  (FILE *) ctx  );
+}
+
+/*
+ * Test recv/send functions that make sure each try returns
+ * WANT_READ/WANT_WRITE at least once before sucesseding
+ */
+static int my_recv( void *ctx, unsigned char *buf, size_t len )
+{
+    static int first_try = 1;
+    int ret;
+
+    if( first_try )
+    {
+        first_try = 0;
+        return( POLARSSL_ERR_NET_WANT_READ );
+    }
+
+    ret = net_recv( ctx, buf, len );
+    if( ret != POLARSSL_ERR_NET_WANT_READ )
+        first_try = 1; /* Next call will be a new operation */
+    return( ret );
+}
+
+static int my_send( void *ctx, const unsigned char *buf, size_t len )
+{
+    static int first_try = 1;
+    int ret;
+
+    if( first_try )
+    {
+        first_try = 0;
+        return( POLARSSL_ERR_NET_WANT_WRITE );
+    }
+
+    ret = net_send( ctx, buf, len );
+    if( ret != POLARSSL_ERR_NET_WANT_WRITE )
+        first_try = 1; /* Next call will be a new operation */
+    return( ret );
+}
+
 /*
  * Used by sni_parse and psk_parse to handle coma-separated lists
  */
@@ -441,7 +442,7 @@
     dst = p;                    \
     while( *p != ',' )          \
         if( ++p > end )         \
-            return( NULL );     \
+            goto error;         \
     *p++ = '\0';
 
 #if defined(POLARSSL_SNI)
@@ -454,53 +455,6 @@
     sni_entry *next;
 };
 
-/*
- * Parse a string of triplets name1,crt1,key1[,name2,crt2,key2[,...]]
- * into a usable sni_entry list.
- *
- * Modifies the input string! This is not production quality!
- * (leaks memory if parsing fails, no error reporting, ...)
- */
-sni_entry *sni_parse( char *sni_string )
-{
-    sni_entry *cur = NULL, *new = NULL;
-    char *p = sni_string;
-    char *end = p;
-    char *crt_file, *key_file;
-
-    while( *end != '\0' )
-        ++end;
-    *end = ',';
-
-    while( p <= end )
-    {
-        if( ( new = polarssl_malloc( sizeof( sni_entry ) ) ) == NULL )
-            return( NULL );
-
-        memset( new, 0, sizeof( sni_entry ) );
-
-        if( ( new->cert = polarssl_malloc( sizeof( x509_crt ) ) ) == NULL ||
-            ( new->key = polarssl_malloc( sizeof( pk_context ) ) ) == NULL )
-            return( NULL );
-
-        x509_crt_init( new->cert );
-        pk_init( new->key );
-
-        GET_ITEM( new->name );
-        GET_ITEM( crt_file );
-        GET_ITEM( key_file );
-
-        if( x509_crt_parse_file( new->cert, crt_file ) != 0 ||
-            pk_parse_keyfile( new->key, key_file, "" ) != 0 )
-            return( NULL );
-
-        new->next = cur;
-        cur = new;
-    }
-
-    return( cur );
-}
-
 void sni_free( sni_entry *head )
 {
     sni_entry *cur = head, *next;
@@ -520,6 +474,67 @@
 }
 
 /*
+ * Parse a string of triplets name1,crt1,key1[,name2,crt2,key2[,...]]
+ * into a usable sni_entry list.
+ *
+ * Modifies the input string! This is not production quality!
+ */
+sni_entry *sni_parse( char *sni_string )
+{
+    sni_entry *cur = NULL, *new = NULL;
+    char *p = sni_string;
+    char *end = p;
+    char *crt_file, *key_file;
+
+    while( *end != '\0' )
+        ++end;
+    *end = ',';
+
+    while( p <= end )
+    {
+        if( ( new = polarssl_malloc( sizeof( sni_entry ) ) ) == NULL )
+        {
+            sni_free( cur );
+            return( NULL );
+        }
+
+        memset( new, 0, sizeof( sni_entry ) );
+
+        if( ( new->cert = polarssl_malloc( sizeof( x509_crt ) ) ) == NULL ||
+            ( new->key = polarssl_malloc( sizeof( pk_context ) ) ) == NULL )
+        {
+            polarssl_free( new->cert );
+            polarssl_free( new );
+            sni_free( cur );
+            return( NULL );
+        }
+
+        x509_crt_init( new->cert );
+        pk_init( new->key );
+
+        GET_ITEM( new->name );
+        GET_ITEM( crt_file );
+        GET_ITEM( key_file );
+
+        if( x509_crt_parse_file( new->cert, crt_file ) != 0 ||
+            pk_parse_keyfile( new->key, key_file, "" ) != 0 )
+        {
+            goto error;
+        }
+
+        new->next = cur;
+        cur = new;
+    }
+
+    return( cur );
+
+error:
+    sni_free( new );
+    sni_free( cur );
+    return( NULL );
+}
+
+/*
  * SNI callback.
  */
 int sni_callback( void *p_info, ssl_context *ssl,
@@ -594,11 +609,25 @@
 };
 
 /*
+ * Free a list of psk_entry's
+ */
+void psk_free( psk_entry *head )
+{
+    psk_entry *next;
+
+    while( head != NULL )
+    {
+        next = head->next;
+        polarssl_free( head );
+        head = next;
+    }
+}
+
+/*
  * Parse a string of pairs name1,key1[,name2,key2[,...]]
  * into a usable psk_entry list.
  *
  * Modifies the input string! This is not production quality!
- * (leaks memory if parsing fails, no error reporting, ...)
  */
 psk_entry *psk_parse( char *psk_string )
 {
@@ -622,28 +651,18 @@
         GET_ITEM( key_hex );
 
         if( unhexify( new->key, key_hex, &new->key_len ) != 0 )
-            return( NULL );
+            goto error;
 
         new->next = cur;
         cur = new;
     }
 
     return( cur );
-}
 
-/*
- * Free a list of psk_entry's
- */
-void psk_free( psk_entry *head )
-{
-    psk_entry *next;
-
-    while( head != NULL )
-    {
-        next = head->next;
-        polarssl_free( head );
-        head = next;
-    }
+error:
+    psk_free( new );
+    psk_free( cur );
+    return( 0 );
 }
 
 /*