/**
 * Insert segment into the list (segments covered with new one will be deleted)
 *
 * Called from tcp_receive()
 */
static void
tcp_oos_insert_segment(struct tcp_seg *cseg, struct tcp_seg *next)
{
	struct tcp_seg *old_seg;

	if (TCPH_FLAGS(cseg->tcphdr) & TCP_FIN) {
		/* received segment overlaps all following segments */
		tcp_segs_free(next);
		next = NULL;
	} else {
		/* delete some following segments
		   oos queue may have segments with FIN flag */
		while (next &&
			   TCP_SEQ_GEQ((seqno + cseg->len),
						   (next->tcphdr->seqno + next->len))) {
			/* cseg with FIN already processed */
			if (TCPH_FLAGS(next->tcphdr) & TCP_FIN) {
				TCPH_FLAGS_SET(cseg->tcphdr, TCPH_FLAGS(cseg->tcphdr) | TCP_FIN);
			}
			old_seg = next;
			next = next->next;
			tcp_seg_free(old_seg);
		}
		if (next &&
			TCP_SEQ_GT(seqno + cseg->len, next->tcphdr->seqno)) {
			/* We need to trim the incoming segment. */
			cseg->len = (u16_t)(next->tcphdr->seqno - seqno);
			pbuf_realloc(cseg->p, cseg->len);
		}
	}
	cseg->next = next;
}

static void
send_data(void)
{
  u16_t *payload;
  int ret;

  if(tftp_state.last_data != NULL) {
    pbuf_free(tftp_state.last_data);
  }
  
  tftp_state.last_data = pbuf_alloc(PBUF_TRANSPORT, TFTP_HEADER_LENGTH + TFTP_MAX_PAYLOAD_SIZE, PBUF_RAM);
  if(tftp_state.last_data == NULL) {
    return;
  }

  payload = (u16_t *) tftp_state.last_data->payload;
  payload[0] = PP_HTONS(TFTP_DATA);
  payload[1] = lwip_htons(tftp_state.blknum);

  ret = tftp_state.ctx->read(tftp_state.handle, &payload[2], TFTP_MAX_PAYLOAD_SIZE);
  if (ret < 0) {
    send_error(&tftp_state.addr, tftp_state.port, TFTP_ERROR_ACCESS_VIOLATION, "Error occured while reading the file.");
    close_handle();
    return;
  }

  pbuf_realloc(tftp_state.last_data, (u16_t)(TFTP_HEADER_LENGTH + ret));
  resend_data();
}

// on data received
err_t client_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err)
{
    struct client *c = arg;
    if (err!=ERR_OK) {
        pbuf_free(p);
        return client_kill(c);
    }
    // no buffer
    if (!p) {
        c->st = TERMINATED;
        // and no queued data -> close
        if (!c->p)
            return client_kill(c);
        return flushq(c);
    }
    switch (c->st) {
        case ACCEPTED: {
            c->st++;
        }
        case RECEIVED: {
            // this is a hack - we assume each line fits into a single packet
            // this could be done properly using pbuf header magic and keeping line state
            u8_t *dst = p->payload;
            u8_t *res = memchr(dst, '\n', p->len);

            // invalid data or exit requested
            if (!res || !memcmp(dst,"exit",4)) {
                pbuf_free(p);
                return client_kill(c);
            }

            // reverse the line
            if (res[-1] == '\r') res--;
            u32_t len = res-dst;
            for (int i = 0; i < (len)/2;i++) {
                u8_t t = dst[i];
                dst[i] = dst[len-i-1];
                dst[len-i-1] = t;
            }
            dst[len] = '\n';
            pbuf_realloc(p, len+1);


            // and enqueue it
            if (c->p) {
                pbuf_chain(c->p, p);
                return ERR_OK;
            }
            c->p = p;
            return flushq(c);
        }
        // unknown state
        default: {
            tcp_recved(pcb, p->tot_len);
            c->p = NULL; // XXX leaky?
            pbuf_free(p);
        }
    }
    return ERR_OK;
}

