zephyr/net/ip/tinydtls/tests/secure-server.c

/* secure-server -- A (broken) DTLS server example
 *
 * Copyright (C) 2011 Olaf Bergmann <bergmann@tzi.org>
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy,
 * modify, merge, publish, distribute, sublicense, and/or sell copies
 * of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdio.h>
#include <ctype.h>
#include <sys/select.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <sys/stat.h>
#include <errno.h>
#include <signal.h>

#include <openssl/ssl.h>
#include <openssl/bio.h>
#include <openssl/err.h>
#include <openssl/rand.h>

#ifdef WITH_DTLS
#define SERVER_CERT_PEM "./server-cert.pem"
#define SERVER_KEY_PEM  "./server-key.pem"
#define CA_CERT_PEM     "./ca-cert.pem"
#endif

#ifdef HAVE_ASSERT_H
# include <assert.h>
#else
# define assert(x)
#endif /* HAVE_ASSERT_H */

static int quit=0;

/* SIGINT handler: set quit to 1 for graceful termination */
void
handle_sigint(int signum) {
  quit = 1;
}

int 
check_connect(int sockfd, char *buf, int buflen, 
	   struct sockaddr *src, int *ifindex) {

  /* for some reason, the definition in netinet/in.h is not exported */
#ifndef IN6_PKTINFO
  struct in6_pktinfo
  {
    struct in6_addr ipi6_addr;	/* src/dst IPv6 address */
    unsigned int ipi6_ifindex;	/* send/recv interface index */
  };
#endif

  size_t bytes;

  struct iovec iov[1] = { {buf, buflen} };
  char cmsgbuf[CMSG_SPACE(sizeof(struct in6_pktinfo))];
  struct in6_pktinfo *p = NULL;
  
  struct msghdr msg = { 0 };
  struct cmsghdr *cmsg;

  msg.msg_name = src;
  msg.msg_namelen = sizeof(struct sockaddr_in6);
  msg.msg_iov = iov;
  msg.msg_iovlen = 1;
  msg.msg_control = cmsgbuf;
  msg.msg_controllen = sizeof(cmsgbuf);

  bytes = recvmsg(sockfd, &msg, MSG_DONTWAIT | MSG_PEEK);
  if (bytes < 0) {
    perror("recvmsg");
    return bytes;
  }

  /* TODO: handle msg.msg_flags & MSG_TRUNC */
  if (msg.msg_flags & MSG_CTRUNC) {
    fprintf(stderr, "control was truncated!\n");
    return -1;
  }

  if (ifindex) {
    /* Here we try to retrieve the interface index where the packet was received */
    *ifindex = 0;
    for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
	 cmsg = CMSG_NXTHDR(&msg, cmsg)) {

      if (cmsg->cmsg_level == IPPROTO_IPV6 && cmsg->cmsg_type == IPV6_PKTINFO) {
	p = (struct in6_pktinfo *)(CMSG_DATA(cmsg));
	*ifindex = p->ipi6_ifindex;
	break;
      }
    }
  }

  return bytes;
}

typedef enum { UNKNOWN=0, DTLS=1 } protocol_t;

protocol_t
demux_protocol(const char *buf, int len) {
  return DTLS;
}

#ifdef WITH_DTLS
typedef enum { 
  PEER_ST_ESTABLISHED, PEER_ST_PENDING, PEER_ST_CLOSED 
 } peer_state_t;
typedef struct {
  peer_state_t state;
  unsigned long h;
  SSL *ssl;
} ssl_peer_t;

#define MAX_SSL_PENDING      2	/* must be less than MAX_SSL_PEERS */
#define MAX_SSL_PEERS       10	/* MAX_SSL_PENDING of these might be pending  */
ssl_peer_t *ssl_peer_storage[MAX_SSL_PEERS];
static int pending = 0;

