// Helper function for recv/recvfrom to handle UDP packets
STATIC mp_uint_t lwip_udp_receive(lwip_socket_obj_t *socket, byte *buf, mp_uint_t len, byte *ip, mp_uint_t *port, int *_errno) {
if (socket->incoming.pbuf == NULL) {
if (socket->timeout != -1) {
for (mp_uint_t retries = socket->timeout / 100; retries--;) {
mp_hal_delay_ms(100);
if (socket->incoming.pbuf != NULL) break;
}
if (socket->incoming.pbuf == NULL) {
*_errno = MP_ETIMEDOUT;
return -1;
}
} else {
while (socket->incoming.pbuf == NULL) {
poll_sockets();
}
}
}
if (ip != NULL) {
memcpy(ip, &socket->peer, sizeof(socket->peer));
*port = socket->peer_port;
}
struct pbuf *p = socket->incoming.pbuf;
u16_t result = pbuf_copy_partial(p, buf, ((p->tot_len > len) ? len : p->tot_len), 0);
pbuf_free(p);
socket->incoming.pbuf = NULL;
return (mp_uint_t) result;
}
// Helper function for recv/recvfrom to handle TCP packets
STATIC mp_uint_t lwip_tcp_receive(lwip_socket_obj_t *socket, byte *buf, mp_uint_t len, int *_errno) {
// Check for any pending errors
STREAM_ERROR_CHECK(socket);
if (socket->incoming.pbuf == NULL) {
// Non-blocking socket
if (socket->timeout == 0) {
if (socket->state == STATE_PEER_CLOSED) {
return 0;
}
*_errno = MP_EAGAIN;
return -1;
}
mp_uint_t start = mp_hal_ticks_ms();
while (socket->state == STATE_CONNECTED && socket->incoming.pbuf == NULL) {
if (socket->timeout != -1 && mp_hal_ticks_ms() - start > socket->timeout) {
*_errno = MP_ETIMEDOUT;
return -1;
}
poll_sockets();
}
if (socket->state == STATE_PEER_CLOSED) {
if (socket->incoming.pbuf == NULL) {
// socket closed and no data left in buffer
return 0;
}
} else if (socket->state != STATE_CONNECTED) {
assert(socket->state < 0);
*_errno = error_lookup_table[-socket->state];
return -1;
}
}
assert(socket->pcb.tcp != NULL);
struct pbuf *p = socket->incoming.pbuf;
if (socket->leftover_count == 0) {
socket->leftover_count = p->tot_len;
}
u16_t result = pbuf_copy_partial(p, buf, ((socket->leftover_count >= len) ? len : socket->leftover_count), (p->tot_len - socket->leftover_count));
if (socket->leftover_count > len) {
// More left over...
socket->leftover_count -= len;
} else {
pbuf_free(p);
socket->incoming.pbuf = NULL;
socket->leftover_count = 0;
}
tcp_recved(socket->pcb.tcp, result);
return (mp_uint_t) result;
}
// Helper function for send/sendto to handle TCP packets
STATIC mp_uint_t lwip_tcp_send(lwip_socket_obj_t *socket, const byte *buf, mp_uint_t len, int *_errno) {
// Check for any pending errors
STREAM_ERROR_CHECK(socket);
u16_t available = tcp_sndbuf(socket->pcb.tcp);
if (available == 0) {
// Non-blocking socket
if (socket->timeout == 0) {
*_errno = MP_EAGAIN;
return MP_STREAM_ERROR;
}
mp_uint_t start = mp_hal_ticks_ms();
// Assume that STATE_PEER_CLOSED may mean half-closed connection, where peer closed it
// sending direction, but not receiving. Consequently, check for both STATE_CONNECTED
// and STATE_PEER_CLOSED as normal conditions and still waiting for buffers to be sent.
// If peer fully closed socket, we would have socket->state set to ERR_RST (connection
// reset) by error callback.
// Avoid sending too small packets, so wait until at least 16 bytes available
while (socket->state >= STATE_CONNECTED && (available = tcp_sndbuf(socket->pcb.tcp)) < 16) {
if (socket->timeout != -1 && mp_hal_ticks_ms() - start > socket->timeout) {
*_errno = MP_ETIMEDOUT;
return MP_STREAM_ERROR;
}
poll_sockets();
}
// While we waited, something could happen
STREAM_ERROR_CHECK(socket);
}
u16_t write_len = MIN(available, len);
err_t err = tcp_write(socket->pcb.tcp, buf, write_len, TCP_WRITE_FLAG_COPY);
// If the output buffer is getting full then send the data to the lower layers
if (err == ERR_OK && tcp_sndbuf(socket->pcb.tcp) < TCP_SND_BUF / 4) {
err = tcp_output(socket->pcb.tcp);
}
if (err != ERR_OK) {
*_errno = error_lookup_table[-err];
return MP_STREAM_ERROR;
}
return write_len;
}
olsr_scheduler(void)
{
OLSR_PRINTF(1, "Scheduler started - polling every %f ms\n", (double)olsr_cnf->pollrate);
/* Main scheduler loop */
while (true) {
uint32_t next_interval;
/*
* Update the global timestamp. We are using a non-wallclock timer here
* to avoid any undesired side effects if the system clock changes.
*/
now_times = olsr_times();
next_interval = GET_TIMESTAMP(olsr_cnf->pollrate * 1000);
/* Read incoming data */
poll_sockets();
/* Process timers */
walk_timers(&timer_last_run);
/* Update */
olsr_process_changes();
/* Check for changes in topology */
if (link_changes) {
increase_local_ansn();
OLSR_PRINTF(3, "ANSN UPDATED %d\n\n", get_local_ansn());
link_changes = false;
}
/* Read incoming data and handle it immediiately */
handle_fds(next_interval);
#ifdef _WIN32
if (olsr_win32_end_request) {
olsr_win32_end_flag = true;
}
#endif /* _WIN32 */
}
}
// lwip.getaddrinfo
STATIC mp_obj_t lwip_getaddrinfo(mp_obj_t host_in, mp_obj_t port_in) {
mp_uint_t hlen;
const char *host = mp_obj_str_get_data(host_in, &hlen);
mp_int_t port = mp_obj_get_int(port_in);
getaddrinfo_state_t state;
state.status = 0;
err_t ret = dns_gethostbyname(host, (ip_addr_t*)&state.ipaddr, lwip_getaddrinfo_cb, &state);
switch (ret) {
case ERR_OK:
// cached
state.status = 1;
break;
case ERR_INPROGRESS:
while (state.status == 0) {
poll_sockets();
}
break;
default:
state.status = ret;
}
if (state.status < 0) {
// TODO: CPython raises gaierror, we raise with native lwIP negative error
// values, to differentiate from normal errno's at least in such way.
nlr_raise(mp_obj_new_exception_arg1(&mp_type_OSError, MP_OBJ_NEW_SMALL_INT(state.status)));
}
mp_obj_tuple_t *tuple = mp_obj_new_tuple(5, NULL);
tuple->items[0] = MP_OBJ_NEW_SMALL_INT(MOD_NETWORK_AF_INET);
tuple->items[1] = MP_OBJ_NEW_SMALL_INT(MOD_NETWORK_SOCK_STREAM);
tuple->items[2] = MP_OBJ_NEW_SMALL_INT(0);
tuple->items[3] = MP_OBJ_NEW_QSTR(MP_QSTR_);
tuple->items[4] = netutils_format_inet_addr((uint8_t*)&state.ipaddr, port, NETUTILS_BIG);
return mp_obj_new_list(1, (mp_obj_t*)&tuple);
}
void PollSet::update(int poll_timeout)
{
createNativePollset();
// Poll across the sockets we're servicing
int ret;
size_t ufds_count = ufds_.size();
if((ret = poll_sockets(&ufds_.front(), ufds_count, poll_timeout)) < 0)
{
ROS_ERROR_STREAM("poll failed with error " << last_socket_error_string());
}
else if (ret > 0) // ret = 0 implies the poll timed out, nothing to do
{
// We have one or more sockets to service
for(size_t i=0; i<ufds_count; i++)
{
if (ufds_[i].revents == 0)
{
continue;
}
SocketUpdateFunc func;
TransportPtr transport;
int events = 0;
{
boost::mutex::scoped_lock lock(socket_info_mutex_);
M_SocketInfo::iterator it = socket_info_.find(ufds_[i].fd);
// the socket has been entirely deleted
if (it == socket_info_.end())
{
continue;
}
const SocketInfo& info = it->second;
// Store off the function and transport in case the socket is deleted from another thread
func = info.func_;
transport = info.transport_;
events = info.events_;
}
// If these are registered events for this socket, OR the events are ERR/HUP/NVAL,
// call through to the registered function
int revents = ufds_[i].revents;
if (func
&& ((events & revents)
|| (revents & POLLERR)
|| (revents & POLLHUP)
|| (revents & POLLNVAL)))
{
bool skip = false;
if (revents & (POLLNVAL|POLLERR|POLLHUP))
{
// If a socket was just closed and then the file descriptor immediately reused, we can
// get in here with what we think is a valid socket (since it was just re-added to our set)
// but which is actually referring to the previous fd with the same #. If this is the case,
// we ignore the first instance of one of these errors. If it's a real error we'll
// hit it again next time through.
boost::mutex::scoped_lock lock(just_deleted_mutex_);
if (std::find(just_deleted_.begin(), just_deleted_.end(), ufds_[i].fd) != just_deleted_.end())
{
skip = true;
}
}
if (!skip)
{
func(revents & (events|POLLERR|POLLHUP|POLLNVAL));
}
}
ufds_[i].revents = 0;
}
boost::mutex::scoped_lock lock(just_deleted_mutex_);
just_deleted_.clear();
}
}
/*
* Network listener for SIP Short Messages.
* Timeout in milliseconds (negative means infinity).
*
* Result is <0 if error;
* Result is >0 if buffer contains a received packet.
* (Packet's source address is saved away for future access by
* our caller.)
* Result == 0 if caller was interested in writing, there's no received
* packet yet, and it's OK to write now. OR if we timed out.
*/
int
SMnet::get_next_dgram (char *buffer, size_t bufferlen, int mstimeout)
{
int i, fd, flags;
nfds_t j;
short revents;
socklen_t addrlen;
size_t recvlength;
i = poll_sockets(mstimeout);
if (i < 0) // error
return i;
if (i == 0) // timeout
return 0;
for (j = 0; j < numsockets; j++) { // Walk the sockets.
fd = sockets[j].fd;
revents = sockets[j].revents;
#ifdef POLLRDHUP
if (revents & (POLLIN|POLLPRI|POLLRDHUP))
#else
if (revents & (POLLIN|POLLPRI))
#endif
{ // input OK
// FIXME, TCP and files aren't supported yet
addrlen = sizeof(src_addr);
flags = MSG_DONTWAIT|MSG_TRUNC;
recvlength = recvfrom(fd, buffer, bufferlen, flags,
(sockaddr *)&src_addr, &addrlen);
if (recvlength < 0) {
// Error on receive.
cerr << "Error " << strerror(errno)
<< "on recvfrom" << endl;
// We shouldn't loop -- or the error might make
// an endless loop. Return.
return -1;
}
if (addrlen > sizeof(src_addr)) {
// Received address truncated. BUG in program!
cerr << "recvfrom received address truncated!"
<< endl;
// FIXME, print src_addr too
abfuckingort();
}
recvaddrlen = addrlen; // Save for later
if (recvlength > bufferlen) {
// Received packet itself truncated.
cerr << "recvfrom data packet truncated, "
"buffer has "
<< bufferlen << " bytes, packet of "
<< recvlength << "!" << endl;
// FIXME, print src_addr too
return -1;
}
//
// OK, we got a full packet from a particular address.
// Pass it upstairs for further processing.
//
return recvlength;
}
if (revents & (POLLOUT)) { // output OK
return 0; // Tell caller to retry write
}
if (revents & (POLLERR|POLLHUP|POLLNVAL)) { // errors
cerr << "Poll error " << strerror(errno)
<< "huh!" << endl;
return -1;
}
// If no bits set on this FD, loop to the next one.
}
cerr << "Poll() returned " << i << " without any pending I/O" << endl;
return -1;
}
STATIC mp_obj_t lwip_socket_connect(mp_obj_t self_in, mp_obj_t addr_in) {
lwip_socket_obj_t *socket = self_in;
if (socket->pcb.tcp == NULL) {
mp_raise_OSError(MP_EBADF);
}
// get address
uint8_t ip[NETUTILS_IPV4ADDR_BUFSIZE];
mp_uint_t port = netutils_parse_inet_addr(addr_in, ip, NETUTILS_BIG);
ip_addr_t dest;
IP4_ADDR(&dest, ip[0], ip[1], ip[2], ip[3]);
err_t err = ERR_ARG;
switch (socket->type) {
case MOD_NETWORK_SOCK_STREAM: {
if (socket->state != STATE_NEW) {
if (socket->state == STATE_CONNECTED) {
mp_raise_OSError(MP_EISCONN);
} else {
mp_raise_OSError(MP_EALREADY);
}
}
// Register our receive callback.
tcp_recv(socket->pcb.tcp, _lwip_tcp_recv);
socket->state = STATE_CONNECTING;
err = tcp_connect(socket->pcb.tcp, &dest, port, _lwip_tcp_connected);
if (err != ERR_OK) {
socket->state = STATE_NEW;
mp_raise_OSError(error_lookup_table[-err]);
}
socket->peer_port = (mp_uint_t)port;
memcpy(socket->peer, &dest, sizeof(socket->peer));
// And now we wait...
if (socket->timeout != -1) {
for (mp_uint_t retries = socket->timeout / 100; retries--;) {
mp_hal_delay_ms(100);
if (socket->state != STATE_CONNECTING) break;
}
if (socket->state == STATE_CONNECTING) {
mp_raise_OSError(MP_EINPROGRESS);
}
} else {
while (socket->state == STATE_CONNECTING) {
poll_sockets();
}
}
if (socket->state == STATE_CONNECTED) {
err = ERR_OK;
} else {
err = socket->state;
}
break;
}
case MOD_NETWORK_SOCK_DGRAM: {
err = udp_connect(socket->pcb.udp, &dest, port);
break;
}
}
if (err != ERR_OK) {
mp_raise_OSError(error_lookup_table[-err]);
}
return mp_const_none;
}
STATIC mp_obj_t lwip_socket_accept(mp_obj_t self_in) {
lwip_socket_obj_t *socket = self_in;
if (socket->pcb.tcp == NULL) {
mp_raise_OSError(MP_EBADF);
}
if (socket->type != MOD_NETWORK_SOCK_STREAM) {
mp_raise_OSError(MP_EOPNOTSUPP);
}
// I need to do this because "tcp_accepted", later, is a macro.
struct tcp_pcb *listener = socket->pcb.tcp;
if (listener->state != LISTEN) {
mp_raise_OSError(MP_EINVAL);
}
// accept incoming connection
if (socket->incoming.connection == NULL) {
if (socket->timeout == 0) {
mp_raise_OSError(MP_EAGAIN);
} else if (socket->timeout != -1) {
for (mp_uint_t retries = socket->timeout / 100; retries--;) {
mp_hal_delay_ms(100);
if (socket->incoming.connection != NULL) break;
}
if (socket->incoming.connection == NULL) {
mp_raise_OSError(MP_ETIMEDOUT);
}
} else {
while (socket->incoming.connection == NULL) {
poll_sockets();
}
}
}
// create new socket object
lwip_socket_obj_t *socket2 = m_new_obj_with_finaliser(lwip_socket_obj_t);
socket2->base.type = (mp_obj_t)&lwip_socket_type;
// We get a new pcb handle...
socket2->pcb.tcp = socket->incoming.connection;
socket->incoming.connection = NULL;
// ...and set up the new socket for it.
socket2->domain = MOD_NETWORK_AF_INET;
socket2->type = MOD_NETWORK_SOCK_STREAM;
socket2->incoming.pbuf = NULL;
socket2->timeout = socket->timeout;
socket2->state = STATE_CONNECTED;
socket2->recv_offset = 0;
socket2->callback = MP_OBJ_NULL;
tcp_arg(socket2->pcb.tcp, (void*)socket2);
tcp_err(socket2->pcb.tcp, _lwip_tcp_error);
tcp_recv(socket2->pcb.tcp, _lwip_tcp_recv);
tcp_accepted(listener);
// make the return value
uint8_t ip[NETUTILS_IPV4ADDR_BUFSIZE];
memcpy(ip, &(socket2->pcb.tcp->remote_ip), sizeof(ip));
mp_uint_t port = (mp_uint_t)socket2->pcb.tcp->remote_port;
mp_obj_tuple_t *client = mp_obj_new_tuple(2, NULL);
client->items[0] = socket2;
client->items[1] = netutils_format_inet_addr(ip, port, NETUTILS_BIG);
return client;
}
// Helper function for recv/recvfrom to handle TCP packets
STATIC mp_uint_t lwip_tcp_receive(lwip_socket_obj_t *socket, byte *buf, mp_uint_t len, int *_errno) {
// Check for any pending errors
STREAM_ERROR_CHECK(socket);
if (socket->incoming.pbuf == NULL) {
// Non-blocking socket
if (socket->timeout == 0) {
if (socket->state == STATE_PEER_CLOSED) {
return 0;
}
*_errno = MP_EAGAIN;
return -1;
}
mp_uint_t start = mp_hal_ticks_ms();
while (socket->state == STATE_CONNECTED && socket->incoming.pbuf == NULL) {
if (socket->timeout != -1 && mp_hal_ticks_ms() - start > socket->timeout) {
*_errno = MP_ETIMEDOUT;
return -1;
}
poll_sockets();
}
if (socket->state == STATE_PEER_CLOSED) {
if (socket->incoming.pbuf == NULL) {
// socket closed and no data left in buffer
return 0;
}
} else if (socket->state != STATE_CONNECTED) {
assert(socket->state < 0);
*_errno = error_lookup_table[-socket->state];
return -1;
}
}
assert(socket->pcb.tcp != NULL);
struct pbuf *p = socket->incoming.pbuf;
mp_uint_t remaining = p->len - socket->recv_offset;
if (len > remaining) {
len = remaining;
}
memcpy(buf, (byte*)p->payload + socket->recv_offset, len);
remaining -= len;
if (remaining == 0) {
socket->incoming.pbuf = p->next;
// If we don't ref here, free() will free the entire chain,
// if we ref, it does what we need: frees 1st buf, and decrements
// next buf's refcount back to 1.
pbuf_ref(p->next);
pbuf_free(p);
socket->recv_offset = 0;
} else {
socket->recv_offset += len;
}
tcp_recved(socket->pcb.tcp, len);
return len;
}
