static int cw_read_wtp_descriptor(mbag_t mbag, struct conn *conn,
				  struct cw_action_in *a, uint8_t * data, int len,
				  int silent)
{


	mbag_set_byte(mbag, CW_ITEM_WTP_MAX_RADIOS, cw_get_byte(data));
	mbag_set_byte(mbag, CW_ITEM_WTP_RADIOS_IN_USE,
			      cw_get_byte(data + 1));


	/* Get number of encryption elements */
	int ncrypt = cw_get_byte(data + 2);
	if (ncrypt == 0) {
		if (conn->strict_capwap) {
			if (!silent)
				cw_dbg(DBG_ELEM_ERR,
				       "Bad WTP Descriptor, number of encryption elements is 0.");
			return 0;
		}
		if (!silent)
			cw_dbg(DBG_RFC,
			       "Non standard conform WTP Descriptor, number of encryptoin elements is 0.");
	}

	int pos = 3;
	int i;
	for (i = 0; i < ncrypt; i++) {
		// It's a dummy for now
		pos += 3;
	}

	return cw_read_wtp_descriptor_versions(mbag, data + pos, len - pos, silent);

}

static int detect(struct conn *conn, const uint8_t * rawmsg, int rawlen, int elems_len,
		  struct sockaddr *from, int mode)
{


	int offset = cw_get_hdr_msg_offset(rawmsg);
	const uint8_t *msg_ptr = rawmsg + offset;

	const uint8_t *elems_ptr = cw_get_msg_elems_ptr(msg_ptr);
	const uint8_t *elem;