void
check_peers() {
typedef struct bio_dgram_data_st
	{
	union {
		struct sockaddr sa;
		struct sockaddr_in sa_in;
		struct sockaddr_in6 sa_in6;
	} peer;
	unsigned int connected;
	unsigned int _errno;
	unsigned int mtu;
	struct timeval next_timeout;
	struct timeval socket_timeout;
	} bio_dgram_data;

  struct sockaddr_in6 peer;
  int i;
  BIO *bio;
  for (i = 0; i < MAX_SSL_PEERS; i++) {
    if (ssl_peer_storage[i]) {
      if (!ssl_peer_storage[i]->ssl)
	fprintf(stderr, "invalid SSL object for peer %d!\n",i);
      else {
	bio = SSL_get_rbio(ssl_peer_storage[i]->ssl);
	if (bio) {
	  (void) BIO_dgram_get_peer(bio, (struct sockaddr *)&peer);
	  if (peer.sin6_port && ssl_peer_storage[i]->h != ntohs(peer.sin6_port)) {
	    fprintf(stderr, "   bio %p: port differs from hash: %d != %d! (%sconnected)\n", bio,
		    ssl_peer_storage[i]->h, 
		    ntohs(((struct sockaddr_in6 *)&peer)->sin6_port),
		    ((bio_dgram_data *)bio->ptr)->connected ? "" : "not ");
	  }

	}
      }
    }
  }
}

/** Creates a hash value from the first num bytes of s, taking init as
 * initialization value. */
static inline unsigned long
_hash(unsigned long init, const char *s, int num) {
  int c;

  while (num--)
    while ( (c = *s++) ) {
      init = ((init << 7) + init) + c;
    }

  return init;
}

static inline unsigned long
hash_peer(const struct sockaddr *peer, int ifindex) {
  unsigned long h;

  /* initialize hash value to interface index */
  h = _hash(0, (char *)&ifindex, sizeof(int));

#define CAST(TYPE,VAR) ((TYPE)VAR)

  assert(peer);
  switch (peer->sa_family) {
  case AF_INET: 
    return ntohs(CAST(const struct sockaddr_in *, peer)->sin_port);
    h = _hash(h, (char *) &CAST(const struct sockaddr_in *, peer)->sin_addr, 
	      sizeof(struct in_addr));
    h = _hash(h, (char *) &CAST(const struct sockaddr_in *, peer)->sin_port, 
	      sizeof(in_port_t));
    break;
  case AF_INET6:
    return ntohs(CAST(const struct sockaddr_in6 *, peer)->sin6_port);
    h = _hash(h, 
	      (char *) &CAST(const struct sockaddr_in6 *, peer)->sin6_addr, 
	      sizeof(struct in6_addr));
    h = _hash(h, 
	      (char *) &CAST(const struct sockaddr_in6 *, peer)->sin6_port, 
	      sizeof(in_port_t));
    break;
  default:
    /* last resort */
    h = _hash(h, (char *)peer, sizeof(struct sockaddr));
  }

  return 42;
  return h;
}

/* Returns index of peer object for specified address/ifindex pair. */
int
get_index_of_peer(const struct sockaddr *peer, int ifindex) {
  unsigned long h;
  int idx;
#ifndef NDEBUG
  char addr[INET6_ADDRSTRLEN];
  char port[6];
#endif

  if (!peer)
    return -1;

  h = hash_peer(peer,ifindex);

  for (idx = 0; idx < MAX_SSL_PEERS; idx++) {
    if (ssl_peer_storage[idx] && ssl_peer_storage[idx]->h == h) {
#ifndef NDEBUG
      getnameinfo((struct sockaddr *)peer, sizeof(struct sockaddr_in6), 
		  addr, sizeof(addr), port, sizeof(port), 
		  NI_NUMERICHOST | NI_NUMERICSERV);

      fprintf(stderr, "get_index_of_peer: [%s]:%s  =>  %lu\n",
	      addr, port, h);
#endif
      return idx;
    }
  }
  return -1;
}