static void
send_data(const ip_addr_t *addr, u16_t port)
{
  u16_t *payload;
  int ret;

  if (tftp_state.last_data != NULL) {
    pbuf_free(tftp_state.last_data);
  }

  tftp_state.last_data = init_packet(TFTP_DATA, tftp_state.blknum, TFTP_MAX_PAYLOAD_SIZE);
  if (tftp_state.last_data == NULL) {
    return;
  }

  payload = (u16_t *) tftp_state.last_data->payload;

  ret = tftp_state.ctx->read(tftp_state.handle, &payload[2], TFTP_MAX_PAYLOAD_SIZE);
  if (ret < 0) {
    send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Error occured while reading the file.");
    close_handle();
    return;
  }

  pbuf_realloc(tftp_state.last_data, (u16_t)(TFTP_HEADER_LENGTH + ret));
  resend_data(addr, port);
}

static err_t dhcp_release(struct dhcp_state *state)
{
  err_t result;
  u16_t msecs;
  DEBUGF(DHCP_DEBUG, ("dhcp_release()\n"));
  // and idle DHCP client
  dhcp_set_state(state, DHCP_OFF);

  // create and initialize the DHCP message header
  result = dhcp_create_request(state);
  if (result == ERR_OK)
  {
    dhcp_option(state, DHCP_OPTION_MESSAGE_TYPE, DHCP_OPTION_MESSAGE_TYPE_LEN);
    dhcp_option_byte(state, DHCP_RELEASE);

    dhcp_option_trailer(state);

    pbuf_realloc(state->p_out, sizeof(struct dhcp_msg) - DHCP_OPTIONS_LEN + state->options_out_len);

    udp_bind(state->pcb, IP_ADDR_ANY, DHCP_CLIENT_PORT);
    udp_connect(state->pcb, &state->server_ip_addr, DHCP_SERVER_PORT);
    udp_send(state->pcb, state->p_out);
    dhcp_delete_request(state);
  }
  state->tries++;
  msecs = state->tries < 10 ? state->tries * 1000 : 10 * 1000;
  state->request_timeout = (msecs + DHCP_FINE_TIMER_MSECS - 1) / DHCP_FINE_TIMER_MSECS;
  DEBUGF(DHCP_DEBUG, ("dhcp_release(): request timeout %u msecs\n", msecs));
  // remove IP address from interface
  netif_set_ipaddr(state->netif, IP_ADDR_ANY);
  netif_set_gw(state->netif, IP_ADDR_ANY);
  netif_set_netmask(state->netif, IP_ADDR_ANY);
  return result;
}

// decline a
static err_t dhcp_decline(struct dhcp_state *state)
{
  err_t result = ERR_OK;
  u16_t msecs;
  DEBUGF(DHCP_DEBUG, ("dhcp_decline()\n"));
  dhcp_set_state(state, DHCP_BACKING_OFF);
  // create and initialize the DHCP message header
  result = dhcp_create_request(state);
  if (result == ERR_OK)
  {
    dhcp_option(state, DHCP_OPTION_MESSAGE_TYPE, DHCP_OPTION_MESSAGE_TYPE_LEN);
    dhcp_option_byte(state, DHCP_DECLINE);

    dhcp_option(state, DHCP_OPTION_MAX_MSG_SIZE, DHCP_OPTION_MAX_MSG_SIZE_LEN);
    dhcp_option_short(state, 576);

    dhcp_option_trailer(state);
    // resize pbuf to reflect true size of options
    pbuf_realloc(state->p_out, sizeof(struct dhcp_msg) - DHCP_OPTIONS_LEN + state->options_out_len);

    udp_bind(state->pcb, IP_ADDR_ANY, DHCP_CLIENT_PORT);
    udp_connect(state->pcb, &state->server_ip_addr, DHCP_SERVER_PORT);
    udp_send(state->pcb, state->p_out);
    dhcp_delete_request(state);
  }
  state->tries++;
  msecs = 10*1000;
  state->request_timeout = (msecs + DHCP_FINE_TIMER_MSECS - 1) / DHCP_FINE_TIMER_MSECS;
  DEBUGF(DHCP_DEBUG, ("dhcp_decline(): request timeout %u msecs\n", msecs));
  return result;
}