	/* To detect a Fortinet AP we look for any vendor
	 * specific payload Fortinet identifier */
	cw_foreach_elem(elem, elems_ptr, elems_len) {
		int id = cw_get_elem_id(elem);
		if (id == CW_ELEM_VENDOR_SPECIFIC_PAYLOAD) {
			uint32_t vendor_id = cw_get_dword(cw_get_elem_data(elem));
			if (vendor_id == CW_VENDOR_ID_FORTINET) {
				//              conn->actions = &actions;
				if (mode == MOD_MODE_CAPWAP) {
					cw_dbg(DBG_MOD, "Fortinet capwap detected: yes");
				} else {
					cw_dbg(DBG_MOD, "Fortinet bindings detected: yes");
				}

				return 1;

			}

		}

	}

/**
 * Reads CAPWAP DTLS data from a connection object.
 * @param conn conn object
 * @param out where to write data to
 * @param maxlen maximum number of bytes to read
 * @return the number of bytes read
 */ 
int dtls_bio_read(struct conn *conn, char *out, int maxlen)
{
	if (conn->dtls_buffer_len == 0) {
		int len = conn->recv_packet(conn, conn->dtls_buffer, 2048);
		if (len < 4)
			return 0;
		conn->dtls_buffer_len = len - 4;
		conn->dtls_buffer_pos = 4;
	}

	if (conn->dtls_buffer_len > maxlen) {
		memcpy(out, conn->dtls_buffer + conn->dtls_buffer_pos, maxlen);
		conn->dtls_buffer_len -= maxlen;
		conn->dtls_buffer_pos += maxlen;
		cw_dbg(DBG_DTLS_BIO, "SSL BIO read: (maxlen = %d), read %d, remain %d", maxlen,
		       maxlen, conn->dtls_buffer_len);
		cw_dbg_dmp(DBG_DTLS_BIO_DMP, (uint8_t *) out, maxlen, "Dump...");

		return maxlen;
	}

	memcpy(out, conn->dtls_buffer + conn->dtls_buffer_pos, conn->dtls_buffer_len);
	int ret = conn->dtls_buffer_len;
	conn->dtls_buffer_len = 0;
	cw_dbg(DBG_DTLS_BIO, "SSL BIO read: (maxlen = %d), read %d, remain %d", maxlen, ret,
	       conn->dtls_buffer_len);
	cw_dbg_dmp(DBG_DTLS_BIO_DMP, (uint8_t *) out, ret, "Dump...");
	return ret;
}

static struct cw_actiondef *load_mods(struct conn *conn, uint8_t * rawmsg, int len,
				      int elems_len, struct sockaddr *from)
{

	struct mod_ac *cmod =
	    detect_mod(conn, rawmsg, len, elems_len, from, MOD_MODE_CAPWAP);
	if (cmod == MOD_NULL) {
		cw_dbg(DBG_MSG_ERR,
		       "Can't find mod to handle connection from %s , discarding message",
		       sock_addr2str_p(from));
		return NULL;
	}

	struct mod_ac *bmod =
	    detect_mod(conn, rawmsg, len, elems_len, from, MOD_MODE_BINDINGS);

	cw_dbg(DBG_INFO, "Mods deteced: %s,%s", cmod->name, bmod->name);

	struct cw_actiondef *ad = mod_cache_add(conn,cmod, bmod);

	return ad;




/*
	if (bindins_mod) {
		cw_dbg(DBG_INFO, "Using mod '%s' to handle CAWPAP for %s", mod->name,
		       sock_addr2str_p(from));
		conn->detected=1;
	}
	else{
//              errno = EAGAIN;
//              return -1;
	}


	mod = detect_mod(conn, rawmsg, len, elems_len, from, MOD_DETECT_BINDINGS);
	if (mod) {
		cw_dbg(DBG_INFO, "Using bindings '%s' to handle %s", mod->name,
		       sock_addr2str_p(from));
		conn->detected=1;
	}
	else{
		cw_dbg(DBG_MSG_ERR, "Can't detect bindings ... for %s",
		       sock_addr2str_p(from));
	}

	
	return 0;
	*/
}

static void wtpman_image_data(struct wtpman *wtpman)
{
	struct conn *conn = wtpman->conn;

	/* Image upload */
	const char *filename = mbag_get_str(conn->outgoing, CW_ITEM_IMAGE_FILENAME, NULL);
	if (!filename) {
		cw_log(LOG_ERR,
		       "Can't send image to %s. No Image Filename Item found.",
		       sock_addr2str(&conn->addr));
		return;
	}
	cw_dbg(DBG_INFO, "Sending image file '%s' to '%s'.", filename,
	       sock_addr2str(&conn->addr));
	FILE *infile = fopen(filename, "rb");
	if (infile == NULL) {
		cw_log(LOG_ERR, "Can't open image %s: %s",
		       sock_addr2str(&conn->addr), strerror(errno));
		return;
	}



	CW_CLOCK_DEFINE(clk);
	cw_clock_start(&clk);

	mbag_item_t *eof = mbag_set_const_ptr(conn->outgoing, CW_ITEM_IMAGE_FILEHANDLE,
					      infile);

	int rc = 0;
	while (conn->capwap_state == CW_STATE_IMAGE_DATA && rc == 0 && eof != NULL) {
		rc = cw_send_request(conn, CW_MSG_IMAGE_DATA_REQUEST);
		eof = mbag_get(conn->outgoing, CW_ITEM_IMAGE_FILEHANDLE);
	}


	if (rc) {
		cw_log(LOG_ERR, "Error sending image to %s: %s",
		       sock_addr2str(&conn->addr), cw_strrc(rc));
	} else {
		cw_dbg(DBG_INFO, "Image '%s' sucessful sent to %s in %0.1f seconds.",
		       filename, sock_addr2str(&conn->addr), cw_clock_stop(&clk));
		conn->capwap_state = CW_STATE_NONE;
	}

	fclose(infile);
	wtpman_remove(wtpman);


}

static int wtpman_join(void *arg, time_t timer)
{
	struct wtpman *wtpman = (struct wtpman *) arg;
	struct conn *conn = wtpman->conn;

	wtpman->conn->outgoing = mbag_create();
	wtpman->conn->incomming = mbag_create();
	conn->config = conn->incomming;
//      wtpman->conn->local = ac_config;

	mbag_set_str(conn->local, CW_ITEM_AC_NAME, conf_acname);



	wtpman->conn->capwap_state = CW_STATE_JOIN;
//	wtpman->conn->actions = &capwap_actions;

//      wtpman->conn->itemstore = mbag_create();


	cw_dbg(DBG_INFO, "Join State - %s", sock_addr2str(&conn->addr));

	int rc;
	while (!cw_timer_timeout(timer) && wtpman->conn->capwap_state == CW_STATE_JOIN) {
		rc = cw_read_messages(wtpman->conn);
		if (rc < 0) {
			break;
		}
	}

	if (rc != 0) {
		cw_log(LOG_ERR, "Error joining WTP %s", cw_strerror(rc));
		return 0;

	}


	if (wtpman->conn->capwap_state == CW_STATE_JOIN) {
		cw_dbg(DBG_MSG_ERR, "No join request from %s after %d seconds, WTP died.",
		       sock_addr2str(&wtpman->conn->addr), wtpman->conn->wait_dtls);

		return 0;
	}


	return 1;


}

int wtpconf_primary_if()
{

	if (!conf_primary_if ) {
	        conf_primary_if  = sock_get_primary_if(AF_INET6);
	        if (!conf_primary_if)
	                conf_primary_if = sock_get_primary_if(AF_INET);
	}


        if (!conf_primary_if){
                cw_log(LOG_ERR,"Fatal: Unable to detect primary interface");
                return 0;
        }               

        if (!sock_getifhwaddr(conf_primary_if,conf_macaddress,&conf_macaddress_len)){


                cw_log(LOG_ERR,"Fatal: Unable to detect link layer address for %s:",conf_primary_if, 
				strerror(errno));
                return 0;
        };
	
	cw_dbg(DBG_INFO, "Primary interface: %s, mac address: %s.",
			conf_primary_if,
			sock_hwaddr2str(conf_macaddress,conf_macaddress_len)
			);
	return 1;
}

int cw_in_check_disc_req(struct conn *conn, struct cw_action_in *a, uint8_t * data,
			 int len,struct sockaddr *from)
{

	cw_action_in_t *mlist[120];
	int n = cw_check_missing_mand(mlist, conn, a);

	conn->capwap_state = CW_STATE_NONE;


	if (n && conn->strict_capwap) {
		cw_dbg_missing_mand(DBG_MSG_ERR, conn, mlist, n, a);
		/* if mandatory elements are missing, in strict 
		   mode send no discovery response */
		cw_dbg(DBG_MSG_ERR,
		       "Ignoring Discovery Request from %s - missing mandatory elements.",
		       sock_addr2str(from));
		return -1;
	}

	if ( n ) {
		/* put a warning here */
		cw_dbg_missing_mand(DBG_RFC, conn, mlist, n, a);
	}

		

	/* ok, send response */
	conn->capwap_state = CW_STATE_JOIN;

	return 0;
}

int cw_in_check_join_req(struct conn *conn, struct cw_action_in *a, uint8_t * data,
			 int len,struct sockaddr *from)
{

	cw_action_in_t * mlist[60];

	/* Check for mandatory elements */
	int n = cw_check_missing_mand(mlist,conn,a);
	if (n) {
		if ( conn->strict_capwap ){
			cw_dbg_missing_mand(DBG_MSG_ERR,conn,mlist,n,a);
			conn->capwap_state=CW_STATE_JOIN;
			return CW_RESULT_MISSING_MAND_ELEM;
		}
		cw_dbg_missing_mand(DBG_RFC,conn,mlist,n,a);
	}

	connlist_lock(conn->connlist);
	struct conn *cc = connlist_get_by_session_id(conn->connlist,conn);
	if (!cc){
		connlist_add_by_session_id(conn->connlist,conn);
	}
	connlist_unlock(conn->connlist);
	if (cc){
		cw_dbg(DBG_ELEM_ERR,"Session already in use %s",format_bin2hex(conn->session_id,16));
		conn->capwap_state=CW_STATE_JOIN;
		return CW_RESULT_JOIN_FAILURE_SESSION_ALREADY_IN_USE;
	}
	
	/* set result code to ok and change to configure state */
	mbag_set_dword(conn->outgoing,CW_ITEM_RESULT_CODE,0);
	conn->capwap_state = CW_STATE_CONFIGURE;

	return 0;
}

int netconn_process_packet(struct netconn *nc, uint8_t * packet, int len,
			struct sockaddr *from)
{
	char sock_buf[SOCK_ADDR_BUFSIZE];

/*//	cw_dbg_pkt_nc(DBG_PKT_IN, nc, packet, len, from);*/
	if (len < 8) {
		/* packet too short */
		cw_dbg(DBG_PKT_ERR,
		       "Discarding packet from %s, packet too short, len=%d,  at least 8 expected.",
		       sock_addr2str(&nc->addr,sock_buf), len);
		errno = EAGAIN;
		return -1;
	}