SSL *
get_ssl(SSL_CTX *ctx, int sockfd, struct sockaddr *src, int ifindex) {
  int idx;
  BIO *bio;
  SSL *ssl;
#ifndef NDEBUG
  struct sockaddr_storage peer;
  char addr[INET6_ADDRSTRLEN];
  char port[6];
  int i;
#endif

  idx = get_index_of_peer(src,ifindex);
  if (idx >= 0) {
    fprintf(stderr,"found peer %d ",idx);
    switch (ssl_peer_storage[idx]->state) {
    case PEER_ST_ESTABLISHED: fprintf(stderr,"established\n"); break;
    case PEER_ST_PENDING:     fprintf(stderr,"pending\n"); break;
    case PEER_ST_CLOSED:      fprintf(stderr,"closed\n"); break;
    default:
      OPENSSL_assert(0);
    }

#ifndef NDEBUG
    memset(&peer, 0, sizeof(peer));
    (void) BIO_dgram_get_peer(SSL_get_rbio(ssl_peer_storage[idx]->ssl), &peer);

    getnameinfo((struct sockaddr *)&peer, sizeof(peer), 
		addr, sizeof(addr), port, sizeof(port), 
		NI_NUMERICHOST | NI_NUMERICSERV);

    fprintf(stderr,"      [%s]:%s   \n", addr, port);
#endif
    return ssl_peer_storage[idx]->ssl;
  }

  /* none found, create new if sufficient space available */
  if (pending < MAX_SSL_PENDING) {
    for (idx = 0; idx < MAX_SSL_PEERS; idx++) {
      if (ssl_peer_storage[idx] == NULL) { /* found space */
	ssl = SSL_new(ctx);
	
	if (ssl) {
	  bio = BIO_new_dgram(sockfd, BIO_NOCLOSE);
	  if (!bio) {
	    SSL_free(ssl);
	    return NULL;
	  }
	  
	  SSL_set_bio(ssl, bio, bio);
	  SSL_set_options(ssl, SSL_OP_COOKIE_EXCHANGE);
	  
	  SSL_set_accept_state(ssl);
	  ssl_peer_storage[idx] = (ssl_peer_t *) malloc(sizeof(ssl_peer_t));
	  if (!ssl_peer_storage[idx]) {
	    SSL_free(ssl);
	    return NULL;
	  }
	  ssl_peer_storage[idx]->state = PEER_ST_PENDING;
	  ssl_peer_storage[idx]->h = hash_peer(src,ifindex);
	  ssl_peer_storage[idx]->ssl = ssl;
	  
	  pending++;
	  
	  fprintf(stderr,
		  "created new SSL peer %d for ssl object %p (storage: %p)\n", 
		 idx, ssl, ssl_peer_storage[idx]);
#ifndef NDEBUG
    if (getnameinfo((struct sockaddr *)&src, sizeof(src), 
		addr, sizeof(addr), port, sizeof(port), 
		    NI_NUMERICHOST | NI_NUMERICSERV) != 0) {
      perror("getnameinfo");
      fprintf(stderr, "port was %u\n", ntohs(((struct sockaddr_in6 *)src)->sin6_port));
    } else {
    fprintf(stderr,"      [%s]:%s   \n", addr, port);
      }
#endif
    OPENSSL_assert(ssl_peer_storage[idx]->ssl == ssl);
	  fprintf(stderr,"%d objects pending\n", pending);
	  check_peers();
	  return ssl;
	}
      }
    }
  } else {
    fprintf(stderr, "too many pending SSL objects\n");
    return NULL;
  }

  fprintf(stderr, "too many peers\n");
  return NULL;
}

/** Deletes peer stored at index idx and frees allocated memory. */
static inline void
delete_peer(int idx) {
  if (idx < 0 || !ssl_peer_storage[idx])
    return;

  if (ssl_peer_storage[idx]->state == PEER_ST_PENDING)
    pending--;

  OPENSSL_assert(ssl_peer_storage[idx]->ssl);
  SSL_free(ssl_peer_storage[idx]->ssl);
    
  free(ssl_peer_storage[idx]);
  ssl_peer_storage[idx] = NULL;

  printf("deleted peer %d\n",idx);
}

/** Deletes all closed objects from ssl_peer_storage. */
void
remove_closed() {
  int idx;

  for (idx = 0; idx < MAX_SSL_PEERS; idx++)
    if (ssl_peer_storage[idx] 
	&& ssl_peer_storage[idx]->state == PEER_ST_CLOSED)
      delete_peer(idx);
}

#define min(a,b) ((a) < (b) ? (a) : (b))

unsigned int
psk_server_callback(SSL *ssl, const char *identity,
		    unsigned char *psk, unsigned int max_psk_len) {
  static char keybuf[] = "secretPSK";

  printf("psk_server_callback: check identity of client %s\n", identity);
  memcpy(psk, keybuf, min(strlen(keybuf), max_psk_len));

  return min(strlen(keybuf), max_psk_len);
}