/* Transmit a packet on the General Port. */
size_t netSendGeneral(Octet *buf, UInteger16 length, NetPath *netPath)
{
    int i, j;
    struct pbuf *pcopy;

    /* Reallocate the tx pbuf based on the current size. */
    pbuf_realloc(netPath->generalTxBuf, length);
    pcopy = netPath->generalTxBuf;

    /* Copy the incoming data into the pbuf payload. */
    j = 0;
    for(i = 0; i < length; i++) {
        ((u8_t *)pcopy->payload)[j++] = buf[i];
        if(j == pcopy->len) {
            pcopy = pcopy->next;
            j = 0;
        }
    }

    /* send the buffer. */
    udp_sendto(netPath->eventPcb, netPath->generalTxBuf,
               (void *)&netPath->multicastAddr, PTP_GENERAL_PORT);

    return(length);
}

/* Pass received pbufs into 6LowPAN netif */
static void
zepif_udp_recv(void *arg, struct udp_pcb *pcb, struct pbuf *p,
               const ip_addr_t *addr, u16_t port)
{
  err_t err;
  struct netif *netif_lowpan6 = (struct netif *)arg;
  struct zep_hdr *zep;

  LWIP_ASSERT("arg != NULL", arg != NULL);
  LWIP_ASSERT("pcb != NULL", pcb != NULL);
  LWIP_UNUSED_ARG(pcb); /* for LWIP_NOASSERT */
  LWIP_UNUSED_ARG(addr);
  LWIP_UNUSED_ARG(port);
  if (p == NULL) {
    return;
  }

  /* Parse and hide the ZEP header */
  if (p->len < sizeof(struct zep_hdr)) {
    /* need the zep_hdr in one piece */
    goto err_return;
  }
  zep = (struct zep_hdr *)p->payload;
  if (zep->prot_id[0] != 'E') {
    goto err_return;
  }
  if (zep->prot_id[1] != 'X') {
    goto err_return;
  }
  if (zep->prot_version != 2) {
    /* we only support this version for now */
    goto err_return;
  }
  if (zep->type != 1) {
    goto err_return;
  }
  if (zep->crc_mode != 1) {
    goto err_return;
  }
  if (zep->len != p->tot_len - sizeof(struct zep_hdr)) {
    goto err_return;
  }
  /* everything seems to be OK, hide the ZEP header */
  if (pbuf_remove_header(p, sizeof(struct zep_hdr))) {
    goto err_return;
  }
  /* TODO Check CRC? */
  /* remove CRC trailer */
  pbuf_realloc(p, p->tot_len - 2);

  /* Call into 6LoWPAN code. */
  err = netif_lowpan6->input(p, netif_lowpan6);
  if (err == ERR_OK) {
    return;
  }
err_return:
  pbuf_free(p);
}