	int preamble = cw_get_hdr_preamble(packet);

	if ((preamble & 0xf0) != (CAPWAP_VERSION << 4)) {
		/* wrong version */
		cw_dbg(DBG_PKT_ERR,
		       "Discarding packet from %s, wrong version, version=%d, version %d expected.",
		       sock_addr2str(&nc->addr,sock_buf), (preamble & 0xf0) >> 4,
		       CAPWAP_VERSION);
		errno = EAGAIN;
		return -1;
	}

static void wtpman_run_discovery(void *arg)
{
	struct wtpman *wtpman = (struct wtpman *) arg;

	time_t timer = cw_timer_start(10);

	wtpman->conn->capwap_state = CAPWAP_STATE_DISCOVERY;


	while (!cw_timer_timeout(timer)
	       && wtpman->conn->capwap_state == CAPWAP_STATE_DISCOVERY) {
		int rc;
		rc = cw_read_messages(wtpman->conn);
		if (cw_result_is_ok(rc)){
			wtpman->conn->capwap_state=CAPWAP_STATE_JOIN;
			
			cw_dbg(DBG_INFO,"Discovery has detected mods: %s %s", 
				wtpman->conn->cmod->name,wtpman->conn->bmod->name);

			wtplist_lock();
			discovery_cache_add(discovery_cache,(struct sockaddr*)&wtpman->conn->addr,
				wtpman->conn->cmod,wtpman->conn->bmod);
			wtplist_unlock();
			
		}
	}

	return;
}

void dataman_run(struct dataman *dm)
{
	time_t timer = cw_timer_start(2);

	dm->nc->process_packet=netconn_process_packet;
	dm->nc->process_message=dataman_process_message0;
	dm->nc->data = dm;

	while (!cw_timer_timeout(timer)){
		netconn_read_messages(dm->nc);
	}

	if (!dm->wtpman){
		cw_log(LOG_ERR,"Data session not associated");
		dataman_destroy(dm);
		return;
	}

	cw_dbg(DBG_X,"Data channel established");
	dm->nc->process_message=dataman_process_message;

	while (1){
		time_t timer = cw_timer_start(2);
		while (!cw_timer_timeout(timer)){
			netconn_read_messages(dm->nc);
		}
	}

	
	
}

void cw_dbg_msg(int level,struct conn *conn, uint8_t * packet, int len,struct sockaddr *from)
{
	if (!cw_dbg_is_level(level))
		return;

	char buf[1024];
	char *s = buf;



	uint8_t * msgptr = cw_get_hdr_msg_ptr(packet);
//	int pplen = len - (msgptr-packet);

	int msg_id=cw_get_msg_id(msgptr);
	s+=sprintf(s,"%s Message (type=%d) ",cw_strmsg(msg_id),msg_id);
	if ( level == DBG_MSG_IN ) 
		s+=sprintf(s,"from %s ",sock_addr2str(from));
	else
		s+=sprintf(s,"to %s ",sock_addr2str(from));

	s+=sprintf(s,", Seqnum: %d ElemLen: %d",cw_get_msg_seqnum(msgptr),cw_get_msg_elems_len(msgptr));

//abort:
	cw_dbg(level,"%s",buf);
}

int wtpconf_primary_if()
{

#ifdef WITH_IPV6
        conf_primary_if  = sock_get_primary_if(AF_INET6);
        if (!conf_primary_if)
                conf_primary_if = sock_get_primary_if(AF_INET);
#else   
        conf_primary_if = get_primary_if(AF_INET);
#endif

        if (!conf_primary_if){
                cw_log(LOG_ERR,"Fatal: Unable to detect primary interface");
                return 0;
        }               

        if (!sock_getifhwaddr(conf_primary_if,conf_macaddress,&conf_macaddress_len)){
                cw_log(LOG_ERR,"Fatal: Unable to detect link layer address for %s.",conf_primary_if);
                return 0;
        };
	
	cw_dbg(DBG_CW_INFO, "Primary interface: %s, mac address: %s.",
			conf_primary_if,
			sock_hwaddr2str(conf_macaddress,conf_macaddress_len)
			);


	return 1;

}

int cw_in_cisco_image_identifier(struct conn *conn,struct cw_action_in * a,uint8_t *data,int len,struct sockaddr *from)
{

	if (len<a->min_len) {
		cw_dbg(DBG_ELEM_ERR,"Message element too short, %d < %d", len,a->min_len);
		return 0;
	}

	uint32_t vendor_id = cw_get_dword(data);
	int dstart;

	switch (vendor_id) {
		case CW_VENDOR_ID_ZYXEL:
		case CW_VENDOR_ID_CISCO:
		case CW_VENDOR_ID_FSF:
		case 0:
			dstart=4;
			len-=4;
			break;
		default:
			vendor_id=CW_VENDOR_ID_CISCO;
			dstart=0;
	}

//	mbag_set(conn->remote,a->item_id,a->itemtype,data+dstart,len);
	mbag_set_bstrv(conn->incomming,a->item_id,vendor_id,data+dstart,len);
	return 1;
}

int cw_in_radio_generic(struct conn *conn, struct cw_action_in *a, uint8_t * data,
			int len, struct sockaddr *from)
{

	const cw_itemdef_t * idef = cw_itemdef_get(conn->actions->radioitems,a->item_id,CW_ITEM_NONE);
	if (!idef){
		cw_log(LOG_ERR,"No definition found for %s",a->item_id);
		return 0;
	}


	int rid = cw_get_byte(data);
	mbag_t radio = mbag_i_get_mbag(conn->radios, rid, NULL);
	if (!radio) {
		if (a->vendor_id != 0
		    || ( (a->vendor_id == 0) && (a->msg_id != CW_MSG_DISCOVERY_REQUEST
			&& a->msg_id != CW_MSG_JOIN_REQUEST) )) {
			cw_dbg(DBG_ELEM_ERR, "Radio not found %d", rid);
			return 0;
		}
		mbag_i_set_mbag(conn->radios,rid,mbag_create());
		radio = mbag_i_get_mbag(conn->radios, rid, NULL);
	}

	int rc = mbag_set_from_buf(radio,idef->type,a->item_id,data+1,len-1);
	if (!rc){
		cw_log(LOG_ERR,
		       "Can't handle item type %s in definition for incomming msg %d (%s) - %d, cw_in_radio_generic.",
		       idef->type->name, a->msg_id, cw_strmsg(a->msg_id), a->elem_id);
	}

	return rc;


}

void wtpman_start(struct wtpman *wtpman, int dtlsmode)
{
	cw_dbg(DBG_INFO, "Starting wtpman, DTLS mode = %d",dtlsmode);
	wtpman->dtlsmode=dtlsmode;
	pthread_create(&wtpman->thread, NULL, wtpman_main,
		       (void *) wtpman);
	return;
}

static int wtpman_join(void *arg)
{
	int rc;
	char sock_buf[SOCK_ADDR_BUFSIZE];
	struct wtpman *wtpman = (struct wtpman *) arg;
	struct conn *conn = wtpman->conn;
	time_t timer, wait_join;

	cw_dbg(DBG_INFO, "Join State - %s", sock_addr2str(&conn->addr,sock_buf));
	
	wait_join = cw_ktv_get_word(conn->global_cfg,"wait-join",CAPWAP_WAIT_JOIN);

	timer = cw_timer_start(wait_join);


	while (!cw_timer_timeout(timer) && wtpman->conn->capwap_state == CAPWAP_STATE_JOIN) {
		rc = cw_read_messages(wtpman->conn);
		if (rc < 0) {
			if (errno == EAGAIN)
				continue;
				
			break;
		}
		cw_dbg_ktv_dump(conn->remote_cfg,DBG_INFO,
		"-------------dump------------",
		"DMP","---------end dump --------");
	}

	if (rc != 0) {
		cw_log(LOG_ERR, "Error joining WTP %s", cw_strerror(rc));
		return 0;
	}


	if (wtpman->conn->capwap_state != CAPWAP_STATE_JOIN_COMPLETE) {
		cw_dbg(DBG_MSG_ERR, "No join request from %s after %d seconds, WTP died.",
		       sock_addr2str(&wtpman->conn->addr,sock_buf), wait_join);

		return 0;
	}


	return 1;


}

static int wtpman_establish_dtls(void *arg)
{
	struct wtpman *wtpman = (struct wtpman *) arg;

	/* setup cipher */
	wtpman->conn->dtls_cipher = conf_sslcipher;

	/* setup DTSL certificates */
	int dtls_ok = 0;
	if (conf_sslkeyfilename && conf_sslcertfilename) {


		wtpman->conn->dtls_key_file = conf_sslkeyfilename;
		wtpman->conn->dtls_cert_file = conf_sslcertfilename;
		wtpman->conn->dtls_key_pass = conf_sslkeypass;
		wtpman->conn->dtls_verify_peer = conf_dtls_verify_peer;
		cw_dbg(DBG_DTLS, "Using key file %s", wtpman->conn->dtls_key_file);
		cw_dbg(DBG_DTLS, "Using cert file %s", wtpman->conn->dtls_cert_file);
		dtls_ok = 1;
	}

	/* setup DTLS psk */
	if (conf_dtls_psk) {
		wtpman->conn->dtls_psk = conf_dtls_psk;
		wtpman->conn->dtls_psk_len = strlen(conf_dtls_psk);
		dtls_ok = 1;
	}

	if (!dtls_ok) {
		cw_log(LOG_ERR,
		       "Can't establish DTLS session, neither psk nor certs set in config file.");
		return 0;
	}

	/* try to accept the connection */
	if (!dtls_accept(wtpman->conn)) {
		cw_dbg(DBG_DTLS, "Error establishing DTLS session with %s",
		       sock_addr2str_p(&wtpman->conn->addr));
		return 0;
	}

	cw_dbg(DBG_DTLS, "DTLS session established with %s, cipher=%s",
	       sock_addr2str_p(&wtpman->conn->addr), dtls_get_cipher(wtpman->conn));

	return 1;
}

void cw_dbg_pkt_nc(int level,struct netconn *nc, uint8_t * packet, int len,struct sockaddr *from)
{
	if (!cw_dbg_is_level(level))
		return;

	char buf[1024];
	cw_format_pkt_hdr(buf,level,NULL,packet,len,from);

	int hlen = cw_get_hdr_msg_offset(packet);

	if (cw_dbg_is_level(DBG_PKT_DMP)){
		char  *dmp = cw_dbg_mkdmp_c(packet,len,hlen);
		cw_dbg(level,"%s%s",buf,dmp);
		free(dmp);
	}
	else
		cw_dbg(level,"%s",buf);
}