#endif

#ifdef WITH_DTLS
/**
 * This function tracks the status changes from libssl to manage local
 * object state.
 */
void
info_callback(const SSL *ssl, int where, int ret) {
  int idx, i;
  struct sockaddr_storage peer;
  struct sockaddr_storage peer2;
  char addr[INET6_ADDRSTRLEN];
  char port[6];

  if (where & SSL_CB_LOOP)  /* do not care for intermediary states */
    return;

  memset(&peer, 0, sizeof(peer));
  (void) BIO_dgram_get_peer(SSL_get_rbio(ssl), &peer);

  /* lookup SSL object */   /* FIXME: need to get the ifindex */
  idx = get_index_of_peer((struct sockaddr *)&peer, 0);
  
  if (idx >= 0)
    fprintf(stderr, "info_callback: assert: %d < 0 || %p == %p (storage: %p)\n",
	    idx, ssl, ssl_peer_storage[idx]->ssl, ssl_peer_storage[idx]); 
  if (idx >= 0 && ssl != ssl_peer_storage[idx]->ssl) {
    getnameinfo((struct sockaddr *)&peer, sizeof(peer), 
		addr, sizeof(addr), port, sizeof(port), 
		NI_NUMERICHOST | NI_NUMERICSERV);

    fprintf(stderr," ssl: [%s]:%s   ", addr, port);
    
    (void) BIO_dgram_get_peer(SSL_get_rbio(ssl_peer_storage[idx]->ssl), &peer2);
    getnameinfo((struct sockaddr *)&peer2, sizeof(peer2), 
		addr, sizeof(addr), port, sizeof(port), 
		NI_NUMERICHOST | NI_NUMERICSERV);

    fprintf(stderr," ssl_peer_storage[idx]->ssl: [%s]:%s\n", addr, port);

    fprintf(stderr, " hash:%lu     h: %lu\n",
	    hash_peer((const struct sockaddr *)&peer, 0),
	    ssl_peer_storage[idx]->h);

    for (i = 0; i < MAX_SSL_PEERS; i++) {
      if (ssl_peer_storage[i]) {
	fprintf(stderr, "%02d: %p ssl: %p  ",
		i, ssl_peer_storage[i] ,ssl_peer_storage[i]->ssl);

	(void) BIO_dgram_get_peer(SSL_get_rbio(ssl_peer_storage[i]->ssl), &peer2);
	getnameinfo((struct sockaddr *)&peer2, sizeof(peer2), 
		    addr, sizeof(addr), port, sizeof(port), 
		    NI_NUMERICHOST | NI_NUMERICSERV);
	
	fprintf(stderr," peer: [%s]:%s    h: %lu\n", addr, port, ssl_peer_storage[i]->h);
      }
    }
    fprintf(stderr, "***** ASSERT FAILED ******\n");

    memset(&peer, 0, sizeof(peer));
    (void) BIO_dgram_get_peer(SSL_get_wbio(ssl), &peer);

    idx = get_index_of_peer((struct sockaddr *)&peer, 0);
    fprintf(stderr, "  get_index_of_peer for wbio returns %d, type is %04x\n",
	    idx, where);    
  }
#if 1
	  check_peers();
  OPENSSL_assert((idx < 0) || (ssl == ssl_peer_storage[idx]->ssl));
#endif

  if (where & SSL_CB_ALERT) {
#ifndef NDEBUG
    if (ret != 0)
      fprintf(stderr,"%s:%s:%s\n", SSL_alert_type_string(ret),
	      SSL_alert_desc_string(ret), SSL_alert_desc_string_long(ret));
#endif

    /* examine alert type */
    switch (*SSL_alert_type_string(ret)) {
    case 'F':
      /* move SSL object from pending to close */
      if (idx >= 0) {
	ssl_peer_storage[idx]->state = PEER_ST_CLOSED;
	pending--;
      }
      break;
    case 'W': 
      if ((ret & 0xff) == SSL_AD_CLOSE_NOTIFY) {
	if (where == SSL_CB_WRITE_ALERT) 
	  fprintf(stderr,"sent CLOSE_NOTIFY\n");
	else /* received CN */
	  fprintf(stderr,"received CLOSE_NOTIFY\n");
      }
      break;
    default: 			/* handle unknown alert types */
#ifndef NDEBUG
      printf("not handled!\n");
#endif
    }
  }

  if (where & SSL_CB_HANDSHAKE_DONE) {
    /* move SSL object from pending to established */
    printf("HANDSHAKE_DONE ");
    if (idx >= 0) {
      
      if (ssl_peer_storage[idx]->state == PEER_ST_PENDING) {
	ssl_peer_storage[idx]->state = PEER_ST_ESTABLISHED;
	pending--;
	printf("moved SSL object %d to ESTABLISHED\n", idx);
	printf("%d objects pending\n", pending);
      } else {
#ifndef NDEBUG
	printf("huh, object %d was not pending? (%d)\n", idx,
	       ssl_peer_storage[idx]->state);
#endif
      }
      return;
    }
    return;
  }

  return;
}
#endif