static VOID _DHCPOfferGenAndSend(PDHCP_CLIENT pClient, INT8U * pClientMacAddr, INT32U Xid)
{
	INT32U Len;
	INT8U * Body;
	PDHCP_MSG pDhcpMsg;
	struct pbuf * pDhcpBuf;

	pDhcpBuf = pbuf_alloc(PBUF_TRANSPORT, sizeof(DHCP_MSG), PBUF_RAM);
	if(pDhcpBuf == NULL)
	{
		return;
	}
	
	pDhcpMsg = &DhcpMsg;
	memset(pDhcpMsg, 0, sizeof(*pDhcpMsg));

	/* Initialize the DHCP message header. */
	pDhcpMsg->Op = DHCP_OP_REPLY;
	pDhcpMsg->HType = DHCP_HWTYPE_ETHERNET;
	pDhcpMsg->HLen = 6;
	pDhcpMsg->Xid = htonl(Xid);
	pDhcpMsg->Yiaddr = pClient->IpAddr.addr;
	pDhcpMsg->Siaddr = 0;
	NST_MOVE_MEM(pDhcpMsg->Chaddr, pClientMacAddr, 6);
	pDhcpMsg->Magic = htonl(DHCP_MAGIC);

	Len = 240;
	Body = &pDhcpMsg->Options[0];

	/* Set the message type. */
	DHCP_SET_OPTION_MSG_TYPE(Body, DHCP_MSG_OFFER, Len);

	/* Set the lease time. */
	DHCP_SET_OPTION_LEASE_TIME(Body, DHCP_DEFAULT_LEASE_TIME, Len);

	/* Set the server's ip address */
	DHCP_SET_OPTION_SERVER_ID(Body, DhcpServer.ServerIpAddr.addr, Len);

	/* Set the subnet mask. */
	DHCP_SET_OPTION_SUBNET_MASK(Body, DhcpServer.SubnetMask.addr, Len);

	/* Set the default gatway's ip address. */
	DHCP_SET_OPTION_GW(Body, DhcpServer.GateWay.addr, Len);

	/* Set the dns server's ip address. */
	DHCP_SET_OPTION_DNS(Body, DhcpServer.Dns1.addr, Len);

	DHCP_SET_OPTION_END(Body, Len);

	pbuf_take(pDhcpBuf, (const VOID *)pDhcpMsg, Len);
	pbuf_realloc(pDhcpBuf, Len);

	/* Send broadcast to the DHCP client. */
	udp_sendto(DhcpServer.Socket, pDhcpBuf, IP_ADDR_BROADCAST, DHCP_CLIENT_UDP_PORT);
	pbuf_free(pDhcpBuf);
}

enum rpc_stat
rpc_send(struct pbuf *pbuf, int len, struct udp_pcb *pcb, 
     void (*func)(void *, uintptr_t, struct pbuf *), 
     void *callback, uintptr_t token)
{
    assert(pcb);
    pbuf_realloc(pbuf, len);
    /* Add to a queue */
    add_to_queue(pbuf, pcb, func, callback, token);
    return my_udp_send(pcb, pbuf);
}

/**
 * Should allocate a pbuf and transfer the bytes of the incoming
 * packet from the interface into the pbuf.
 *
 * @param netif the lwip network interface structure for this ethernetif
 * @return a pbuf filled with the received packet (including MAC header)
 *         NULL on memory error
 */
static struct pbuf *
low_level_input(struct netif *netif)
{
        struct pbuf *p;
	struct wlif_t *priv = (struct wlif_t*) netif->state;

        char *stripped_pkt;
        size_t stripped_pkt_len;
        u16_t vlan;
        u8_t rx_hdr_size;
        int status;
        u16_t len;

        /* maximum packet length from wl_rx() */
        len = WL_MAX_PKT_LEN;

        /* We allocate a continous pbuf */
        p = pbuf_alloc(PBUF_RAW, len, PBUF_RAM);
        if (p == NULL) {
                LWIP_DEBUGF(NETIF_DEBUG, ("low_level_input: fail to alloc "
                                          "pbuf of len:%"S32_F"\n", len));
                return NULL;
        }

        /* Read the entire msg */
	priv->rx_pending = 0;
        wl_rx(p->payload, &len);
        if (len == 0) {
                LWIP_DEBUGF(NETIF_DEBUG, ("low_level_input: len was 0"));
                return NULL;
        }

        status = wl_process_rx(
                p->payload,             /* input buf */
                len,                    /* input buf length */
                &stripped_pkt,          
                &stripped_pkt_len,      
                &vlan);

        if (status == WL_ABSORBED) {
                LWIP_DEBUGF(NETIF_DEBUG, ("low_level_input: absorbed"));
                pbuf_free(p);
                return NULL;
        }
		
        /* Data packet, remove padding */
        rx_hdr_size = stripped_pkt - (char*) p->payload;
        pbuf_realloc(p, stripped_pkt_len + rx_hdr_size);
        
        LINK_STATS_INC(link.recv);
        return p;  
}

