err_t process_packet_FPV_tcp_recv(struct tcp_pcb * pcb, struct pbuf * pbuf,
err_t err) {
struct pbuf * p = pbuf;
if (p == NULL) {
return close_conn(pcb, CONNCLOSED_USER, ERR_OK);
}
while (p) {
int data_left = p->len;
uint8_t *data_ptr = p->payload;
int fsar = 0;
if (ps_buffered) {
/* There's some leftover data from the last pbuf that
* we processed, so copy it in first. */
int more = PS_BUFFER_SIZE - ps_buffered;
if (more > data_left)
more = data_left;
ASSERT_NOT_EQUAL(more, 0);
memcpy(ps_buffer + ps_buffered, data_ptr, more);
fsar = recv_fsm(pcb, ps_buffer, ps_buffered + more);
/* We still may not yet have enough. This should only
* happen becuase we're out of data in this pbuf, not
* because we couldn't fit it all in ps_buffer. */
if (fsar == 0) {
ASSERT_EQUAL(data_left, more);
data_ptr += more;
data_left -= more;
ps_buffered += more;
} else if (fsar < 0) {
break;
} else {
/* Now, depending on the command, we may not
* have needed *all* PS_BUFFER_SIZE bytes. The
* *next* command, however, should start in
* the pbuf - if not, something's wrong, since
* we should have processed the previous
* command before now. */
ASSERT(fsar > ps_buffered);
/* Consume only what we actually used from the
* buffer, so that the next recv_fsm call can
* point directly at the buffer. */
data_ptr += (fsar - ps_buffered);
data_left -= (fsar - ps_buffered);
ps_buffered = 0;
}
}
/* At this point, there should be no more buffered data. */
if (data_left)
ASSERT_EQUAL(ps_buffered, 0);
/* Now that we've dealt with playing out anything that was
* buffered with a previous pbuf, let's deal with this one. */
while (data_left) {
fsar = recv_fsm(pcb, data_ptr, data_left);
if (fsar == 0) {
/* There isn't enough. */
ASSERT(data_left < PS_BUFFER_SIZE);
memcpy(ps_buffer, data_ptr, data_left);
ps_buffered = data_left;
data_left = 0;
} else if (fsar < 0) {
break;
} else {
data_ptr += fsar;
data_left -= fsar;
}
}
if (fsar < 0)
break;
/* Move on to the next pbuf. */
p = p->next;
}
ps_send_deferred_acks(pcb);
/* Tell lwIP we're done with this packet. */
tcp_recved(pcb, pbuf->tot_len);
pbuf_free(pbuf);
return ERR_OK;
}
static err_t rfb_recv(void *arg, struct tcp_pcb *pcb,
struct pbuf *p, err_t err) {
struct rfb_state *state = arg;
uint16_t copylen;
if (state == NULL)
if (err != ERR_OK) {
outputf("RFB: recv err %d", err);
/* FIXME do something better here? */
return ERR_OK;
}
if (p == NULL) {
outputf("RFB: Connection closed");
close_conn(pcb, state);
return ERR_OK;
}
if (p->tot_len > (RFB_BUF_SIZE - state->writepos)) {
/* Overflow! */
outputf("RFB: Overflow!");
close_conn(pcb, state);
return ERR_OK;
}
copylen = pbuf_copy_partial(p, state->data + state->writepos, p->tot_len, 0);
outputf("RFB: Processing %d, wp %d, cp %d", p->tot_len, state->writepos, copylen);
state->writepos += p->tot_len;
tcp_recved(pcb, p->tot_len);
pbuf_free(p);
while (1) {
switch (recv_fsm(pcb, state)) {
case NEEDMORE:
outputf("RFB FSM: blocking");
goto doneprocessing;
case OK:
if (state->readpos == state->writepos) {
state->readpos = 0;
state->writepos = 0;
goto doneprocessing;
} else {
memmove(state->data,
state->data + state->readpos,
state->writepos - state->readpos);
state->writepos -= state->readpos;
state->readpos = 0;
}
break;
case FAIL:
/* Shit */
outputf("RFB: Protocol error");
close_conn(pcb, state);
return ERR_OK;
}
}
doneprocessing:
/* Kick off a send. */
if (state->send_state == SST_IDLE && state->update_requested) {
send_fsm(pcb, state);
}
return ERR_OK;
}