#ifdef WITH_DTLS
/* checks if ssl object was closed and can be removed */
int 
check_close(SSL *ssl) {
  int res, err, idx;
  struct sockaddr_storage peer;
  
  memset(&peer, 0, sizeof(peer));
  (void)BIO_dgram_get_peer(SSL_get_rbio(ssl), &peer);

  res = 0;
  if (SSL_get_shutdown(ssl) & SSL_RECEIVED_SHUTDOWN) {
    printf("SSL_RECEIVED_SHUTDOWN\n");
    res = SSL_shutdown(ssl);
    if (res == 0) {
      printf("must call SSL_shutdown again\n");
      res = SSL_shutdown(ssl);
    }
    if (res < 0) {
	err = SSL_get_error(ssl,res);	
	fprintf(stderr, "shutdown: SSL error %d: %s\n", err,
		ERR_error_string(err, NULL));
    } 

    /* we can close the SSL object anyway */
    /* FIXME: need to get ifindex from somewhere */
    idx = get_index_of_peer((struct sockaddr *)&peer, 0);
    OPENSSL_assert(idx < 0 || ssl == ssl_peer_storage[idx]->ssl);
    if (idx >= 0) {
      ssl_peer_storage[idx]->state = PEER_ST_CLOSED;
      printf("moved SSL object %d to CLOSED\n",idx);
    }
  }
  
  return res;
}

int 
check_timeout() {
  int i, result, err;

  for (i = 0; i < MAX_SSL_PEERS; i++) {
    if (ssl_peer_storage[i]) {
      OPENSSL_assert(ssl_peer_storage[i]->ssl);
      result = DTLSv1_handle_timeout(ssl_peer_storage[i]->ssl);
      if (result < 0) {
	err = SSL_get_error(ssl_peer_storage[i]->ssl,result);
	fprintf(stderr, "dtls1_handle_timeout (%d): %s\n",
		err, ERR_error_string(err, NULL));
      }
    }
  }

  /* remove outdated obbjects? */
  
  return 0;
}
#endif /* WITH_DTLS */
  