void dhcp_inform(struct netif *netif)
{
  struct dhcp_state *state = NULL;
  err_t result = ERR_OK;
  state = mem_malloc(sizeof(struct dhcp_state));
  if (state == NULL)
  {
    DEBUGF(DHCP_DEBUG, ("dhcp_inform(): could not allocate dhcp_state\n"));
    return;
  }  
  memset(state, 0, sizeof(struct dhcp_state));

  DEBUGF(DHCP_DEBUG, ("dhcp_inform(): allocated dhcp_state\n"));
  state->pcb = udp_new();
  if (state->pcb == NULL) {
    DEBUGF(DHCP_DEBUG, ("dhcp_inform(): could not obtain pcb\n"));
    mem_free((void *)state);
    return;
  }
  DEBUGF(DHCP_DEBUG, ("dhcp_inform(): created new udp pcb\n"));
  state->netif = netif;
  // we are last in list 
  state->next = NULL;
  // create and initialize the DHCP message header
  result = dhcp_create_request(state);
  if (result == ERR_OK)
  {

    dhcp_option(state, DHCP_OPTION_MESSAGE_TYPE, DHCP_OPTION_MESSAGE_TYPE_LEN);
    dhcp_option_byte(state, DHCP_INFORM);

    dhcp_option(state, DHCP_OPTION_MAX_MSG_SIZE, DHCP_OPTION_MAX_MSG_SIZE_LEN);
    dhcp_option_short(state, 576);

    dhcp_option_trailer(state);

    pbuf_realloc(state->p_out, sizeof(struct dhcp_msg) - DHCP_OPTIONS_LEN + state->options_out_len);

    udp_bind(state->pcb, IP_ADDR_ANY, DHCP_CLIENT_PORT);
    udp_connect(state->pcb, IP_ADDR_BROADCAST, DHCP_SERVER_PORT);
    udp_send(state->pcb, state->p_out);
    udp_connect(state->pcb, IP_ADDR_ANY, DHCP_SERVER_PORT);
    dhcp_delete_request(state);
  }

  if (state != NULL)
  {
    if (state->pcb != NULL) udp_remove(state->pcb);
    state->pcb = NULL;
    mem_free((void *)state);
  }
}

/**
 * Start the DHCP process, discover a server
 *
 */
static err_t dhcp_discover(struct dhcp_state *state)
{
  err_t result = ERR_OK;
  u16_t msecs;
  DEBUGF(DHCP_DEBUG, ("dhcp_discover(%p)\n", state));
  ip_addr_set(&state->offered_ip_addr, IP_ADDR_ANY);
  // create and initialize the DHCP message header
  DEBUGF(DHCP_DEBUG, ("set ip addr, creating request()\n"));

  result = dhcp_create_request(state);
  DEBUGF(DHCP_DEBUG, ("created request\n"));
  
  if (result == ERR_OK)
  {
    dhcp_option(state, DHCP_OPTION_MESSAGE_TYPE, DHCP_OPTION_MESSAGE_TYPE_LEN);
    dhcp_option_byte(state, DHCP_DISCOVER);

    dhcp_option(state, DHCP_OPTION_MAX_MSG_SIZE, DHCP_OPTION_MAX_MSG_SIZE_LEN);
    dhcp_option_short(state, 576);

    dhcp_option(state, DHCP_OPTION_PARAMETER_REQUEST_LIST, 3);
    dhcp_option_byte(state, DHCP_OPTION_SUBNET_MASK);
    dhcp_option_byte(state, DHCP_OPTION_ROUTER);
    dhcp_option_byte(state, DHCP_OPTION_BROADCAST);

    dhcp_option_trailer(state);

    pbuf_realloc(state->p_out, sizeof(struct dhcp_msg) - DHCP_OPTIONS_LEN + state->options_out_len);
    DEBUGF(DHCP_DEBUG, ("about to do udp recv\n"));
    udp_recv(state->pcb, dhcp_recv, state);
    DEBUGF(DHCP_DEBUG, ("about to do udp bind\n"));
    udp_bind(state->pcb, IP_ADDR_ANY, DHCP_CLIENT_PORT);
    DEBUGF(DHCP_DEBUG, ("about to do udp connect\n"));
    udp_connect(state->pcb, IP_ADDR_BROADCAST, DHCP_SERVER_PORT);

    DEBUGF(DHCP_DEBUG, ("about to do udp send\n"));
    udp_send(state->pcb, state->p_out);
    udp_bind(state->pcb, IP_ADDR_ANY, DHCP_CLIENT_PORT);
    udp_connect(state->pcb, IP_ADDR_ANY, DHCP_SERVER_PORT);
    dhcp_delete_request(state);
  }
  state->tries++;
  msecs = state->tries < 4 ? (state->tries + 1) * 1000 : 10 * 1000;
  state->request_timeout = (msecs + DHCP_FINE_TIMER_MSECS - 1) / DHCP_FINE_TIMER_MSECS;
  DEBUGF(DHCP_DEBUG, ("dhcp_discover(): request timeout %u msecs\n", msecs));
  dhcp_set_state(state, DHCP_SELECTING);
  return result;
}

