// 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;
}
// This works for both UDP and TCP.
static err_t recv_data(int s, struct pbuf * p) {
sockfd_t * fd;
// Get our socket.
if (sock_verify(s) < 0)
return ERR_CONN;
fd = fds + s;
// Get access.
mutex_lock(fd->mutex);
// Do we already have some data chained up in pbufs?
if (fd->recv_buf) {
pbuf_chain(fd->recv_buf, p);
pbuf_free(p);
} else
fd->recv_buf = p;
// Set a new receive count
fd->recv = fd->recv_buf->tot_len;
// Here would be the place we'd want to implement TCP_NODELAY (or the lack
// thereof) but for now we'll just send it all through...
cond_signal(fd->recv_avail);
// Note that we DON'T ACK the data until after a client has received
// it using recv(). Otherwise the peer could send and send and send...
mutex_unlock(fd->mutex);
genwait_wake_all(&select_wait);
return ERR_OK;
}
void
netbuf_chain(struct netbuf *head, struct netbuf *tail)
{
pbuf_chain(head->p, tail->p);
head->ptr = head->p;
memp_free(MEMP_NETBUF, tail);
}
/**
* Send the raw IP packet to the given address. Note that actually you cannot
* modify the IP headers (this is inconsitent with the receive callback where
* you actually get the IP headers), you can only specifiy the ip payload here.
* It requires some more changes in LWIP. (there will be a raw_send() function
* then)
*
* @param pcb the raw pcb which to send
* @param p the ip payload to send
* @param ipaddr the destination address of the whole IP packet
*
*/
err_t
raw_send_to(struct raw_pcb *pcb, struct pbuf *p, struct ip_addr *ipaddr)
{
err_t err;
struct netif *netif;
struct ip_addr *src_ip;
struct pbuf *q; /* q will be sent down the stack */
LWIP_DEBUGF(RAW_DEBUG | DBG_TRACE | 3, ("raw_send_to\n"));
/* not enough space to add an IP header to first pbuf in given p chain? */
if (pbuf_header(p, IP_HLEN)) {
/* allocate header in new pbuf */
q = pbuf_alloc(PBUF_IP, 0, PBUF_RAM);
/* new header pbuf could not be allocated? */
if (q == NULL) {
LWIP_DEBUGF(RAW_DEBUG | DBG_TRACE | 2, ("raw_send_to: could not allocate header\n"));
return ERR_MEM;
}
/* chain header q in front of given pbuf p */
pbuf_chain(q, p);
/* { first pbuf q points to header pbuf } */
LWIP_DEBUGF(RAW_DEBUG, ("raw_send_to: added header pbuf %p before given pbuf %p\n", (void *)q, (void *)p));
} else {
/* first pbuf q equals given pbuf */
q = p;
pbuf_header(q, -IP_HLEN);
}
if ((netif = ip_route(ipaddr)) == NULL) {
LWIP_DEBUGF(RAW_DEBUG | 1, ("raw_send_to: No route to 0x%lx\n", ipaddr->addr));
#if RAW_STATS
/* ++lwip_stats.raw.rterr;*/
#endif /* RAW_STATS */
if (q != p) {
pbuf_free(q);
}
return ERR_RTE;
}
if (ip_addr_isany(&pcb->local_ip)) {
/* use outgoing network interface IP address as source address */
src_ip = &(netif->ip_addr);
} else {
/* use RAW PCB local IP address as source address */
src_ip = &(pcb->local_ip);
}
err = ip_output_if (q, src_ip, ipaddr, pcb->ttl, pcb->tos, pcb->protocol, netif);
/* did we chain a header earlier? */
if (q != p) {
/* free the header */
pbuf_free(q);
}
return err;
}
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 */
void _tcp_flush(void *v) {
TCP *es=v;
if(es->pcb->snd_queuelen > TCP_SND_QUEUELEN-1) // !!!! pred vpisom preveri, ce ni queue ze poln
tcp_output(es->pcb); // kratkih blokov (Nagle algoritem bo javil MEM error....)
else if(es->io) { // sicer nadaljevanje...
char c[256];
int k,n=0;
if(es->rx) { // a je kaj za sprejem ???
struct pbuf *q;
for(q=es->rx; q != NULL; q=es->rx) { // preskanirat je treba celo verigo pbuf
n+=k=_buffer_push(es->io[0],q->payload, q->len); // push v io
if(k < q->len) {
pbuf_header(q,-k); // skrajsaj header
break;
}
es->rx = es->rx->next;
if (es->rx != NULL)
pbuf_ref(es->rx);
pbuf_free(q);
}
tcp_recved(es->pcb,n); // free raw input
}
n=_buffer_count(es->io[1]); // koliko je v buferju za izpis ...
if(n > tcp_sndbuf(es->pcb)) // ne sme biti vec kot je placa na raw output ....
n = tcp_sndbuf(es->pcb);
if(n > 256) // ne sme bit vec kot 1024.... glej c[1024]
n=256;
if(n) { // ce je sploh kej ...
struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, n , PBUF_POOL);
if(p != NULL) { // ce je alokacija pbuf uspela
n=_buffer_pull(es->io[1],c,n); // kopiraj pull vsebino v vmesni buffer
pbuf_take(p,c,n); // formiraj pbuf
if(es->tx) // verizi, ce je se kaj od prej..
pbuf_chain(es->tx,p); //
else
es->tx = p; // sicer nastavi nov pointer
tcp_sent(es->pcb, TCP_sent); // set callback & send..
TCP_send(es->pcb, es);
tcp_output(es->pcb);
}
}
}
}
/**
* @brief This function is the implementation for tcp_recv LwIP callback
* @param arg: pointer on a argument for the tcp_pcb connection
* @param tpcb: pointer on the tcp_pcb connection
* @param pbuf: pointer on the received pbuf
* @param err: error information regarding the reveived pbuf
* @retval err_t: error code
*/
static err_t
TCP_recv(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err) {
err_t ret_err;
TCP *es;
LWIP_ASSERT("arg != NULL",arg != NULL);
es = (TCP *)arg;
if (p == NULL) { /* if we receive an empty tcp frame from client => close connection */
es->state = ES_CLOSING; /* remote host closed connection */
if (es->tx == NULL) {
TCP_close(tpcb, es); /* we're done sending, close connection */
} else { /* we're not done yet */
tcp_sent(tpcb, TCP_sent); /* send remaining data*/
TCP_send(tpcb, es); /* acknowledge received packet */
}
ret_err = ERR_OK;
} else if(err != ERR_OK) { /* else : a non empty frame was received from client but for some reason err != ERR_OK */
if (p != NULL) {
es->tx = NULL;
pbuf_free(p); /* free received pbuf*/
}
ret_err = err;
} else if(es->state == ES_ACCEPTED) { /* first data chunk in p->payload */
es->state = ES_RECEIVED;
es->rx = p; /* store reference to incoming pbuf (chain) */
ret_err = ERR_OK;
} else if (es->state == ES_RECEIVED) {
if (es->rx)
pbuf_chain(es->rx,p);
else
es->rx = p;
ret_err = ERR_OK;
} else { /* data received when connection already closed */
tcp_recved(tpcb, p->tot_len); /* Acknowledge data reception */
es->tx = NULL;
pbuf_free(p); /* free pbuf and do nothing */
ret_err = ERR_OK;
}
return ret_err;
}
static err_t mg_lwip_tcp_recv_cb(void *arg, struct tcp_pcb *tpcb,
struct pbuf *p, err_t err) {
struct mg_connection *nc = (struct mg_connection *) arg;
DBG(("%p %p %u %d", nc, tpcb, (p != NULL ? p->tot_len : 0), err));
if (p == NULL) {
if (nc != NULL) {
system_os_post(MG_TASK_PRIORITY, MG_SIG_CLOSE_CONN, (uint32_t) nc);
} else {
/* Tombstoned connection, do nothing. */
}
return ERR_OK;
} else if (nc == NULL) {
tcp_abort(tpcb);
return ERR_ARG;
}
struct mg_lwip_conn_state *cs = (struct mg_lwip_conn_state *) nc->sock;
/*
* If we get a chain of more than one segment at once, we need to bump
* refcount on the subsequent bufs to make them independent.
*/
if (p->next != NULL) {
struct pbuf *q = p->next;
for (; q != NULL; q = q->next) pbuf_ref(q);
}
if (cs->rx_chain == NULL) {
cs->rx_chain = p;
cs->rx_offset = 0;
} else {
pbuf_chain(cs->rx_chain, p);
}
#ifdef SSL_KRYPTON
if (nc->ssl != NULL) {
if (nc->flags & MG_F_SSL_HANDSHAKE_DONE) {
mg_lwip_ssl_recv(nc);
} else {
mg_lwip_ssl_do_hs(nc);
}
return ERR_OK;
}
#endif
while (cs->rx_chain != NULL) {
struct pbuf *seg = cs->rx_chain;
size_t len = (seg->len - cs->rx_offset);
char *data = (char *) malloc(len);
if (data == NULL) {
DBG(("OOM"));
return ERR_MEM;
}
pbuf_copy_partial(seg, data, len, cs->rx_offset);
mg_if_recv_tcp_cb(nc, data, len); /* callee takes over data */
cs->rx_offset += len;
if (cs->rx_offset == cs->rx_chain->len) {
cs->rx_chain = pbuf_dechain(cs->rx_chain);
pbuf_free(seg);
cs->rx_offset = 0;
}
}
if (nc->send_mbuf.len > 0) {
mg_lwip_mgr_schedule_poll(nc->mgr);
}
return ERR_OK;
}
err_t
echo_recv(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err)
{
struct echo_state *es;
err_t ret_err;
LWIP_ASSERT("arg != NULL",arg != NULL);
es = arg;
if (p == NULL)
{
/* remote host closed connection */
es->state = ES_CLOSING;
if(es->p == NULL)
{
/* we're done sending, close it */
echo_close(tpcb, es);
}
else
{
/* we're not done yet */
tcp_sent(tpcb, echo_sent);
echo_send(tpcb, es);
}
ret_err = ERR_OK;
}
else if(err != ERR_OK)
{
/* cleanup, for unkown reason */
if (p != NULL)
{
es->p = NULL;
pbuf_free(p);
}
ret_err = err;
}
else if(es->state == ES_ACCEPTED)
{
/* first data chunk in p->payload */
es->state = ES_RECEIVED;
/* store reference to incoming pbuf (chain) */
es->p = p;
/* install send completion notifier */
tcp_sent(tpcb, echo_sent);
echo_send(tpcb, es);
ret_err = ERR_OK;
}
else if (es->state == ES_RECEIVED)
{
/* read some more data */
if(es->p == NULL)
{
es->p = p;
tcp_sent(tpcb, echo_sent);
echo_send(tpcb, es);
}
else
{
struct pbuf *ptr;
/* chain pbufs to the end of what we recv'ed previously */
ptr = es->p;
pbuf_chain(ptr,p);
}
ret_err = ERR_OK;
}
else if(es->state == ES_CLOSING)
{
/* odd case, remote side closing twice, trash data */
tcp_recved(tpcb, p->tot_len);
es->p = NULL;
pbuf_free(p);
ret_err = ERR_OK;
}
else
{
/* unkown es->state, trash data */
tcp_recved(tpcb, p->tot_len);
es->p = NULL;
pbuf_free(p);
ret_err = ERR_OK;
}
return ret_err;
}
/**
* Send the raw IP packet to the given address. Note that actually you cannot
* modify the IP headers (this is inconsistent with the receive callback where
* you actually get the IP headers), you can only specify the IP payload here.
* It requires some more changes in lwIP. (there will be a raw_send() function
* then.)
*
* @param pcb the raw pcb which to send
* @param p the IP payload to send
* @param ipaddr the destination address of the IP packet
*
*/
err_t ICACHE_FLASH_ATTR
raw_sendto(struct raw_pcb *pcb, struct pbuf *p, ip_addr_t *ipaddr)
{
err_t err;
struct netif *netif;
ipX_addr_t *src_ip;
struct pbuf *q; /* q will be sent down the stack */
s16_t header_size;
ipX_addr_t *dst_ip = ip_2_ipX(ipaddr);
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_TRACE, ("raw_sendto\n"));
header_size = (
#if LWIP_IPV6
PCB_ISIPV6(pcb) ? IP6_HLEN :
#endif /* LWIP_IPV6 */
IP_HLEN);
/* not enough space to add an IP header to first pbuf in given p chain? */
if (pbuf_header(p, header_size)) {
/* allocate header in new pbuf */
q = pbuf_alloc(PBUF_IP, 0, PBUF_RAM);
/* new header pbuf could not be allocated? */
if (q == NULL) {
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS, ("raw_sendto: could not allocate header\n"));
return ERR_MEM;
}
if (p->tot_len != 0) {
/* chain header q in front of given pbuf p */
pbuf_chain(q, p);
}
/* { first pbuf q points to header pbuf } */
LWIP_DEBUGF(RAW_DEBUG, ("raw_sendto: added header pbuf %p before given pbuf %p\n", (void *)q, (void *)p));
} else {
/* first pbuf q equals given pbuf */
q = p;
if(pbuf_header(q, -header_size)) {
LWIP_ASSERT("Can't restore header we just removed!", 0);
return ERR_MEM;
}
}
netif = ipX_route(PCB_ISIPV6(pcb), &pcb->local_ip, dst_ip);
if (netif == NULL) {
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_LEVEL_WARNING, ("raw_sendto: No route to "));
ipX_addr_debug_print(PCB_ISIPV6(pcb), RAW_DEBUG | LWIP_DBG_LEVEL_WARNING, dst_ip);
/* free any temporary header pbuf allocated by pbuf_header() */
if (q != p) {
pbuf_free(q);
}
return ERR_RTE;
}
#if IP_SOF_BROADCAST
#if LWIP_IPV6
/* @todo: why does IPv6 not filter broadcast with SOF_BROADCAST enabled? */
if (!PCB_ISIPV6(pcb))
#endif /* LWIP_IPV6 */
{
/* broadcast filter? */
if (!ip_get_option(pcb, SOF_BROADCAST) && ip_addr_isbroadcast(ipaddr, netif)) {
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_LEVEL_WARNING, ("raw_sendto: SOF_BROADCAST not enabled on pcb %p\n", (void *)pcb));
/* free any temporary header pbuf allocated by pbuf_header() */
if (q != p) {
pbuf_free(q);
}
return ERR_VAL;
}
}
#endif /* IP_SOF_BROADCAST */
if (ipX_addr_isany(PCB_ISIPV6(pcb), &pcb->local_ip)) {
/* use outgoing network interface IP address as source address */
src_ip = ipX_netif_get_local_ipX(PCB_ISIPV6(pcb), netif, dst_ip);
#if LWIP_IPV6
if (src_ip == NULL) {
if (q != p) {
pbuf_free(q);
}
return ERR_RTE;
}
#endif /* LWIP_IPV6 */
} else {
/* use RAW PCB local IP address as source address */
src_ip = &pcb->local_ip;
}
NETIF_SET_HWADDRHINT(netif, &pcb->addr_hint);
err = ipX_output_if(PCB_ISIPV6(pcb), q, ipX_2_ip(src_ip), ipX_2_ip(dst_ip), pcb->ttl, pcb->tos, pcb->protocol, netif);
NETIF_SET_HWADDRHINT(netif, NULL);
/* did we chain a header earlier? */
if (q != p) {
/* free the header */
pbuf_free(q);
}
return err;
}
/**
* @brief This function is the implementation for tcp_recv LwIP callback
* @param arg: pointer on a argument for the tcp_pcb connection
* @param tpcb: pointer on the tcp_pcb connection
* @param pbuf: pointer on the received pbuf
* @param err: error information regarding the reveived pbuf
* @retval err_t: error code
*/
static err_t tcp_echoserver_recv(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err)
{
struct tcp_echoserver_struct *es;
err_t ret_err;
LWIP_ASSERT("arg != NULL",arg != NULL);
es = (struct tcp_echoserver_struct *)arg;
/* if we receive an empty tcp frame from client => close connection */
if (p == NULL)
{
/* remote host closed connection */
es->state = ES_CLOSING;
if(es->p == NULL)
{
/* we're done sending, close connection */
tcp_echoserver_connection_close(tpcb, es);
}
else
{
/* we're not done yet */
/* acknowledge received packet */
tcp_sent(tpcb, tcp_echoserver_sent);
/* send remaining data*/
tcp_echoserver_send(tpcb, es);
}
ret_err = ERR_OK;
}
/* else : a non empty frame was received from client but for some reason err != ERR_OK */
else if(err != ERR_OK)
{
/* free received pbuf*/
es->p = NULL;
pbuf_free(p);
ret_err = err;
}
else if(es->state == ES_ACCEPTED)
{
/* first data chunk in p->payload */
es->state = ES_RECEIVED;
/* store reference to incoming pbuf (chain) */
es->p = p;
/* initialize LwIP tcp_sent callback function */
tcp_sent(tpcb, tcp_echoserver_sent);
/* send back the received data (echo) */
tcp_echoserver_send(tpcb, es);
ret_err = ERR_OK;
}
else if (es->state == ES_RECEIVED)
{
/* more data received from client and previous data has been already sent*/
if(es->p == NULL)
{
es->p = p;
/* send back received data */
tcp_echoserver_send(tpcb, es);
}
else
{
struct pbuf *ptr;
/* chain pbufs to the end of what we recv'ed previously */
ptr = es->p;
pbuf_chain(ptr,p);
}
ret_err = ERR_OK;
}
/* data received when connection already closed */
else
{
/* Acknowledge data reception */
tcp_recved(tpcb, p->tot_len);
/* free pbuf and do nothing */
es->p = NULL;
pbuf_free(p);
ret_err = ERR_OK;
}
return ret_err;
}
//------------------------------------------------------------------------------------------------------------------------------------------------------
err_t CallbackOnRecieve(void *_arg, struct tcp_pcb *_tpcb, struct pbuf *_p, err_t _err)
{
err_t ret_err;
LWIP_ASSERT("arg != NULL", _arg != NULL);
struct State *ss = (struct State*)_arg;
if (_p == NULL)
{
// remote host closed connection
ss->state = S_CLOSING;
if (ss->p == NULL)
{
// we're done sending, close it
CloseConnection(_tpcb, ss);
}
else
{
// we're not done yet
//tcp_sent(_tpcb, CallbackOnSent);
}
ret_err = ERR_OK;
}
else if (_err != ERR_OK)
{
// cleanup, for unkown reason
if (_p != NULL)
{
ss->p = NULL;
pbuf_free(_p);
}
ret_err = _err;
}
else if (ss->state == S_ACCEPTED)
{
if (ss->numPort == POLICY_PORT)
{
pbuf_free(_p);
ss->state = S_RECIEVED;
SendAnswer(ss, _tpcb);
ss->state = S_CLOSING;
ret_err = ERR_OK;
}
else
{
// first data chunk in _p->payload
ss->state = S_RECIEVED;
// store reference to incoming pbuf (chain)
ss->p = _p;
Send(_tpcb, ss);
ret_err = ERR_OK;
}
}
else if (ss->state == S_RECIEVED)
{
// read some more data
if (ss->p == NULL)
{
//ss->p = _p;
//tcp_sent(_tpcb, CallbackOnSent);
//Send(_tpcb, ss);
SocketFuncReciever((char*)_p->payload, _p->len);
u8_t freed = 0;
do
{
// try hard to free pbuf
freed = pbuf_free(_p);
} while (freed == 0);
}
else
{
struct pbuf *ptr;
// chain pbufs to the end of what we recv'ed previously
ptr = ss->p;
pbuf_chain(ptr, _p);
}
ret_err = ERR_OK;
}
else if (ss->state == S_CLOSING)
{
// odd case, remote side closing twice, trash data
tcp_recved(_tpcb, _p->tot_len);
ss->p = NULL;
pbuf_free(_p);
ret_err = ERR_OK;
}
else
{
// unknown ss->state, trash data
tcp_recved(_tpcb, _p->tot_len);
ss->p = NULL;
pbuf_free(_p);
ret_err = ERR_OK;
}
return ret_err;
}
static err_t recv_tcp(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err) {
struct socket *s = arg;
struct sockreq *req;
struct sockreq *next;
int bytes;
int waitrecv;
int bytesrecv = 0;
if (p) {
if (s->tcp.recvtail) {
pbuf_chain(s->tcp.recvtail, p);
s->tcp.recvtail = p;
} else {
s->tcp.recvhead = p;
s->tcp.recvtail = p;
}
} else {
s->state = SOCKSTATE_CLOSING;
set_io_event(&s->iob, IOEVT_CLOSE);
}
if (s->state == SOCKSTATE_CLOSING && s->tcp.recvhead == NULL) {
req = s->waithead;
while (req) {
next = req->next;
if (req->type == SOCKREQ_RECV || req->type == SOCKREQ_WAITRECV) release_socket_request(req, 0);
req = next;
}
return 0;
}
while (1) {
if (!s->tcp.recvhead) break;
req = s->waithead;
waitrecv = 0;
while (req) {
if (req->type == SOCKREQ_RECV) break;
if (req->type == SOCKREQ_WAITRECV) waitrecv++;
req = req->next;
}
if (!req) {
if (waitrecv) {
req = s->waithead;
while (req) {
next = req->next;
if (req->type == SOCKREQ_WAITRECV) release_socket_request(req, 0);
req = next;
}
}
break;
}
bytes = fetch_rcvbuf(s, req->msg->msg_iov, req->msg->msg_iovlen);
if (bytes > 0) {
bytesrecv += bytes;
req->rc += bytes;
release_socket_request(req, req->rc);
}
}
if (bytesrecv) tcp_recved(pcb, bytesrecv);
if (s->tcp.recvhead) {
set_io_event(&s->iob, IOEVT_READ);
} else {
clear_io_event(&s->iob, IOEVT_READ);
}
return 0;
}
/**
* Send the raw IP packet to the given address. Note that actually you cannot
* modify the IP headers (this is inconsistent with the receive callback where
* you actually get the IP headers), you can only specify the IP payload here.
* It requires some more changes in lwIP. (there will be a raw_send() function
* then.)
*
* @param pcb the raw pcb which to send
* @param p the IP payload to send
* @param ipaddr the destination address of the IP packet
*
*/
err_t
raw_sendto(struct raw_pcb *pcb, struct pbuf *p, const ip_addr_t *ipaddr)
{
err_t err;
struct netif *netif;
const ip_addr_t *src_ip;
struct pbuf *q; /* q will be sent down the stack */
s16_t header_size;
const ip_addr_t *dst_ip = ipaddr;
if ((pcb == NULL) || (ipaddr == NULL) || !IP_ADDR_PCB_VERSION_MATCH(pcb, ipaddr)) {
return ERR_VAL;
}
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_TRACE, ("raw_sendto\n"));
header_size = (
#if LWIP_IPV4 && LWIP_IPV6
IP_IS_V6(ipaddr) ? IP6_HLEN : IP_HLEN);
#elif LWIP_IPV4
IP_HLEN);
#else
IP6_HLEN);
#endif
/* not enough space to add an IP header to first pbuf in given p chain? */
if (pbuf_header(p, header_size)) {
/* allocate header in new pbuf */
q = pbuf_alloc(PBUF_IP, 0, PBUF_RAM);
/* new header pbuf could not be allocated? */
if (q == NULL) {
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS, ("raw_sendto: could not allocate header\n"));
return ERR_MEM;
}
if (p->tot_len != 0) {
/* chain header q in front of given pbuf p */
pbuf_chain(q, p);
}
/* { first pbuf q points to header pbuf } */
LWIP_DEBUGF(RAW_DEBUG, ("raw_sendto: added header pbuf %p before given pbuf %p\n", (void *)q, (void *)p));
} else {
/* first pbuf q equals given pbuf */
q = p;
if (pbuf_header(q, -header_size)) {
LWIP_ASSERT("Can't restore header we just removed!", 0);
return ERR_MEM;
}
}
netif = ip_route(&pcb->local_ip, dst_ip);
if (netif == NULL) {
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_LEVEL_WARNING, ("raw_sendto: No route to "));
ip_addr_debug_print(RAW_DEBUG | LWIP_DBG_LEVEL_WARNING, dst_ip);
/* free any temporary header pbuf allocated by pbuf_header() */
if (q != p) {
pbuf_free(q);
}
return ERR_RTE;
}
#if IP_SOF_BROADCAST
if (IP_IS_V4(ipaddr))
{
/* broadcast filter? */
if (!ip_get_option(pcb, SOF_BROADCAST) && ip_addr_isbroadcast(ipaddr, netif)) {
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_LEVEL_WARNING, ("raw_sendto: SOF_BROADCAST not enabled on pcb %p\n", (void *)pcb));
/* free any temporary header pbuf allocated by pbuf_header() */
if (q != p) {
pbuf_free(q);
}
return ERR_VAL;
}
}
#endif /* IP_SOF_BROADCAST */
if (ip_addr_isany(&pcb->local_ip)) {
/* use outgoing network interface IP address as source address */
src_ip = ip_netif_get_local_ip(netif, dst_ip);
#if LWIP_IPV6
if (src_ip == NULL) {
if (q != p) {
pbuf_free(q);
}
return ERR_RTE;
}
#endif /* LWIP_IPV6 */
} else {
/* use RAW PCB local IP address as source address */
src_ip = &pcb->local_ip;
}
#if LWIP_IPV6
/* If requested, based on the IPV6_CHECKSUM socket option per RFC3542,
compute the checksum and update the checksum in the payload. */
if (IP_IS_V6(dst_ip) && pcb->chksum_reqd) {
u16_t chksum = ip6_chksum_pseudo(p, pcb->protocol, p->tot_len, ip_2_ip6(src_ip), ip_2_ip6(dst_ip));
LWIP_ASSERT("Checksum must fit into first pbuf", p->len >= (pcb->chksum_offset + 2));
SMEMCPY(((u8_t *)p->payload) + pcb->chksum_offset, &chksum, sizeof(u16_t));
}
#endif
NETIF_SET_HWADDRHINT(netif, &pcb->addr_hint);
err = ip_output_if(q, src_ip, dst_ip, pcb->ttl, pcb->tos, pcb->protocol, netif);
NETIF_SET_HWADDRHINT(netif, NULL);
/* did we chain a header earlier? */
if (q != p) {
/* free the header */
pbuf_free(q);
}
return err;
}
/**
* Send data to a specified address using UDP.
* The netif used for sending can be specified.
*
* This function exists mainly for DHCP, to be able to send UDP packets
* on a netif that is still down.
*
* @param pcb UDP PCB used to send the data.
* @param p chain of pbuf's to be sent.
* @param dst_ip Destination IP address.
* @param dst_port Destination UDP port.
* @param netif the netif used for sending.
*
* dst_ip & dst_port are expected to be in the same byte order as in the pcb.
*
* @return lwIP error code (@see udp_send for possible error codes)
*
* @see udp_disconnect() udp_send()
*/
err_t
udp_sendto_if(struct udp_pcb *pcb, struct pbuf *p,
struct ip_addr *dst_ip, u16_t dst_port, struct netif *netif)
{
struct udp_hdr *udphdr;
struct ip_addr *src_ip;
err_t err;
struct pbuf *q; /* q will be sent down the stack */
#if IP_SOF_BROADCAST
/* broadcast filter? */
if ( ((pcb->so_options & SOF_BROADCAST) == 0) && ip_addr_isbroadcast(dst_ip, netif) ) {
LWIP_DEBUGF(UDP_DEBUG | LWIP_DBG_LEVEL_SERIOUS,
("udp_sendto_if: SOF_BROADCAST not enabled on pcb %p\n", (void *)pcb));
return ERR_VAL;
}
#endif /* IP_SOF_BROADCAST */
/* if the PCB is not yet bound to a port, bind it here */
if (pcb->local_port == 0) {
LWIP_DEBUGF(UDP_DEBUG | LWIP_DBG_TRACE, ("udp_send: not yet bound to a port, binding now\n"));
err = udp_bind(pcb, &pcb->local_ip, pcb->local_port);
if (err != ERR_OK) {
LWIP_DEBUGF(UDP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS, ("udp_send: forced port bind failed\n"));
return err;
}
}
/* not enough space to add an UDP header to first pbuf in given p chain? */
if (pbuf_header(p, UDP_HLEN)) {
/* allocate header in a separate new pbuf */
q = pbuf_alloc(PBUF_IP, UDP_HLEN, PBUF_RAM);
/* new header pbuf could not be allocated? */
if (q == NULL) {
LWIP_DEBUGF(UDP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS, ("udp_send: could not allocate header\n"));
return ERR_MEM;
}
/* chain header q in front of given pbuf p */
pbuf_chain(q, p);
/* first pbuf q points to header pbuf */
LWIP_DEBUGF(UDP_DEBUG,
("udp_send: added header pbuf %p before given pbuf %p\n", (void *)q, (void *)p));
} else {
/* adding space for header within p succeeded */
/* first pbuf q equals given pbuf */
q = p;
LWIP_DEBUGF(UDP_DEBUG, ("udp_send: added header in given pbuf %p\n", (void *)p));
}
LWIP_ASSERT("check that first pbuf can hold struct udp_hdr",
(q->len >= sizeof(struct udp_hdr)));
/* q now represents the packet to be sent */
udphdr = q->payload;
udphdr->src = htons(pcb->local_port);
udphdr->dest = htons(dst_port);
/* in UDP, 0 checksum means 'no checksum' */
udphdr->chksum = 0x0000;
/* PCB local address is IP_ANY_ADDR? */
if (ip_addr_isany(&pcb->local_ip)) {
/* use outgoing network interface IP address as source address */
src_ip = &(netif->ip_addr);
} else {
/* check if UDP PCB local IP address is correct
* this could be an old address if netif->ip_addr has changed */
if (!ip_addr_cmp(&(pcb->local_ip), &(netif->ip_addr))) {
/* local_ip doesn't match, drop the packet */
if (q != p) {
/* free the header pbuf */
pbuf_free(q);
q = NULL;
/* p is still referenced by the caller, and will live on */
}
return ERR_VAL;
}
/* use UDP PCB local IP address as source address */
src_ip = &(pcb->local_ip);
}
LWIP_DEBUGF(UDP_DEBUG, ("udp_send: sending datagram of length %"U16_F"\n", q->tot_len));
#if LWIP_UDPLITE
/* UDP Lite protocol? */
if (pcb->flags & UDP_FLAGS_UDPLITE) {
u16_t chklen, chklen_hdr;
LWIP_DEBUGF(UDP_DEBUG, ("udp_send: UDP LITE packet length %"U16_F"\n", q->tot_len));
/* set UDP message length in UDP header */
chklen_hdr = chklen = pcb->chksum_len_tx;
if ((chklen < sizeof(struct udp_hdr)) || (chklen > q->tot_len)) {
if (chklen != 0) {
LWIP_DEBUGF(UDP_DEBUG, ("udp_send: UDP LITE pcb->chksum_len is illegal: %"U16_F"\n", chklen));
}
/* For UDP-Lite, checksum length of 0 means checksum
over the complete packet. (See RFC 3828 chap. 3.1)
At least the UDP-Lite header must be covered by the
checksum, therefore, if chksum_len has an illegal
value, we generate the checksum over the complete
packet to be safe. */
chklen_hdr = 0;
chklen = q->tot_len;
}
udphdr->len = htons(chklen_hdr);
/* calculate checksum */
#if CHECKSUM_GEN_UDP
udphdr->chksum = inet_chksum_pseudo_partial(q, src_ip, dst_ip,
IP_PROTO_UDPLITE, q->tot_len, chklen);
/* chksum zero must become 0xffff, as zero means 'no checksum' */
if (udphdr->chksum == 0x0000)
udphdr->chksum = 0xffff;
#endif /* CHECKSUM_CHECK_UDP */
/* output to IP */
LWIP_DEBUGF(UDP_DEBUG, ("udp_send: ip_output_if (,,,,IP_PROTO_UDPLITE,)\n"));
#if LWIP_NETIF_HWADDRHINT
netif->addr_hint = &(pcb->addr_hint);
#endif /* LWIP_NETIF_HWADDRHINT*/
err = ip_output_if(q, src_ip, dst_ip, pcb->ttl, pcb->tos, IP_PROTO_UDPLITE, netif);
#if LWIP_NETIF_HWADDRHINT
netif->addr_hint = NULL;
#endif /* LWIP_NETIF_HWADDRHINT*/
} else
#endif /* LWIP_UDPLITE */
{ /* UDP */
LWIP_DEBUGF(UDP_DEBUG, ("udp_send: UDP packet length %"U16_F"\n", q->tot_len));
udphdr->len = htons(q->tot_len);
/* calculate checksum */
#if CHECKSUM_GEN_UDP
if ((pcb->flags & UDP_FLAGS_NOCHKSUM) == 0) {
udphdr->chksum = inet_chksum_pseudo(q, src_ip, dst_ip, IP_PROTO_UDP, q->tot_len);
/* chksum zero must become 0xffff, as zero means 'no checksum' */
if (udphdr->chksum == 0x0000) udphdr->chksum = 0xffff;
}
#endif /* CHECKSUM_CHECK_UDP */
LWIP_DEBUGF(UDP_DEBUG, ("udp_send: UDP checksum 0x%04"X16_F"\n", udphdr->chksum));
LWIP_DEBUGF(UDP_DEBUG, ("udp_send: ip_output_if (,,,,IP_PROTO_UDP,)\n"));
/* output to IP */
#if LWIP_NETIF_HWADDRHINT
netif->addr_hint = &(pcb->addr_hint);
#endif /* LWIP_NETIF_HWADDRHINT*/
err = ip_output_if(q, src_ip, dst_ip, pcb->ttl, pcb->tos, IP_PROTO_UDP, netif);
#if LWIP_NETIF_HWADDRHINT
netif->addr_hint = NULL;
#endif /* LWIP_NETIF_HWADDRHINT*/
}
/* TODO: must this be increased even if error occured? */
snmp_inc_udpoutdatagrams();
/* did we chain a separate header pbuf earlier? */
if (q != p) {
/* free the header pbuf */
pbuf_free(q);
q = NULL;
/* p is still referenced by the caller, and will live on */
}
UDP_STATS_INC(udp.xmit);
return err;
}
void
l2cap_process_sig(struct pbuf *q, struct l2cap_hdr *l2caphdr, struct bd_addr *bdaddr)
{
struct l2cap_sig_hdr *sighdr;
struct l2cap_sig *sig = NULL;
struct l2cap_pcb *pcb = NULL;
struct l2cap_pcb_listen *lpcb;
struct l2cap_cfgopt_hdr *opthdr;
u16_t result, status, flags, psm, dcid, scid;
u16_t len;
u16_t siglen;
struct pbuf *p, *r = NULL, *s = NULL, *data;
err_t ret;
u8_t i;
u16_t rspstate = L2CAP_CFG_SUCCESS;
if(q->len != q->tot_len) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Fragmented packet received. Reassemble into one buffer\n"));
if((p = pbuf_alloc(PBUF_RAW, q->tot_len, PBUF_RAM)) != NULL) {
i = 0;
for(r = q; r != NULL; r = r->next) {
memcpy(((u8_t *)p->payload) + i, r->payload, r->len);
i += r->len;
}
} else {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Could not allocate buffer for fragmented packet\n"));
return;
}
} else {
p = q;
}
len = l2caphdr->len;
while(len > 0) {
/* Set up signal header */
sighdr = p->payload;
pbuf_header(p, -L2CAP_SIGHDR_LEN);
/* Check if this is a response/reject signal, and if so, find the matching request */
if(sighdr->code % 2) { /* if odd this is a resp/rej signal */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Response/reject signal received id = %d code = %d\n",
sighdr->id, sighdr->code));
for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
for(sig = pcb->unrsp_sigs; sig != NULL; sig = sig->next) {
if(sig->sigid == sighdr->id) {
break; /* found */
}
}
if(sig != NULL) {
break;
}
}
} else {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Request signal received id = %d code = %d\n",
sighdr->id, sighdr->code));
}
/* Reject packet if length exceeds MTU */
if(l2caphdr->len > L2CAP_MTU) {
/* Alloc size of reason in cmd rej + MTU */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+2, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_MTU_EXCEEDED;
((u16_t *)data->payload)[1] = L2CAP_MTU;
l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
}
break;
}
switch(sighdr->code) {
case L2CAP_CMD_REJ:
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Our command was rejected so we disconnect\n"));
l2ca_disconnect_req(pcb, NULL);
break;
case L2CAP_CONN_REQ:
psm = ((u16_t *)p->payload)[0];
/* Search for a listening pcb */
for(lpcb = l2cap_listen_pcbs; lpcb != NULL; lpcb = lpcb->next) {
if(lpcb->psm == psm) {
/* Found a listening pcb with the correct PSM */
break;
}
}
/* If no matching pcb was found, send a connection rsp neg (PSM) */
if(lpcb == NULL) {
/* Alloc size of data in conn rsp signal */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_CONN_REF_PSM;
((u16_t *)data->payload)[1] = 0; /* No further info available */
ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
}
} else {
/* Initiate a new active pcb */
pcb = l2cap_new();
if(pcb == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: could not allocate PCB\n"));
/* Send a connection rsp neg (no resources available) and alloc size of data in conn rsp
signal */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_CONN_REF_RES;
((u16_t *)data->payload)[1] = 0; /* No further info available */
ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
}
}
bd_addr_set(&(pcb->remote_bdaddr),bdaddr);
pcb->scid = l2cap_cid_alloc();
pcb->dcid = ((u16_t *)p->payload)[1];
pcb->psm = psm;
pcb->callback_arg = lpcb->callback_arg;
pcb->l2ca_connect_ind = lpcb->l2ca_connect_ind;
pcb->state = L2CAP_CONFIG;
L2CAP_REG(&l2cap_active_pcbs, pcb);
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: A connection request was received. Send a response\n"));
data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM);
if(data == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_connect_rsp: Could not allocate memory for pbuf\n"));
break;
}
((u16_t *)data->payload)[0] = pcb->scid;
((u16_t *)data->payload)[1] = pcb->dcid;
((u16_t *)data->payload)[2] = L2CAP_CONN_SUCCESS;
((u16_t *)data->payload)[3] = 0x0000; /* No further information available */
/* Send the response */
ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
}
break;
case L2CAP_CONN_RSP:
if(pcb == NULL) {
/* A response without a matching request is silently discarded */
break;
}
LWIP_ASSERT("l2cap_process_sig: conn rsp, active pcb->state == W4_L2CAP_CONNECT_RSP\n",
pcb->state == W4_L2CAP_CONNECT_RSP);
result = ((u16_t *)p->payload)[2];
status = ((u16_t *)p->payload)[3];
switch(result) {
case L2CAP_CONN_SUCCESS:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Conn_rsp_sucess, status %d\n", status));
LWIP_ASSERT("l2cap_process_sig: conn rsp success, pcb->scid == ((u16_t *)p->payload)[1]\n",
pcb->scid == ((u16_t *)p->payload)[1]);
/* Set destination connection id */
pcb->dcid = ((u16_t *)p->payload)[0];
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
/* Configure connection */
pcb->state = L2CAP_CONFIG;
/* If initiator send a configuration request */
if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
l2ca_config_req(pcb);
pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_REQ;
}
break;
case L2CAP_CONN_PND:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Conn_rsp_pnd, status %d\n", status));
/* Disable rtx and enable ertx */
sig->rtx = 0;
sig->ertx = L2CAP_ERTX;
break;
default:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Conn_rsp_neg, result %d\n", result));
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
L2CA_ACTION_CONN_CFM(pcb,result,status,ret);
break;
}
break;
case L2CAP_CFG_REQ:
siglen = sighdr->len;
dcid = ((u16_t *)p->payload)[0];
flags = ((u16_t *)p->payload)[1];
siglen -= 4;
pbuf_header(p, -4);
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Congfiguration request, flags = %d\n", flags));
/* Find PCB with matching cid */
for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: dcid = 0x%x, pcb->scid = 0x%x, pcb->dcid = 0x%x\n\n", dcid, pcb->scid, pcb->dcid));
if(pcb->scid == dcid) {
/* Matching cid found */
break;
}
}
/* If no matching cid was found, send a cmd reject (Invalid cid) */
if(pcb == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Cfg req: no matching cid was found\n"));
/* Alloc size of reason in cmd rej + data (dcid + scid) */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_INVALID_CID;
((u16_t *)data->payload)[1] = dcid; /* Requested local cid */
((u16_t *)data->payload)[2] = L2CAP_NULL_CID; /* Remote cid not known */
ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
}
} else { /* Handle config request */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Handle configuration request\n"));
pcb->ursp_id = sighdr->id; /* Set id of request to respond to */
/* Parse options and add to pcb */
while(siglen > 0) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Siglen = %d\n", siglen));
opthdr = p->payload;
/* Check if type of action bit indicates a non-hint. Hints are ignored */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Type of action bit = %d\n", L2CAP_OPTH_TOA(opthdr)));
if(L2CAP_OPTH_TOA(opthdr) == 0) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Type = %d\n", L2CAP_OPTH_TYPE(opthdr)));
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Length = %d\n", opthdr->len));
switch(L2CAP_OPTH_TYPE(opthdr)) {
case L2CAP_CFG_MTU:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Out MTU = %d\n", ((u16_t *)p->payload)[1]));
pcb->cfg.outmtu = ((u16_t *)p->payload)[1];
break;
case L2CAP_FLUSHTO:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: In flush timeout = %d\n", ((u16_t *)p->payload)[1]));
pcb->cfg.influshto = ((u16_t *)p->payload)[1];
break;
case L2CAP_QOS:
/* If service type is Best Effort or No Traffic the remainder fields will be ignored */
if(((u8_t *)p->payload)[3] == L2CAP_QOS_GUARANTEED) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: This implementation does not support the guaranteed QOS service type"));
if(rspstate == L2CAP_CFG_SUCCESS) {
rspstate = L2CAP_CFG_UNACCEPT;
if(pcb->cfg.opt != NULL) {
pbuf_free(pcb->cfg.opt);
pcb->cfg.opt = NULL;
}
}
s = pbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + opthdr->len, PBUF_RAM);
memcpy((u8_t *)s->payload, (u8_t *)p->payload, L2CAP_CFGOPTHDR_LEN + opthdr->len);
if(pcb->cfg.opt == NULL) {
pcb->cfg.opt = s;
} else {
pbuf_chain(pcb->cfg.opt, s);
pbuf_free(s);
}
}
break;
default:
if(rspstate != L2CAP_CFG_REJ) {
/* Unknown option. Add to unknown option type buffer */
if(rspstate != L2CAP_CFG_UNKNOWN) {
rspstate = L2CAP_CFG_UNKNOWN;
if(pcb->cfg.opt != NULL) {
pbuf_free(pcb->cfg.opt);
pcb->cfg.opt = NULL;
}
}
s = pbuf_alloc(PBUF_RAW, L2CAP_CFGOPTHDR_LEN + opthdr->len, PBUF_RAM);
memcpy((u8_t *)s->payload, (u8_t *)p->payload, L2CAP_CFGOPTHDR_LEN + opthdr->len);
if(pcb->cfg.opt == NULL) {
pcb->cfg.opt = s;
} else {
pbuf_chain(pcb->cfg.opt, s);
pbuf_free(s);
}
}
break;
} /* switch */
} /* if(L2CAP_OPTH_TOA(opthdr) == 0) */
pbuf_header(p, -(L2CAP_CFGOPTHDR_LEN + opthdr->len));
siglen -= L2CAP_CFGOPTHDR_LEN + opthdr->len;
} /* while */
/* If continuation flag is set we don't send the final response just yet */
if((flags & 0x0001) == 1) {
/* Send success result with no options until the full request has been received */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CFG_RSP_SIZE, PBUF_RAM)) == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Could not allocate memory for pbuf\n"));
break;
}
((u16_t *)data->payload)[0] = pcb->dcid;
((u16_t *)data->payload)[1] = 0;
((u16_t *)data->payload)[2] = L2CAP_CFG_SUCCESS;
ret = l2cap_signal(pcb, L2CAP_CFG_RSP, pcb->ursp_id, &(pcb->remote_bdaddr), data);
break;
}
/* Send a configure request for outgoing link if it hasnt been configured */
if(!(pcb->cfg.l2capcfg & L2CAP_CFG_IR) && !(pcb->cfg.l2capcfg & L2CAP_CFG_OUT_REQ)) {
l2ca_config_req(pcb);
pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_REQ;
}
/* Send response to configuration request */
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Send response to configuration request\n"));
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CFG_RSP_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = pcb->dcid;
((u16_t *)data->payload)[1] = 0; /* Flags (No continuation) */
((u16_t *)data->payload)[2] = rspstate; /* Result */
if(pcb->cfg.opt != NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: pcb->cfg.opt->len = %d\n", pcb->cfg.opt->len));
pbuf_chain(data, pcb->cfg.opt); /* Add option type buffer to data buffer */
pbuf_free(pcb->cfg.opt);
pcb->cfg.opt = NULL;
}
ret = l2cap_signal(pcb, L2CAP_CFG_RSP, pcb->ursp_id, &(pcb->remote_bdaddr), data);
}
if(rspstate == L2CAP_CFG_SUCCESS) {
pcb->cfg.l2capcfg |= L2CAP_CFG_OUT_SUCCESS;
/* L2CAP connection established if a successful configuration response has been sent */
if(pcb->cfg.l2capcfg & L2CAP_CFG_IN_SUCCESS) {
/* IPCP connection established, notify upper layer that connection is open */
pcb->state = L2CAP_OPEN;
if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_SUCCESS, 0x0000, ret);
} else {
L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
}
}
}
} /* else */
break;
case L2CAP_CFG_RSP:
if(pcb == NULL) {
/* A response without a matching request is silently discarded */
break;
}
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
LWIP_ASSERT(("l2cap_process_sig: cfg rsp, active pcb->state == L2CAP_CONFIG\n"),
pcb->state == L2CAP_CONFIG);
siglen = sighdr->len;
scid = ((u16_t *)p->payload)[0];
flags = ((u16_t *)p->payload)[1];
result = ((u16_t *)p->payload)[2];
siglen -= 6;
pbuf_header(p, -6);
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Outgoing configuration result == %d continuation flag == %d\n", result, flags));
/* Handle config request */
switch(result) {
case L2CAP_CFG_SUCCESS:
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Successfull outgoing configuration\n"));
pcb->cfg.l2capcfg |= L2CAP_CFG_IN_SUCCESS; /* Local side of the connection
has been configured for outgoing data */
pcb->cfg.cfgto = L2CAP_CFG_TO; /* Reset configuration timeout */
if(pcb->cfg.outflushto != L2CAP_CFG_DEFAULT_OUTFLUSHTO) {
lp_write_flush_timeout(&pcb->remote_bdaddr, pcb->cfg.outflushto);
}
/* L2CAP connection established if a successful configuration response has been sent */
if(pcb->cfg.l2capcfg & L2CAP_CFG_OUT_SUCCESS) {
pcb->state = L2CAP_OPEN;
if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_SUCCESS, 0x0000, ret);
} else {
L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
}
}
break;
case L2CAP_CFG_UNACCEPT:
/* Parse and add options to pcb */
while(siglen > 0) {
opthdr = p->payload;
/* Check if type of action bit indicates a non-hint. Hints are ignored */
if(L2CAP_OPTH_TOA(opthdr) == 0) {
switch(L2CAP_OPTH_TYPE(opthdr)) {
case L2CAP_CFG_MTU:
if(L2CAP_MTU > ((u16_t *)p->payload)[1]) {
pcb->cfg.outmtu = ((u16_t *)p->payload)[1];
} else {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Configuration of MTU failed\n"));
l2ca_disconnect_req(pcb, NULL);
return;
}
break;
case L2CAP_FLUSHTO:
pcb->cfg.influshto = ((u16_t *)p->payload)[1];
break;
case L2CAP_QOS:
/* If service type Best Effort is not accepted we will close the connection */
if(((u8_t *)p->payload)[3] != L2CAP_QOS_BEST_EFFORT) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Unsupported service type\n"));
l2ca_disconnect_req(pcb, NULL);
return;
}
break;
default:
/* Should not happen, skip option */
break;
} /* switch */
} /* if(L2CAP_OPTH_TOA(opthdr) == 0) */
pbuf_header(p, -(L2CAP_CFGOPTHDR_LEN + opthdr->len));
siglen -= L2CAP_CFGOPTHDR_LEN + opthdr->len;
} /* while */
/* Send out a new configuration request if the continuation flag isn't set */
if((flags & 0x0001) == 0) {
l2ca_config_req(pcb);
}
break;
case L2CAP_CFG_REJ:
/* Fallthrough */
case L2CAP_CFG_UNKNOWN:
/* Fallthrough */
default:
if((flags & 0x0001) == 0) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Configuration failed\n"));
l2ca_disconnect_req(pcb, NULL);
return;
}
break;
} /* switch(result) */
/* If continuation flag is set we must send a NULL configuration request */
if((flags & 0x0001) == 1) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Continuation flag is set. Send empty (default) config request signal\n"));
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CFG_REQ_SIZE, PBUF_RAM)) == NULL) {
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Could not allocate memory for pbuf\n"));
return;
}
/* Assemble config request packet */
((u16_t *)data->payload)[0] = pcb->scid;
((u16_t *)data->payload)[2] = 0;
l2cap_signal(pcb, L2CAP_CFG_REQ, 0, &(pcb->remote_bdaddr), data);
}
break;
case L2CAP_DISCONN_REQ:
siglen = sighdr->len;
dcid = ((u16_t *)p->payload)[0];
siglen = siglen - 2;
flags = ((u16_t *)p->payload)[1];
siglen = siglen - 2;
pbuf_header(p, -4);
/* Find PCB with matching cid */
for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
if(pcb->scid == dcid) {
/* Matching cid found */
break;
}
}
/* If no matching cid was found, send a cmd reject (Invalid cid) */
if(pcb == NULL) {
/* Alloc size of reason in cmd rej + data (dcid + scid) */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+4, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_INVALID_CID;
((u16_t *)data->payload)[1] = dcid; /* Requested local cid */
((u16_t *)data->payload)[2] = L2CAP_NULL_CID; /* Remote cid not known */
ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
}
} else { /* Handle disconnection request */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_DISCONN_RSP_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = pcb->scid;
((u16_t *)data->payload)[1] = pcb->dcid;
ret = l2cap_signal(pcb, L2CAP_DISCONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
/* Give upper layer indication */
pcb->state = L2CAP_CLOSED;
LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Disconnection request\n"));
L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
}
}
break;
case L2CAP_DISCONN_RSP:
if(pcb == NULL) {
/* A response without a matching request is silently discarded */
break;
}
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
L2CA_ACTION_DISCONN_CFM(pcb,ret); /* NOTE: Application should
now close the connection */
break;
case L2CAP_ECHO_REQ:
if( pcb != NULL)
{
pcb->ursp_id = sighdr->id;
ret = l2cap_signal(pcb, L2CAP_ECHO_RSP, sighdr->id, &(pcb->remote_bdaddr), p);
} else {
ret = l2cap_signal(NULL, L2CAP_ECHO_RSP, sighdr->id, bdaddr, p);
}
pbuf_free(p);
break;
case L2CAP_ECHO_RSP:
if(pcb == NULL) {
/* A response without a matching request is silently discarded */
break;
}
/* Remove signal from unresponded list and deallocate it */
L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
pbuf_free(sig->p);
lwbt_memp_free(MEMP_L2CAP_SIG, sig);
/* Remove temporary pcb from active list */
L2CAP_RMV(&l2cap_active_pcbs, pcb);
L2CA_ACTION_PING_CFM(pcb,L2CAP_ECHO_RCVD,ret);
break;
default:
/* Alloc size of reason in cmd rej */
if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE, PBUF_RAM)) != NULL) {
((u16_t *)data->payload)[0] = L2CAP_CMD_NOT_UNDERSTOOD;
ret = l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
}
break;
} /* switch */
len = len - (sighdr->len + L2CAP_SIGHDR_LEN);
pbuf_header(p, -(sighdr->len));
} /* while */
}
/*-----------------------------------------------------------------------------------*/
err_t
tcp_enqueue(struct tcp_pcb *pcb, void *arg, u16_t len,
u8_t flags, u8_t copy,
u8_t *optdata, u8_t optlen)
{
struct pbuf *p;
struct tcp_seg *seg, *useg, *queue;
u32_t left, seqno;
u16_t seglen;
void *ptr;
u8_t queuelen;
left = len;
ptr = arg;
if(len > pcb->snd_buf) {
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue: too much data %d\n", len));
return ERR_MEM;
}
seqno = pcb->snd_lbb;
queue = NULL;
DEBUGF(TCP_QLEN_DEBUG, ("tcp_enqueue: %d\n", pcb->snd_queuelen));
queuelen = pcb->snd_queuelen;
if(queuelen >= TCP_SND_QUEUELEN) {
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue: too long queue %d (max %d)\n", queuelen, TCP_SND_QUEUELEN));
goto memerr;
}
#ifdef LWIP_DEBUG
if(pcb->snd_queuelen != 0) {
ASSERT("tcp_enqueue: valid queue length", pcb->unacked != NULL ||
pcb->unsent != NULL);
}
#endif /* LWIP_DEBUG */
seg = NULL;
seglen = 0;
while(queue == NULL || left > 0) {
seglen = left > pcb->mss? pcb->mss: left;
/* allocate memory for tcp_seg, and fill in fields */
seg = memp_malloc(MEMP_TCP_SEG);
if(seg == NULL) {
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue: could not allocate memory for tcp_seg\n"));
goto memerr;
}
seg->next = NULL;
seg->p = NULL;
if(queue == NULL) {
queue = seg;
} else {
for(useg = queue; useg->next != NULL; useg = useg->next);
useg->next = seg;
}
/* If copy is set, memory should be allocated
and data copied into pbuf, otherwise data comes from
ROM or other static memory, and need not be copied. If
optdata is != NULL, we have options instead of data. */
if(optdata != NULL) {
if((seg->p = pbuf_alloc(PBUF_TRANSPORT, optlen, PBUF_RAM)) == NULL) {
goto memerr;
}
++queuelen;
seg->dataptr = seg->p->payload;
} else if(copy) {
if((seg->p = pbuf_alloc(PBUF_TRANSPORT, seglen, PBUF_RAM)) == NULL) {
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue: could not allocate memory for pbuf copy\n"));
goto memerr;
}
++queuelen;
if(arg != NULL) {
memcpy(seg->p->payload, ptr, seglen);
}
seg->dataptr = seg->p->payload;
} else {
/* Do not copy the data. */
if((p = pbuf_alloc(PBUF_TRANSPORT, seglen, PBUF_ROM)) == NULL) {
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue: could not allocate memory for pbuf non-copy\n"));
goto memerr;
}
++queuelen;
p->payload = ptr;
seg->dataptr = ptr;
if((seg->p = pbuf_alloc(PBUF_TRANSPORT, 0, PBUF_RAM)) == NULL) {
pbuf_free(p);
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue: could not allocate memory for header pbuf\n"));
goto memerr;
}
++queuelen;
pbuf_chain(seg->p, p);
}
if(queuelen > TCP_SND_QUEUELEN) {
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue: queue too long %d (%d)\n", queuelen, TCP_SND_QUEUELEN));
goto memerr;
}
seg->len = seglen;
/* if((flags & TCP_SYN) || (flags & TCP_FIN)) {
++seg->len;
}*/
/* build TCP header */
if(pbuf_header(seg->p, TCP_HLEN)) {
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue: no room for TCP header in pbuf.\n"));
#ifdef TCP_STATS
++stats.tcp.err;
#endif /* TCP_STATS */
goto memerr;
}
seg->tcphdr = seg->p->payload;
seg->tcphdr->src = htons(pcb->local_port);
seg->tcphdr->dest = htons(pcb->remote_port);
seg->tcphdr->seqno = htonl(seqno);
seg->tcphdr->urgp = 0;
TCPH_FLAGS_SET(seg->tcphdr, flags);
/* don't fill in tcphdr->ackno and tcphdr->wnd until later */
if(optdata == NULL) {
TCPH_OFFSET_SET(seg->tcphdr, 5 << 4);
} else {
TCPH_OFFSET_SET(seg->tcphdr, (5 + optlen / 4) << 4);
/* Copy options into data portion of segment.
Options can thus only be sent in non data carrying
segments such as SYN|ACK. */
memcpy(seg->dataptr, optdata, optlen);
}
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_enqueue: queueing %lu:%lu (0x%x)\n",
ntohl(seg->tcphdr->seqno),
ntohl(seg->tcphdr->seqno) + TCP_TCPLEN(seg),
flags));
left -= seglen;
seqno += seglen;
ptr = (void *)((char *)ptr + seglen);
}
/* Go to the last segment on the ->unsent queue. */
if(pcb->unsent == NULL) {
useg = NULL;
} else {
for(useg = pcb->unsent; useg->next != NULL; useg = useg->next);
}
/* If there is room in the last pbuf on the unsent queue,
chain the first pbuf on the queue together with that. */
if(useg != NULL &&
TCP_TCPLEN(useg) != 0 &&
!(TCPH_FLAGS(useg->tcphdr) & (TCP_SYN | TCP_FIN)) &&
!(flags & (TCP_SYN | TCP_FIN)) &&
useg->len + queue->len <= pcb->mss) {
/* Remove TCP header from first segment. */
pbuf_header(queue->p, -TCP_HLEN);
pbuf_chain(useg->p, queue->p);
useg->len += queue->len;
useg->next = queue->next;
DEBUGF(TCP_OUTPUT_DEBUG, ("tcp_output: chaining, new len %u\n", useg->len));
if(seg == queue) {
seg = NULL;
}
memp_free(MEMP_TCP_SEG, queue);
} else {
if(useg == NULL) {
pcb->unsent = queue;
} else {
useg->next = queue;
}
}
if((flags & TCP_SYN) || (flags & TCP_FIN)) {
++len;
}
pcb->snd_lbb += len;
pcb->snd_buf -= len;
pcb->snd_queuelen = queuelen;
DEBUGF(TCP_QLEN_DEBUG, ("tcp_enqueue: %d (after enqueued)\n", pcb->snd_queuelen));
#ifdef LWIP_DEBUG
if(pcb->snd_queuelen != 0) {
ASSERT("tcp_enqueue: valid queue length", pcb->unacked != NULL ||
pcb->unsent != NULL);
}
#endif /* LWIP_DEBUG */
/* Set the PSH flag in the last segment that we enqueued, but only
if the segment has data (indicated by seglen > 0). */
if(seg != NULL && seglen > 0 && seg->tcphdr != NULL) {
TCPH_FLAGS_SET(seg->tcphdr, TCPH_FLAGS(seg->tcphdr) | TCP_PSH);
}
return ERR_OK;
memerr:
#ifdef TCP_STATS
++stats.tcp.memerr;
#endif /* TCP_STATS */
if(queue != NULL) {
tcp_segs_free(queue);
}
#ifdef LWIP_DEBUG
if(pcb->snd_queuelen != 0) {
ASSERT("tcp_enqueue: valid queue length", pcb->unacked != NULL ||
pcb->unsent != NULL);
}
#endif /* LWIP_DEBUG */
DEBUGF(TCP_QLEN_DEBUG, ("tcp_enqueue: %d (with mem err)\n", pcb->snd_queuelen));
return ERR_MEM;
}
/**
* Fragment an IP datagram if too large for the netif.
*
* Chop the datagram in MTU sized chunks and send them in order
* by using a fixed size static memory buffer (PBUF_ROM)
*/
err_t
ip_frag(struct pbuf *p, struct netif *netif, struct ip_addr *dest)
{
struct pbuf *rambuf;
struct pbuf *header;
struct ip_hdr *iphdr;
u16_t nfb = 0;
u16_t left, cop;
u16_t mtu = netif->mtu;
u16_t ofo, omf;
u16_t last;
u16_t poff = IP_HLEN;
u16_t tmp;
/* Get a RAM based MTU sized pbuf */
rambuf = pbuf_alloc(PBUF_LINK, 0, PBUF_REF);
if (rambuf == NULL) {
return ERR_MEM;
}
rambuf->tot_len = rambuf->len = mtu;
rambuf->payload = MEM_ALIGN((void *)buf);
/* Copy the IP header in it */
iphdr = rambuf->payload;
memcpy(iphdr, p->payload, IP_HLEN);
/* Save original offset */
tmp = ntohs(IPH_OFFSET(iphdr));
ofo = tmp & IP_OFFMASK;
omf = tmp & IP_MF;
left = p->tot_len - IP_HLEN;
while (left) {
last = (left <= mtu - IP_HLEN);
/* Set new offset and MF flag */
ofo += nfb;
tmp = omf | (IP_OFFMASK & (ofo));
if (!last)
tmp = tmp | IP_MF;
IPH_OFFSET_SET(iphdr, htons(tmp));
/* Fill this fragment */
nfb = (mtu - IP_HLEN) / 8;
cop = last ? left : nfb * 8;
p = copy_from_pbuf(p, &poff, (u8_t *) iphdr + IP_HLEN, cop);
/* Correct header */
IPH_LEN_SET(iphdr, htons(cop + IP_HLEN));
IPH_CHKSUM_SET(iphdr, 0);
IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, IP_HLEN));
if (last)
pbuf_realloc(rambuf, left + IP_HLEN);
/* This part is ugly: we alloc a RAM based pbuf for
* the link level header for each chunk and then
* free it.A PBUF_ROM style pbuf for which pbuf_header
* worked would make things simpler.
*/
header = pbuf_alloc(PBUF_LINK, 0, PBUF_RAM);
if (header != NULL) {
pbuf_chain(header, rambuf);
netif->output(netif, header, dest);
IPFRAG_STATS_INC(ip_frag.xmit);
pbuf_free(header);
} else {
pbuf_free(rambuf);
return ERR_MEM;
}
left -= cop;
}
pbuf_free(rambuf);
return ERR_OK;
}
/**
* Send the raw IP packet to the given address. Note that actually you cannot
* modify the IP headers (this is inconsistent with the receive callback where
* you actually get the IP headers), you can only specify the IP payload here.
* It requires some more changes in lwIP. (there will be a raw_send() function
* then.)
*
* @param pcb the raw pcb which to send
* @param p the IP payload to send
* @param ipaddr the destination address of the IP packet
*
*/
err_t
raw_sendto(struct raw_pcb *pcb, struct pbuf *p, struct ip_addr *ipaddr)
{
err_t err;
struct netif *netif;
struct ip_addr *src_ip;
struct pbuf *q; /* q will be sent down the stack */
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_TRACE, ("raw_sendto\n"));
/* not enough space to add an IP header to first pbuf in given p chain? */
if (pbuf_header(p, IP_HLEN)) {
/* allocate header in new pbuf */
q = pbuf_alloc(PBUF_IP, 0, PBUF_RAM);
/* new header pbuf could not be allocated? */
if (q == NULL) {
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS, ("raw_sendto: could not allocate header\n"));
return ERR_MEM;
}
/* chain header q in front of given pbuf p */
pbuf_chain(q, p);
/* { first pbuf q points to header pbuf } */
LWIP_DEBUGF(RAW_DEBUG, ("raw_sendto: added header pbuf %p before given pbuf %p\n", (void *)q, (void *)p));
} else {
/* first pbuf q equals given pbuf */
q = p;
if(pbuf_header(q, -IP_HLEN)) {
LWIP_ASSERT("Can't restore header we just removed!", 0);
return ERR_MEM;
}
}
if ((netif = ip_route(ipaddr)) == NULL) {
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_LEVEL_WARNING, ("raw_sendto: No route to 0x%"X32_F"\n", ipaddr->addr));
/* free any temporary header pbuf allocated by pbuf_header() */
if (q != p) {
pbuf_free(q);
}
return ERR_RTE;
}
#if IP_SOF_BROADCAST
/* broadcast filter? */
if ( ((pcb->so_options & SOF_BROADCAST) == 0) && ip_addr_isbroadcast(ipaddr, netif) ) {
LWIP_DEBUGF(RAW_DEBUG | LWIP_DBG_LEVEL_WARNING, ("raw_sendto: SOF_BROADCAST not enabled on pcb %p\n", (void *)pcb));
/* free any temporary header pbuf allocated by pbuf_header() */
if (q != p) {
pbuf_free(q);
}
return ERR_VAL;
}
#endif /* IP_SOF_BROADCAST */
if (ip_addr_isany(&pcb->local_ip)) {
/* use outgoing network interface IP address as source address */
src_ip = &(netif->ip_addr);
} else {
/* use RAW PCB local IP address as source address */
src_ip = &(pcb->local_ip);
}
#if LWIP_NETIF_HWADDRHINT
netif->addr_hint = &(pcb->addr_hint);
#endif /* LWIP_NETIF_HWADDRHINT*/
err = ip_output_if (q, src_ip, ipaddr, pcb->ttl, pcb->tos, pcb->protocol, netif);
#if LWIP_NETIF_HWADDRHINT
netif->addr_hint = NULL;
#endif /* LWIP_NETIF_HWADDRHINT*/
/* did we chain a header earlier? */
if (q != p) {
/* free the header */
pbuf_free(q);
}
return err;
}
/*-----------------------------------------------------------------------------------*/
err_t
phybusif_input(struct phybusif_cb *cb)
{
unsigned char c;
unsigned char n;
while((n = read(fd,&c,1))) {
switch(cb->state) {
case W4_PACKET_TYPE:
switch(c) {
case HCI_ACL_DATA_PACKET:
cb->state = W4_ACL_HDR;
break;
case HCI_EVENT_PACKET:
cb->state = W4_EVENT_HDR;
break;
default:
LWIP_DEBUGF(PHYBUSIF_DEBUG, ("phybusif_input: Unknown packet type\n"));
break;
}
break;
case W4_EVENT_HDR:
((u8_t *)cb->q->payload)[cb->recvd] = c;
cb->tot_recvd++;
cb->recvd++;
if(cb->recvd == HCI_EVENT_HDR_LEN) {
cb->evhdr = cb->p->payload;
pbuf_header(cb->p, -HCI_EVENT_HDR_LEN);
cb->recvd = cb->tot_recvd = 0;
if(cb->evhdr->len > 0) {
cb->state = W4_EVENT_PARAM;
} else {
hci_event_input(cb->p); /* Handle incoming event */
pbuf_free(cb->p);
phybusif_reset(cb);
return ERR_OK; /* Since there most likley won't be any more data in the input buffer */
}
}
break;
case W4_EVENT_PARAM:
((u8_t *)cb->q->payload)[cb->recvd] = c;
cb->tot_recvd++;
cb->recvd++;
if(cb->recvd == cb->q->len) { /* Pbuf full. alloc and add new tail to chain */
cb->recvd = 0;
if((cb->q = pbuf_alloc(PBUF_RAW, PBUF_POOL_BUFSIZE, PBUF_POOL)) == NULL) {
LWIP_DEBUGF(PHYBUSIF_DEBUG, ("phybusif_input: Could not allocate memory for event parameter pbuf\n"));
return ERR_MEM; /* Could not allocate memory for pbuf */
}
pbuf_chain(cb->p, cb->q);
pbuf_free(cb->q);
}
if(cb->tot_recvd == cb->evhdr->len) {
hci_event_input(cb->p); /* Handle incoming event */
pbuf_free(cb->p);
phybusif_reset(cb);
return ERR_OK; /* Since there most likley won't be any more data in the input buffer */
}
break;
case W4_ACL_HDR:
((u8_t *)cb->q->payload)[cb->recvd] = c;
cb->tot_recvd++;
cb->recvd++;
if(cb->recvd == HCI_ACL_HDR_LEN) {
cb->aclhdr = cb->p->payload;
pbuf_header(cb->p, -HCI_ACL_HDR_LEN);
cb->recvd = cb->tot_recvd = 0;
if(cb->aclhdr->len > 0) {
cb->state = W4_ACL_DATA;
} else {
LWIP_DEBUGF(PHYBUSIF_DEBUG, ("phybusif_reset: Forward Empty ACL packet to higher layer\n"));
hci_acl_input(cb->p); /* Handle incoming ACL data */
phybusif_reset(cb);
return ERR_OK; /* Since there most likley won't be any more data in the input buffer */
}
}
break;
case W4_ACL_DATA:
((u8_t *)cb->q->payload)[cb->recvd] = c;
cb->tot_recvd++;
cb->recvd++;
if(cb->recvd == cb->q->len) { /* Pbuf full. alloc and add new tail to chain */
cb->recvd = 0;
if((cb->q = pbuf_alloc(PBUF_RAW, PBUF_POOL_BUFSIZE, PBUF_POOL)) == NULL) {
LWIP_DEBUGF(PHYBUSIF_DEBUG, ("phybusif_input: Could not allocate memory for ACL data pbuf\n"));
return ERR_MEM; /* Could not allocate memory for pbuf */
}
pbuf_chain(cb->p, cb->q);
pbuf_free(cb->q);
}
if(cb->tot_recvd == cb->aclhdr->len) {
LWIP_DEBUGF(PHYBUSIF_DEBUG, ("phybusif_input: Forward ACL packet to higher layer\n"));
hci_acl_input(cb->p); /* Handle incoming ACL data */
phybusif_reset(cb);
return ERR_OK; /* Since there most likley won't be any more data in the input buffer */
}
break;
default:
LWIP_DEBUGF(PHYBUSIF_DEBUG, ("phybusif_input: Unknown state\n\n"));
break;
}
}
return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
void
udp_input(struct pbuf *p, struct netif *inp)
{
struct udp_hdr *udphdr;
struct udp_pcb *pcb;
struct ip_hdr *iphdr;
uint16_t src, dest;
#ifdef UDP_STATS
++stats.udp.recv;
#endif /* UDP_STATS */
iphdr = (struct ip_hdr *)p->payload;
pbuf_header(p, -(UDP_HLEN + IPH_HL(iphdr) * 4));
udphdr = (struct udp_hdr *)((uint8_t *)p->payload - UDP_HLEN);
DEBUGF(UDP_DEBUG, ("udp_input: received datagram of length %d\n", p->tot_len));
src = NTOHS(udphdr->src);
dest = NTOHS(udphdr->dest);
#if UDP_DEBUG
udp_debug_print(udphdr);
#endif /* UDP_DEBUG */
/* Demultiplex packet. First, go for a perfect match. */
for(pcb = udp_pcbs; pcb != NULL; pcb = pcb->next) {
DEBUGF(UDP_DEBUG, ("udp_input: pcb local port %d (dgram %d)\n",
pcb->local_port, ntohs(udphdr->dest)));
if(pcb->remote_port == src &&
pcb->local_port == dest &&
(ip_addr_isany(&pcb->remote_ip) ||
ip_addr_cmp(&(pcb->remote_ip), &(iphdr->src))) &&
(ip_addr_isany(&pcb->local_ip) ||
ip_addr_cmp(&(pcb->local_ip), &(iphdr->dest)))) {
break;
}
}
if(pcb == NULL) {
for(pcb = udp_pcbs; pcb != NULL; pcb = pcb->next) {
DEBUGF(UDP_DEBUG, ("udp_input: pcb local port %d (dgram %d)\n",
pcb->local_port, dest));
if(pcb->local_port == dest &&
(ip_addr_isany(&pcb->remote_ip) ||
ip_addr_cmp(&(pcb->remote_ip), &(iphdr->src))) &&
(ip_addr_isany(&pcb->local_ip) ||
ip_addr_cmp(&(pcb->local_ip), &(iphdr->dest)))) {
break;
}
}
}
/* Check checksum if this is a match or if it was directed at us. */
/* if(pcb != NULL ||
ip_addr_cmp(&inp->ip_addr, &iphdr->dest)) {*/
if(pcb != NULL) {
DEBUGF(UDP_DEBUG, ("udp_input: calculating checksum\n"));
pbuf_header(p, UDP_HLEN);
#ifdef IPv6
if(iphdr->nexthdr == IP_PROTO_UDPLITE) {
#else
if(IPH_PROTO(iphdr) == IP_PROTO_UDPLITE) {
#endif /* IPv4 */
/* Do the UDP Lite checksum */
if(inet_chksum_pseudo(p, (struct ip_addr *)&(iphdr->src),
(struct ip_addr *)&(iphdr->dest),
IP_PROTO_UDPLITE, ntohs(udphdr->len)) != 0) {
DEBUGF(UDP_DEBUG, ("udp_input: UDP Lite datagram discarded due to failing checksum\n"));
#ifdef UDP_STATS
++stats.udp.chkerr;
++stats.udp.drop;
#endif /* UDP_STATS */
pbuf_free(p);
goto end;
}
} else {
if(udphdr->chksum != 0) {
if(inet_chksum_pseudo(p, (struct ip_addr *)&(iphdr->src),
(struct ip_addr *)&(iphdr->dest),
IP_PROTO_UDP, p->tot_len) != 0) {
DEBUGF(UDP_DEBUG, ("udp_input: UDP datagram discarded due to failing checksum\n"));
#ifdef UDP_STATS
++stats.udp.chkerr;
++stats.udp.drop;
#endif /* UDP_STATS */
pbuf_free(p);
goto end;
}
}
}
pbuf_header(p, -UDP_HLEN);
if(pcb != NULL) {
pcb->recv(pcb->recv_arg, pcb, p, &(iphdr->src), src);
} else {
DEBUGF(UDP_DEBUG, ("udp_input: not for us.\n"));
/* No match was found, send ICMP destination port unreachable unless
destination address was broadcast/multicast. */
if(!ip_addr_isbroadcast(&iphdr->dest, &inp->netmask) &&
!ip_addr_ismulticast(&iphdr->dest)) {
/* deconvert from host to network byte order */
udphdr->src = htons(udphdr->src);
udphdr->dest = htons(udphdr->dest);
/* adjust pbuf pointer */
p->payload = iphdr;
icmp_dest_unreach(p, ICMP_DUR_PORT);
}
#ifdef UDP_STATS
++stats.udp.proterr;
++stats.udp.drop;
#endif /* UDP_STATS */
pbuf_free(p);
}
} else {
pbuf_free(p);
}
end:
while(0); /* hack to remove compiler warning */
}
/*-----------------------------------------------------------------------------------*/
err_t
udp_send(struct udp_pcb *pcb, struct pbuf *p)
{
struct udp_hdr *udphdr;
struct ip_addr *src_ip;
err_t err;
struct pbuf *q;
if(pbuf_header(p, UDP_HLEN)) {
q = pbuf_alloc(PBUF_IP, UDP_HLEN, PBUF_RAM);
if(q == NULL) {
return ERR_MEM;
}
pbuf_chain(q, p);
p = q;
}
udphdr = (struct udp_hdr *)p->payload;
udphdr->src = htons(pcb->local_port);
udphdr->dest = htons(pcb->remote_port);
udphdr->chksum = 0x0000;
src_ip = &(pcb->local_ip);
DEBUGF(UDP_DEBUG, ("udp_send: sending datagram of length %d\n", p->tot_len));
if(pcb->flags & UDP_FLAGS_UDPLITE) {
udphdr->len = htons(pcb->chksum_len);
/* calculate checksum */
udphdr->chksum = inet_chksum_pseudo(p, src_ip, &(pcb->remote_ip),
IP_PROTO_UDP, pcb->chksum_len);
if(udphdr->chksum == 0x0000) {
udphdr->chksum = 0xffff;
}
err = sr_lwip_output(p, &pcb->local_ip, &pcb->remote_ip, IP_PROTO_UDPLITE);
} else {
udphdr->len = htons(p->tot_len);
/* calculate checksum */
if((pcb->flags & UDP_FLAGS_NOCHKSUM) == 0) {
udphdr->chksum = inet_chksum_pseudo(p, src_ip, &pcb->remote_ip,
IP_PROTO_UDP, p->tot_len);
if(udphdr->chksum == 0x0000) {
udphdr->chksum = 0xffff;
}
}
err = sr_lwip_output(p,&pcb->local_ip, &pcb->remote_ip, IP_PROTO_UDP);
}
#ifdef UDP_STATS
++stats.udp.xmit;
#endif /* UDP_STATS */
return err;
}
/*-----------------------------------------------------------------------------------*/
err_t
udp_bind(struct udp_pcb *pcb, struct ip_addr *ipaddr, uint16_t port)
{
struct udp_pcb *ipcb;
ip_addr_set(&pcb->local_ip, ipaddr);
pcb->local_port = port;
/* Insert UDP PCB into the list of active UDP PCBs. */
for(ipcb = udp_pcbs; ipcb != NULL; ipcb = ipcb->next) {
if(pcb == ipcb) {
/* Already on the list, just return. */
return ERR_OK;
}
}
/* We need to place the PCB on the list. */
pcb->next = udp_pcbs;
udp_pcbs = pcb;
DEBUGF(UDP_DEBUG, ("udp_bind: bound to port %d\n", port));
return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
err_t
udp_connect(struct udp_pcb *pcb, struct ip_addr *ipaddr, uint16_t port)
{
struct udp_pcb *ipcb;
ip_addr_set(&pcb->remote_ip, ipaddr);
pcb->remote_port = port;
/* Insert UDP PCB into the list of active UDP PCBs. */
for(ipcb = udp_pcbs; ipcb != NULL; ipcb = ipcb->next) {
if(pcb == ipcb) {
/* Already on the list, just return. */
return ERR_OK;
}
}
/* We need to place the PCB on the list. */
pcb->next = udp_pcbs;
udp_pcbs = pcb;
return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
void
udp_recv(struct udp_pcb *pcb,
void (* recv)(void *arg, struct udp_pcb *upcb, struct pbuf *p,
struct ip_addr *addr, uint16_t port),
void *recv_arg)
{
pcb->recv = recv;
pcb->recv_arg = recv_arg;
}
static err_t mg_lwip_tcp_recv_cb(void *arg, struct tcp_pcb *tpcb,
struct pbuf *p, err_t err) {
struct mg_connection *nc = (struct mg_connection *) arg;
DBG(("%p %p %u %d", nc, tpcb, (p != NULL ? p->tot_len : 0), err));
if (p == NULL) {
if (nc != NULL) {
mg_lwip_post_signal(MG_SIG_CLOSE_CONN, nc);
} else {
/* Tombstoned connection, do nothing. */
}
return ERR_OK;
} else if (nc == NULL) {
tcp_abort(tpcb);
return ERR_ARG;
}
struct mg_lwip_conn_state *cs = (struct mg_lwip_conn_state *) nc->sock;
/*
* If we get a chain of more than one segment at once, we need to bump
* refcount on the subsequent bufs to make them independent.
*/
if (p->next != NULL) {
struct pbuf *q = p->next;
for (; q != NULL; q = q->next) pbuf_ref(q);
}
if (cs->rx_chain == NULL) {
cs->rx_chain = p;
cs->rx_offset = 0;
} else {
if (pbuf_clen(cs->rx_chain) >= 4) {
/* ESP SDK has a limited pool of 5 pbufs. We must not hog them all or RX
* will be completely blocked. We already have at least 4 in the chain,
* this one is, so we have to make a copy and release this one. */
struct pbuf *np = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
if (np != NULL) {
pbuf_copy(np, p);
pbuf_free(p);
p = np;
}
}
pbuf_chain(cs->rx_chain, p);
}
#ifdef SSL_KRYPTON
if (nc->ssl != NULL) {
if (nc->flags & MG_F_SSL_HANDSHAKE_DONE) {
mg_lwip_ssl_recv(nc);
} else {
mg_lwip_ssl_do_hs(nc);
}
return ERR_OK;
}
#endif
while (cs->rx_chain != NULL) {
struct pbuf *seg = cs->rx_chain;
size_t len = (seg->len - cs->rx_offset);
char *data = (char *) malloc(len);
if (data == NULL) {
DBG(("OOM"));
return ERR_MEM;
}
pbuf_copy_partial(seg, data, len, cs->rx_offset);
mg_if_recv_tcp_cb(nc, data, len); /* callee takes over data */
cs->rx_offset += len;
if (cs->rx_offset == cs->rx_chain->len) {
cs->rx_chain = pbuf_dechain(cs->rx_chain);
pbuf_free(seg);
cs->rx_offset = 0;
}
}
if (nc->send_mbuf.len > 0) {
mg_lwip_mgr_schedule_poll(nc->mgr);
}
return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
static void
mcf5272fec_rx(void)
{
/* This is the receive ISR. It is written to be a high-level ISR. */
u32_t old_level;
mcf5272if_t *mcf5272 = mcf5272if;
MCF5272_IMM *imm = mcf5272->imm;
u32_t value;
u16_t flags;
unsigned int rx_remove_sof;
unsigned int rx_remove_eof;
struct pbuf *p;
rx_remove_sof = rx_remove_eof = mcf5272->rx_remove;
/* Loop, looking for filled buffers at eof */
while ((((flags = mcf5272->rxbd_a[rx_remove_eof].flags) & MCF5272_FEC_RX_BD_E) == 0) &&
(mcf5272->rx_pbuf_a[rx_remove_eof] != 0))
{
/* See if this is last buffer in frame */
if ((flags & MCF5272_FEC_RX_BD_L) != 0)
{
/* This frame is ready to go. Start at first descriptor in frame. */
p = 0;
do
{
/* Adjust pbuf length if this is last buffer in frame */
if (rx_remove_sof == rx_remove_eof)
{
mcf5272->rx_pbuf_a[rx_remove_sof]->tot_len =
mcf5272->rx_pbuf_a[rx_remove_sof]->len = (u16_t)
(mcf5272->rxbd_a[rx_remove_sof].data_len - (p ? p->tot_len : 0));
}
else
mcf5272->rx_pbuf_a[rx_remove_sof]->len =
mcf5272->rx_pbuf_a[rx_remove_sof]->tot_len = mcf5272->rxbd_a[rx_remove_sof].data_len;
/* Chain pbuf */
if (p == 0)
{
p = mcf5272->rx_pbuf_a[rx_remove_sof]; // First in chain
p->tot_len = p->len; // Important since len might have changed
} else {
pbuf_chain(p, mcf5272->rx_pbuf_a[rx_remove_sof]);
pbuf_free(mcf5272->rx_pbuf_a[rx_remove_sof]);
}
/* Clear pointer to mark descriptor as free */
mcf5272->rx_pbuf_a[rx_remove_sof] = 0;
mcf5272->rxbd_a[rx_remove_sof].p_buf = 0;
if (rx_remove_sof != rx_remove_eof)
INC_RX_BD_INDEX(rx_remove_sof);
else
break;
} while (1);
INC_RX_BD_INDEX(rx_remove_sof);
/* Check error status of frame */
if (flags & (MCF5272_FEC_RX_BD_LG |
MCF5272_FEC_RX_BD_NO |
MCF5272_FEC_RX_BD_CR |
MCF5272_FEC_RX_BD_OV))
{
#ifdef LINK_STATS
lwip_stats.link.drop++;
if (flags & MCF5272_FEC_RX_BD_LG)
lwip_stats.link.lenerr++; //Jumbo gram
else
if (flags & (MCF5272_FEC_RX_BD_NO | MCF5272_FEC_RX_BD_OV))
lwip_stats.link.err++;
else
if (flags & MCF5272_FEC_RX_BD_CR)
lwip_stats.link.chkerr++; // CRC errors
#endif
/* Drop errored frame */
pbuf_free(p);
} else {
/* Good frame. increment stat */
#ifdef LINK_STATS
lwip_stats.link.recv++;
#endif
eth_input(p, mcf5272->netif);
}
}
INC_RX_BD_INDEX(rx_remove_eof);
}
mcf5272->rx_remove = rx_remove_sof;
/* clear interrupt status for rx interrupt */
old_level = sys_arch_protect();
MCF5272_WR_FEC_EIR(imm, MCF5272_FEC_EIR_RXF);
value = MCF5272_RD_FEC_IMR(imm);
/* Set rx interrupt bit again */
MCF5272_WR_FEC_IMR(imm, (value | MCF5272_FEC_IMR_RXFEN));
/* Now we can re-enable higher priority interrupts again */
sys_arch_unprotect(old_level);
/* Fill up empty descriptor rings */
fill_rx_ring(mcf5272);
/* Tell fec that we have filled up her ring */
MCF5272_WR_FEC_RDAR(imm, 1);
return;
}
/**
* Fragment an IP datagram if too large for the netif.
*
* Chop the datagram in MTU sized chunks and send them in order
* by using a fixed size static memory buffer (PBUF_REF) or
* point PBUF_REFs into p (depending on IP_FRAG_USES_STATIC_BUF).
*
* @param p ip packet to send
* @param netif the netif on which to send
* @param dest destination ip address to which to send
*
* @return ERR_OK if sent successfully, err_t otherwise
*/
err_t
ip_frag(struct pbuf *p, struct netif *netif, ip_addr_t *dest)
{
struct pbuf *rambuf;
#if IP_FRAG_USES_STATIC_BUF
struct pbuf *header;
#else
#if !LWIP_NETIF_TX_SINGLE_PBUF
struct pbuf *newpbuf;
#endif
struct ip_hdr *original_iphdr;
#endif
struct ip_hdr *iphdr;
u16_t nfb;
u16_t left, cop;
u16_t mtu = netif->mtu;
u16_t ofo, omf;
u16_t last;
u16_t poff = IP_HLEN;
u16_t tmp;
#if !IP_FRAG_USES_STATIC_BUF && !LWIP_NETIF_TX_SINGLE_PBUF
u16_t newpbuflen = 0;
u16_t left_to_copy;
#endif
/* Get a RAM based MTU sized pbuf */
#if IP_FRAG_USES_STATIC_BUF
/* When using a static buffer, we use a PBUF_REF, which we will
* use to reference the packet (without link header).
* Layer and length is irrelevant.
*/
rambuf = pbuf_alloc(PBUF_LINK, 0, PBUF_REF);
if (rambuf == NULL) {
LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_frag: pbuf_alloc(PBUF_LINK, 0, PBUF_REF) failed\n"));
return ERR_MEM;
}
rambuf->tot_len = rambuf->len = mtu;
rambuf->payload = LWIP_MEM_ALIGN((void *)buf);
/* Copy the IP header in it */
iphdr = (struct ip_hdr *)rambuf->payload;
SMEMCPY(iphdr, p->payload, IP_HLEN);
#else /* IP_FRAG_USES_STATIC_BUF */
original_iphdr = (struct ip_hdr *)p->payload;
iphdr = original_iphdr;
#endif /* IP_FRAG_USES_STATIC_BUF */
/* Save original offset */
tmp = ntohs(IPH_OFFSET(iphdr));
ofo = tmp & IP_OFFMASK;
omf = tmp & IP_MF;
left = p->tot_len - IP_HLEN;
nfb = (mtu - IP_HLEN) / 8;
while (left) {
last = (left <= mtu - IP_HLEN);
/* Set new offset and MF flag */
tmp = omf | (IP_OFFMASK & (ofo));
if (!last) {
tmp = tmp | IP_MF;
}
/* Fill this fragment */
cop = last ? left : nfb * 8;
#if IP_FRAG_USES_STATIC_BUF
poff += pbuf_copy_partial(p, (u8_t*)iphdr + IP_HLEN, cop, poff);
#else /* IP_FRAG_USES_STATIC_BUF */
#if LWIP_NETIF_TX_SINGLE_PBUF
rambuf = pbuf_alloc(PBUF_IP, cop, PBUF_RAM);
if (rambuf == NULL) {
return ERR_MEM;
}
LWIP_ASSERT("this needs a pbuf in one piece!",
(rambuf->len == rambuf->tot_len) && (rambuf->next == NULL));
poff += pbuf_copy_partial(p, rambuf->payload, cop, poff);
/* make room for the IP header */
if(pbuf_header(rambuf, IP_HLEN)) {
pbuf_free(rambuf);
return ERR_MEM;
}
/* fill in the IP header */
SMEMCPY(rambuf->payload, original_iphdr, IP_HLEN);
iphdr = rambuf->payload;
#else /* LWIP_NETIF_TX_SINGLE_PBUF */
/* When not using a static buffer, create a chain of pbufs.
* The first will be a PBUF_RAM holding the link and IP header.
* The rest will be PBUF_REFs mirroring the pbuf chain to be fragged,
* but limited to the size of an mtu.
*/
rambuf = pbuf_alloc(PBUF_LINK, IP_HLEN, PBUF_RAM);
if (rambuf == NULL) {
return ERR_MEM;
}
LWIP_ASSERT("this needs a pbuf in one piece!",
(p->len >= (IP_HLEN)));
SMEMCPY(rambuf->payload, original_iphdr, IP_HLEN);
iphdr = (struct ip_hdr *)rambuf->payload;
/* Can just adjust p directly for needed offset. */
p->payload = (u8_t *)p->payload + poff;
p->len -= poff;
left_to_copy = cop;
while (left_to_copy) {
struct pbuf_custom_ref *pcr;
newpbuflen = (left_to_copy < p->len) ? left_to_copy : p->len;
/* Is this pbuf already empty? */
if (!newpbuflen) {
p = p->next;
continue;
}
pcr = ip_frag_alloc_pbuf_custom_ref();
if (pcr == NULL) {
pbuf_free(rambuf);
return ERR_MEM;
}
/* Mirror this pbuf, although we might not need all of it. */
newpbuf = pbuf_alloced_custom(PBUF_RAW, newpbuflen, PBUF_REF, &pcr->pc, p->payload, newpbuflen);
if (newpbuf == NULL) {
ip_frag_free_pbuf_custom_ref(pcr);
pbuf_free(rambuf);
return ERR_MEM;
}
pbuf_ref(p);
pcr->original = p;
pcr->pc.custom_free_function = ipfrag_free_pbuf_custom;
/* Add it to end of rambuf's chain, but using pbuf_cat, not pbuf_chain
* so that it is removed when pbuf_dechain is later called on rambuf.
*/
pbuf_cat(rambuf, newpbuf);
left_to_copy -= newpbuflen;
if (left_to_copy) {
p = p->next;
}
}
poff = newpbuflen;
#endif /* LWIP_NETIF_TX_SINGLE_PBUF */
#endif /* IP_FRAG_USES_STATIC_BUF */
/* Correct header */
IPH_OFFSET_SET(iphdr, htons(tmp));
IPH_LEN_SET(iphdr, htons(cop + IP_HLEN));
IPH_CHKSUM_SET(iphdr, 0);
IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, IP_HLEN));
#if IP_FRAG_USES_STATIC_BUF
if (last) {
pbuf_realloc(rambuf, left + IP_HLEN);
}
/* This part is ugly: we alloc a RAM based pbuf for
* the link level header for each chunk and then
* free it.A PBUF_ROM style pbuf for which pbuf_header
* worked would make things simpler.
*/
header = pbuf_alloc(PBUF_LINK, 0, PBUF_RAM);
if (header != NULL) {
pbuf_chain(header, rambuf);
netif->output(netif, header, dest);
IPFRAG_STATS_INC(ip_frag.xmit);
snmp_inc_ipfragcreates();
pbuf_free(header);
} else {
LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_frag: pbuf_alloc() for header failed\n"));
pbuf_free(rambuf);
return ERR_MEM;
}
#else /* IP_FRAG_USES_STATIC_BUF */
/* No need for separate header pbuf - we allowed room for it in rambuf
* when allocated.
*/
netif->output(netif, rambuf, dest);
IPFRAG_STATS_INC(ip_frag.xmit);
/* Unfortunately we can't reuse rambuf - the hardware may still be
* using the buffer. Instead we free it (and the ensuing chain) and
* recreate it next time round the loop. If we're lucky the hardware
* will have already sent the packet, the free will really free, and
* there will be zero memory penalty.
*/
pbuf_free(rambuf);
#endif /* IP_FRAG_USES_STATIC_BUF */
left -= cop;
ofo += nfb;
}
#if IP_FRAG_USES_STATIC_BUF
pbuf_free(rambuf);
#endif /* IP_FRAG_USES_STATIC_BUF */
snmp_inc_ipfragoks();
return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
static err_t
tapif_output(struct netif *netif, struct pbuf *p,
struct ip_addr *ipaddr)
{
struct tapif *tapif;
struct pbuf *q;
struct eth_hdr *ethhdr;
struct eth_addr *dest, mcastaddr;
struct ip_addr *queryaddr;
err_t err;
u8_t i;
tapif = netif->state;
/* Make room for Ethernet header. */
if(pbuf_header(p, sizeof(struct eth_hdr)) != 0) {
/* The pbuf_header() call shouldn't fail, but we allocate an extra
pbuf just in case. */
q = pbuf_alloc(PBUF_LINK, sizeof(struct eth_hdr), PBUF_RAM);
if(q == NULL) {
return ERR_MEM;
}
pbuf_chain(q, p);
p = q;
}
/* Construct Ethernet header. Start with looking up deciding which
MAC address to use as a destination address. Broadcasts and
multicasts are special, all other addresses are looked up in the
ARP table. */
queryaddr = ipaddr;
if(ip_addr_isany(ipaddr) ||
ip_addr_isbroadcast(ipaddr, &(netif->netmask))) {
dest = (struct eth_addr *)ðbroadcast;
} else if(ip_addr_ismulticast(ipaddr)) {
/* Hash IP multicast address to MAC address. */
mcastaddr.addr[0] = 0x01;
mcastaddr.addr[1] = 0x0;
mcastaddr.addr[2] = 0x5e;
mcastaddr.addr[3] = ip4_addr2(ipaddr) & 0x7f;
mcastaddr.addr[4] = ip4_addr3(ipaddr);
mcastaddr.addr[5] = ip4_addr4(ipaddr);
dest = &mcastaddr;
} else {
if(ip_addr_maskcmp(ipaddr, &(netif->ip_addr), &(netif->netmask))) {
/* Use destination IP address if the destination is on the same
subnet as we are. */
queryaddr = ipaddr;
} else {
/* Otherwise we use the default router as the address to send
the Ethernet frame to. */
queryaddr = &(netif->gw);
}
dest = arp_lookup(queryaddr);
}
/* If the arp_lookup() didn't find an address, we send out an ARP
query for the IP address. */
if(dest == NULL) {
q = arp_query(netif, tapif->ethaddr, queryaddr);
if(q != NULL) {
printf("Sending ARP after query\n");
err = low_level_output(tapif, q);
pbuf_free(q);
return err;
}
return ERR_MEM;
}
ethhdr = p->payload;
for(i = 0; i < 6; i++) {
ethhdr->dest.addr[i] = dest->addr[i];
ethhdr->src.addr[i] = tapif->ethaddr->addr[i];
}
ethhdr->type = htons(ETHTYPE_IP);
return low_level_output(tapif, p);
}