int 
_read(SSL_CTX *ctx, int sockfd) {
  char buf[2000];
  struct sockaddr_in6 src;
  int len, ifindex, i;
  char addr[INET6_ADDRSTRLEN];
  char port[6];
  socklen_t sz = sizeof(struct sockaddr_in6);
#ifdef WITH_DTLS
  SSL *ssl;
  int err;
#endif

  /* Retrieve remote address and interface index as well as the first
     few bytes of the message to demultiplex protocols. */
  memset(&src, 0, sizeof(struct sockaddr_in6));
  len = check_connect(sockfd, buf, 4, (struct sockaddr *)&src, &ifindex);

  if (len < 0)			/* error */
    return len;

#ifndef NDEBUG
  fprintf(stderr,"received packet");
  
  if (getnameinfo((struct sockaddr *)&src, sizeof(src), 
		  addr, sizeof(addr), port, sizeof(port), 
		  NI_NUMERICHOST | NI_NUMERICSERV) == 0)
    fprintf(stderr," from [%s]:%s", addr, port);
  
  fprintf(stderr," on interface %d\n", ifindex);
#endif

  switch (demux_protocol(buf, len)) {
#ifdef WITH_DTLS
  case DTLS :
    ssl = get_ssl(ctx, sockfd, (struct sockaddr *)&src, ifindex);
    if (!ssl) {
      fprintf(stderr, "cannot create new SSL object\n");
      /*      return recv(sockfd, buf, sizeof(buf), MSG_DONTWAIT);*/
      len = recvfrom(sockfd, buf, sizeof(buf), MSG_DONTWAIT,
		     (struct sockaddr *)&src, &sz);
      getnameinfo((struct sockaddr *)&src, sz, 
		  addr, sizeof(addr), port, sizeof(port), 
		  NI_NUMERICHOST | NI_NUMERICSERV);
      printf("discarded %d bytes from [%s]:%s\n", len, addr, port);      
      return len;
    }
    len = SSL_read(ssl, buf, sizeof(buf));
    break;
#endif
  case UNKNOWN:
  default :
    len = recv(sockfd, buf, sizeof(buf), MSG_DONTWAIT);
  }

  if (len > 0) {
    printf("here is the data:\n");
    for (i=0; i<len; i++)
      printf("%c",buf[i]);
  } if (len == 0) {		/* session closed? */
#ifdef WITH_DTLS
    if (check_close(ssl) <= 0) {
      fprintf(stderr, "not closed\n");
    }
#endif
  } else {
#ifdef WITH_DTLS
    err = SSL_get_error(ssl,len);
    switch (err) {
    case SSL_ERROR_WANT_READ:
      fprintf(stderr, "SSL_ERROR_WANT_READ\n");
      return 0;
    case SSL_ERROR_WANT_WRITE:
      fprintf(stderr, "SSL_ERROR_WANT_WRITE\n");
      return 0;
    default:
      fprintf(stderr, "read: SSL error %d: %s\n", err,
	      ERR_error_string(err, NULL));
      return 0;
    }
#else
    perror("recv");
#endif
  }

  return len;
}

int 
_write(SSL_CTX *ctx, int sockfd) {
  int res = 0;
#ifdef WITH_DTLS
  SSL *ssl;
  int err;

  ssl = get_ssl(ctx, sockfd, NULL, 1);
  if (!ssl) {
    fprintf(stderr, "no SSL object for writing");
    return 0;
  }
  res = SSL_write(ssl, NULL, 0);
  if (res < 0) {
    /*
    if (SSL_want_write(ssl))
      return 0;
    */
    /* FIXME: check SSL_want_read(ssl) */

    err = SSL_get_error(ssl,res);
    fprintf(stderr,"SSL_write returned %d (%s)\n", err, ERR_error_string(err, NULL));
  } else {
    printf("SSL_write successful\n");
  }
#else
#endif
  
  return res;
}


int 
generate_cookie(SSL *ssl, unsigned char *cookie, unsigned int *cookie_len) {
  /* FIXME: generate secure client-specific cookie */
#define DUMMYSTR "ABCDEFGHIJKLMNOP"
  *cookie_len = strlen(DUMMYSTR);
  memcpy(cookie, DUMMYSTR, *cookie_len);

  return 1;
}

int 
verify_cookie(SSL *ssl, unsigned char *cookie, unsigned int cookie_len) {
  /* FIXME */
  return 1;
}

enum { READ, WRITE };