/*-----------------------------------------------------------------------------------*/
static struct pbuf *
sioslipif_input(void)
{
  u8_t c;
  struct pbuf *p, *q;
  int recved;
  int i;

  p = pbuf_alloc(PBUF_LINK, PBUF_MAX_SIZE, PBUF_POOL);
  q = p;
  recved = i = 0;
  
  while (1) {
    c = sio_recv();
    switch (c) {
    case SLIP_END:
      if (recved > 0) {
        /* Received whole packet. */
        pbuf_realloc(p, recved);
        return p;
      }
      break;
    case SLIP_ESC:
      c = sio_recv();
      switch (c) {
      case SLIP_ESC_END:
        c = SLIP_END;
        break;
      case SLIP_ESC_ESC:
        c = SLIP_ESC;
        break;
      }
      /* FALLTHROUGH */
    default:
      if (recved < p->tot_len && q != NULL) {
        ((u8_t *)q->payload)[i] = c;
        recved++;
        i++;
        if (i >= q->len) {
          i = 0;
          q = q->next;
        }
      }
      break;
    }
    
  }
}

/**
 * Select a DHCP server offer out of all offers.
 *
 * Simply select the first offer received, ignore others
 */
static err_t dhcp_select(struct dhcp_state *state)
{
  err_t result;
  u32_t msecs;
  DEBUGF(DHCP_DEBUG, ("dhcp_select()\n"));

  // create and initialize the DHCP message header
  result = dhcp_create_request(state);
  if (result == ERR_OK)
  {
    dhcp_option(state, DHCP_OPTION_MESSAGE_TYPE, DHCP_OPTION_MESSAGE_TYPE_LEN);
    dhcp_option_byte(state, DHCP_REQUEST);

    dhcp_option(state, DHCP_OPTION_MAX_MSG_SIZE, DHCP_OPTION_MAX_MSG_SIZE_LEN);
    dhcp_option_short(state, 576);

    /* MUST request the offered IP address */
    dhcp_option(state, DHCP_OPTION_REQUESTED_IP, 4);
    dhcp_option_long(state, ntohl(state->offered_ip_addr.addr));

    dhcp_option(state, DHCP_OPTION_SERVER_ID, 4);
    dhcp_option_long(state, ntohl(state->server_ip_addr.addr));

    dhcp_option(state, DHCP_OPTION_PARAMETER_REQUEST_LIST, 3);
    dhcp_option_byte(state, DHCP_OPTION_SUBNET_MASK);
    dhcp_option_byte(state, DHCP_OPTION_ROUTER);
    dhcp_option_byte(state, DHCP_OPTION_BROADCAST);

    dhcp_option_trailer(state);

    pbuf_realloc(state->p_out, sizeof(struct dhcp_msg) - DHCP_OPTIONS_LEN + state->options_out_len);

    udp_bind(state->pcb, IP_ADDR_ANY, DHCP_CLIENT_PORT);
    udp_connect(state->pcb, IP_ADDR_BROADCAST, DHCP_SERVER_PORT);
    udp_send(state->pcb, state->p_out);
    // reconnect to any (or to server here?!)
    udp_connect(state->pcb, IP_ADDR_ANY, DHCP_SERVER_PORT);
    dhcp_delete_request(state);
  }
  state->tries++;
  msecs = state->tries < 4 ? state->tries * 1000 : 4 * 1000;
  state->request_timeout = (msecs + DHCP_FINE_TIMER_MSECS - 1) / DHCP_FINE_TIMER_MSECS;
  DEBUGF(DHCP_DEBUG, ("dhcp_select(): request timeout %u msecs\n", msecs));
  dhcp_set_state(state, DHCP_REQUESTING);
  return result;
}

	void
l2cap_input(struct pbuf *p, struct bd_addr *bdaddr)
{
	struct l2cap_seg *inseg;
	struct hci_acl_hdr *aclhdr;
	struct pbuf *data;
	err_t ret;

	pbuf_header(p, HCI_ACL_HDR_LEN);
	aclhdr = p->payload;
	pbuf_header(p, -HCI_ACL_HDR_LEN);

	pbuf_realloc(p, aclhdr->len);

	for(inseg = l2cap_insegs; inseg != NULL; inseg = inseg->next) {
		if(bd_addr_cmp(bdaddr, &(inseg->bdaddr))) {
			break;
		}
	}

	/* Reassembly procedures */
	/* Check if continuing fragment or start of L2CAP packet */
	if(((aclhdr->conhdl_pb_bc >> 12) & 0x03)== L2CAP_ACL_CONT) { /* Continuing fragment */
		if(inseg == NULL)  {
			/* Discard packet */
			LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Continuing fragment. Discard packet\n"));
			pbuf_free(p);
			return;
		} else if(inseg->p->tot_len + p->tot_len > inseg->len) { /* Check if length of
																	segment exceeds
																	l2cap header length */
			/* Discard packet */
			LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_input: Continuing fragment. Length exceeds L2CAP hdr length. Discard packet\n"));
			pbuf_free(inseg->p);
			L2CAP_SEG_RMV(&(l2cap_insegs), inseg);
			lwbt_memp_free(MEMP_L2CAP_SEG, inseg);

			pbuf_free(p);
			return;
		}
		/* Add pbuf to segement */
		pbuf_chain(inseg->p, p);
		pbuf_free(p);

	} else if(((aclhdr->conhdl_pb_bc >> 12) & 0x03) == L2CAP_ACL_START) { /* Start of L2CAP packet */

err_t dhcp_renew(struct dhcp_state *state)
{
  err_t result;
  u16_t msecs;
  DEBUGF(DHCP_DEBUG, ("dhcp_renew()\n"));
  dhcp_set_state(state, DHCP_RENEWING);

  // create and initialize the DHCP message header
  result = dhcp_create_request(state);
  if (result == ERR_OK)
  {

    dhcp_option(state, DHCP_OPTION_MESSAGE_TYPE, DHCP_OPTION_MESSAGE_TYPE_LEN);
    dhcp_option_byte(state, DHCP_REQUEST);

    dhcp_option(state, DHCP_OPTION_MAX_MSG_SIZE, DHCP_OPTION_MAX_MSG_SIZE_LEN);
    dhcp_option_short(state, 576);

#if 0
    dhcp_option(state, DHCP_OPTION_REQUESTED_IP, 4);
    dhcp_option_long(state, ntohl(state->offered_ip_addr.addr));
#endif

#if 0
    dhcp_option(state, DHCP_OPTION_SERVER_ID, 4);
    dhcp_option_long(state, ntohl(state->server_ip_addr.addr));
#endif

    dhcp_option_trailer(state);

    pbuf_realloc(state->p_out, sizeof(struct dhcp_msg) - DHCP_OPTIONS_LEN + state->options_out_len);

    udp_bind(state->pcb, IP_ADDR_ANY, DHCP_CLIENT_PORT);
    udp_connect(state->pcb, &state->server_ip_addr, DHCP_SERVER_PORT);
    udp_send(state->pcb, state->p_out);
    dhcp_delete_request(state);
  }
  state->tries++;
  // back-off on retries, but to a maximum of 20 seconds
  msecs = state->tries < 10 ? state->tries * 2000 : 20 * 1000;
  state->request_timeout = (msecs + DHCP_FINE_TIMER_MSECS - 1) / DHCP_FINE_TIMER_MSECS;
  DEBUGF(DHCP_DEBUG, ("dhcp_renew(): request timeout %u msecs\n", msecs));
  return result;
}

int EthernetUDP::endPacket()
{
	ip_addr_t dest;
	dest.addr = _sendToIP;

	/* Shrink the pbuf to the actual size that was written to it */
	pbuf_realloc(_sendTop, _write);

	/* Send the buffer to the remote host */
	err_t err = udp_sendto(_pcb, _sendTop, &dest, _sendToPort);

	/* udp_sendto is blocking and the pbuf is
	 * no longer needed so free it */
	pbuf_free(_sendTop);

	if(err != ERR_OK)
		return false;

	return true;
}

static void send_data(struct udp_pcb *upcb, ip_addr_t *addr, u16_t port,
                      struct tftp_state *ts)
{
    ts->last_data = pbuf_alloc(PBUF_RAM, 4 + 512, PBUF_RAM);
    u16_t *payload = (u16_t *) ts->last_data->payload;
    payload[0] = htons(DATA);
    payload[1] = htons(ts->blknum);

    int ret = ts->ctx->read(ts->handle, &payload[2], 512);
    if (ret < 0) {
        send_error(upcb, addr, port, ERROR_ACCESS_VIOLATION,
                   "Error occured while reading the file.");
        pbuf_free(ts->last_data);
        ts->last_data = NULL;
        close_handle(ts);
        return;
    }

    pbuf_realloc(ts->last_data, 4 + ret);
    resend_data(upcb, addr, port, ts);
}

static VOID _DHCPNakGenAndSend(INT8U * pClientMacAddr, INT32U Xid)
{
	INT32U Len;
	INT8U * Body;
	PDHCP_MSG pDhcpMsg;
	struct pbuf * pDhcpBuf;

	pDhcpBuf = pbuf_alloc(PBUF_TRANSPORT, sizeof(DHCP_MSG), PBUF_RAM);
	if(pDhcpBuf == NULL)
	{
		return;
	}
	
	pDhcpMsg = &DhcpMsg;
	memset(pDhcpMsg, 0, sizeof(*pDhcpMsg));

	/* Initialize the DHCP message header. */
	pDhcpMsg->Op = DHCP_OP_REPLY;
	pDhcpMsg->HType = DHCP_HWTYPE_ETHERNET;
	pDhcpMsg->HLen = 6;
	pDhcpMsg->Xid = htonl(Xid);
	pDhcpMsg->Siaddr = DhcpServer.ServerIpAddr.addr;
	NST_MOVE_MEM(pDhcpMsg->Chaddr, pClientMacAddr, 6);
	pDhcpMsg->Magic = htonl(DHCP_MAGIC);

	Len = 240;
	Body = &pDhcpMsg->Options[0];

	/* Set the message type. */
	DHCP_SET_OPTION_MSG_TYPE(Body, DHCP_MSG_NAK, Len);

	DHCP_SET_OPTION_END(Body, Len);

	pbuf_take(pDhcpBuf, (const VOID *)pDhcpMsg, Len);
	pbuf_realloc(pDhcpBuf, Len);

	/* Send broadcast to the DHCP client. */
	udp_sendto(DhcpServer.Socket, pDhcpBuf, IP_ADDR_BROADCAST, DHCP_CLIENT_UDP_PORT);
	pbuf_free(pDhcpBuf);
}