int
main(int argc, char **argv) {
  int sockfd = 0;
  int on = 1;
  struct sockaddr_in6 listen_addr = { AF_INET6, htons(20220), 0, IN6ADDR_ANY_INIT, 0 };
  size_t addr_size = sizeof(struct sockaddr_in6);
  fd_set fds[2];
  int result, flags;
  int idx, res = 0;
  struct timeval timeout;
  struct sigaction act, oact;
  
#ifdef WITH_DTLS
  SSL_CTX *ctx;

  memset(ssl_peer_storage, 0, sizeof(ssl_peer_storage));

  SSL_load_error_strings();
  SSL_library_init();
  ctx = SSL_CTX_new(DTLSv1_server_method());

  SSL_CTX_set_cipher_list(ctx, "ALL");
  SSL_CTX_set_session_cache_mode(ctx, SSL_SESS_CACHE_OFF);

  res = SSL_CTX_use_certificate_file(ctx, SERVER_CERT_PEM, SSL_FILETYPE_PEM);
  if (res != 1) {
    fprintf(stderr, "cannot read server certificate from file '%s' (%s)\n", 
	    SERVER_CERT_PEM, ERR_error_string(res,NULL));
    goto end;
  }

  res = SSL_CTX_use_PrivateKey_file(ctx, SERVER_KEY_PEM, SSL_FILETYPE_PEM);
  if (res != 1) {
    fprintf(stderr, "cannot read server key from file '%s' (%s)\n", 
	    SERVER_KEY_PEM, ERR_error_string(res,NULL));
    goto end;
  }

  res = SSL_CTX_check_private_key (ctx);
  if (res != 1) {
    fprintf(stderr, "invalid private key\n");
    goto end;
  }

  res = SSL_CTX_load_verify_locations(ctx, CA_CERT_PEM, NULL);
  if (res != 1) {
    fprintf(stderr, "cannot read ca file '%s'\n", CA_CERT_PEM);
    goto end;
  }

  /* Client has to authenticate */

  /* Client has to authenticate */
  SSL_CTX_set_verify(ctx, SSL_VERIFY_PEER | SSL_VERIFY_CLIENT_ONCE, NULL);

  SSL_CTX_set_read_ahead(ctx, 1); /* disable read-ahead */
  SSL_CTX_set_cookie_generate_cb(ctx, generate_cookie);
  SSL_CTX_set_cookie_verify_cb(ctx, verify_cookie);

  SSL_CTX_use_psk_identity_hint(ctx, "Enter password for CoAP-Gateway");
  SSL_CTX_set_psk_server_callback(ctx, psk_server_callback);

  SSL_CTX_set_info_callback(ctx, info_callback);
#endif

  sockfd = socket(listen_addr.sin6_family, SOCK_DGRAM, 0);
  if ( sockfd < 0 ) {
    perror("socket");
    return -1;
  }

  if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on) ) < 0)
    perror("setsockopt SO_REUSEADDR");

  flags = fcntl(sockfd, F_GETFL, 0);
  if (flags < 0 || fcntl(sockfd, F_SETFL, flags | O_NONBLOCK) < 0) {
    perror("fcntl");
    return -1;
  }

  on = 1;
  if (setsockopt(sockfd, IPPROTO_IPV6, IPV6_RECVPKTINFO, &on, sizeof(on) ) < 0) {
    perror("setsockopt IPV6_PKTINFO");
  }

  if (bind (sockfd, (const struct sockaddr *)&listen_addr, addr_size) < 0) {
    perror("bind");
    res = -2;
    goto end;
  }

  act.sa_handler = handle_sigint;
  sigemptyset(&act.sa_mask);
  act.sa_flags = 0;
  sigaction(SIGINT, &act, &oact);

  while (!quit) {
    FD_ZERO(&fds[READ]);
    FD_ZERO(&fds[WRITE]);
    FD_SET(sockfd, &fds[READ]);

    timeout.tv_sec = 1;
    timeout.tv_usec = 0;
    result = select( FD_SETSIZE, &fds[READ], &fds[WRITE], 0, &timeout);

    if (result < 0) {		/* error */
      if (errno != EINTR)
	perror("select");
    } else if (result > 0) {	/* read from socket */
      if ( FD_ISSET( sockfd, &fds[READ]) ) {
	_read(ctx, sockfd);	/* read received data */
      } else if ( FD_ISSET( sockfd, &fds[WRITE]) ) { /* write to socket */
	_write(ctx, sockfd);		/* write data */
      }
    } else {			/* timeout */
      check_timeout();
    }
    remove_closed();
  }
  
 end:
#ifdef WITH_DTLS
  for (idx = 0; idx < MAX_SSL_PEERS; idx++) {
    if (ssl_peer_storage[idx] && ssl_peer_storage[idx]->ssl) {
      if (ssl_peer_storage[idx]->state == PEER_ST_ESTABLISHED)
	SSL_shutdown(ssl_peer_storage[idx]->ssl);
      SSL_free(ssl_peer_storage[idx]->ssl);
    }
  }

  SSL_CTX_free(ctx);
#endif
  close(sockfd);		/* don't care if we close stdin at this point */
  return res;
}