static void lwip_perf_thread(void *arg)
{
static struct netconn *conn;
static struct netbuf *buf;
//static ip_addr_t *addr;
static unsigned short port;
char *buffer;
lwip_perf_cmd_t cmd;
err_t err = ERR_OK;
uint16_t len, i, j;
LWIP_UNUSED_ARG(arg);
conn = netconn_new(NETCONN_UDP);
LWIP_ASSERT("con != NULL", conn != NULL);
netconn_bind(conn, NULL, 7); // echo port
buffer = malloc(PERF_MAX_PAYLOAD_SIZE);
assert(buffer);
while (1) {
err = netconn_recv(conn, &buf);
if (err == ERR_OK) {
//addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
uprintf(UPRINT_INFO, UPRINT_BLK_NET, "LWIP perf Rx: port=%d\n", port);
len = netbuf_copy(buf, (char *)&cmd, sizeof(lwip_perf_cmd_t));
/* no need netconn_connect here, since the netbuf contains the address */
if(len != sizeof(lwip_perf_cmd_t)) {
LWIP_DEBUGF(LWIP_DBG_ON, ("netbuf_copy failed\n"));
} else {
/* check packet size */
if(cmd.nSize>PERF_MAX_PAYLOAD_SIZE || cmd.nSize==0)
cmd.nSize = PERF_MAX_PAYLOAD_SIZE;
/* link buffer to netbuf */
err = netbuf_ref(buf, buffer, cmd.nSize);
LWIP_DEBUGF(LWIP_DBG_ON, ("lwip perf: nPacket=%d nSize=%d nDelay=%d\n", cmd.nPacket, cmd.nSize, cmd.nDelay));
for(i=0; i<cmd.nPacket && err==ERR_OK; i++)
{
/* simulate buffer construction */
for(j=0; j<cmd.nSize; j++)
buffer[j] = (j&0xFF);
/* send packet now */
err = netconn_send(conn, buf);
if(err != ERR_OK) {
LWIP_DEBUGF(LWIP_DBG_ON, ("netconn_send failed: %d\n", (int)err));
}
if(cmd.nDelay) task_delay(cmd.nDelay);
}
LWIP_DEBUGF(LWIP_DBG_ON, ("lwip perf: send %d packets\n", i));
}
netbuf_delete(buf);
}
}
free(buffer);
}
void udpecho_entry(void *parameter)
{
struct netconn *conn;
struct netbuf *buf;
struct ip_addr *addr;
unsigned short port;
conn = netconn_new(NETCONN_UDP);
netconn_bind(conn, IP_ADDR_ANY, 7);
while(1)
{
/* received data to buffer */
buf = netconn_recv(conn);
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
/* send the data to buffer */
netconn_connect(conn, addr, port);
/* reset address, and send to client */
buf->addr = RT_NULL;
netconn_send(conn, buf);
/* release buffer */
netbuf_delete(buf);
}
}
/*-----------------------------------------------------------------------------------*/
static void
udpecho_thread(void *arg)
{
static struct netconn *conn;
static struct netbuf *buf;
static struct ip_addr *addr;
static unsigned short port;
char buffer[256];
LWIP_UNUSED_ARG(arg);
conn = netconn_new(NETCONN_UDP); /* Create a new netconnect */
LWIP_ASSERT("con != NULL", conn != NULL);
netconn_bind(conn, IP_ADDR_ANY, UDP_ECHO_PORT); /* udpecho using port 7 */
while (1) {
buf = netconn_recv(conn); /* received data to buffer */
if (buf != NULL) {
addr = netbuf_fromaddr(buf); /* get client's IP address */
port = netbuf_fromport(buf); /* get client's port */
netconn_connect(conn, addr, port); /* connect to client */
netbuf_copy(buf, buffer, buf->p->tot_len); /* set echo data */
buffer[buf->p->tot_len] = '\0';
buf->addr = NULL;
netconn_send(conn, buf); /* send data back to client */
LWIP_DEBUGF(LWIP_DBG_ON, ("got %s\n", buffer));
netbuf_delete(buf); /* release buffer */
}
}
}
/*-----------------------------------------------------------------------------------*/
static void udpecho_thread(void *arg)
{
err_t err, recv_err;
LWIP_UNUSED_ARG(arg);
conn = netconn_new(NETCONN_UDP);
if (conn!= NULL)
{
err = netconn_bind(conn, IP_ADDR_ANY, 7);
if (err == ERR_OK)
{
while (1)
{
recv_err = netconn_recv(conn, &buf);
if (recv_err == ERR_OK)
{
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
netconn_connect(conn, addr, port);
buf->addr.addr = 0;
netconn_send(conn,buf);
netbuf_delete(buf);
}
}
}
else
{
netconn_delete(conn);
}
}
}
void handle_someip_sd_packet(struct netbuf *buf)
{
unsigned char *recv_data, *data_ptr;
ip_addr_t *addr;
unsigned short port;
unsigned long entries_len, len;
someip_sd_header_t *sd_header;
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
netbuf_data(buf, &recv_data, &len);
/*
printf("\r\n(1) Received data %d\r\n", len);
int i;
for(i=0;i<len && i<90;++i){
if(i % 8 == 0)
printf("\r\n");
printf("%02x ",recv_data[i]);
}
*/
sd_header = (someip_sd_header_t *)recv_data;
if(sd_header->proto_ver != 0x01 || sd_header->if_ver != 0x01 ||
sd_header->msg_type != 0x02 || sd_header->ret_code != 0x00) {
printf("this packet may not be a someip-sd\r\n");
return;
}
data_ptr = recv_data + sizeof(someip_sd_header_t);
entries_len = ntohl(*((unsigned long *)data_ptr));
data_ptr += sizeof(unsigned long);
while(entries_len) {
switch(*data_ptr ) {
case 0x00:
printf("Got findservice\r\n");
handle_findservice(addr, port, data_ptr, entries_len);
break;
case 0x01:
printf("Got Offerservice\r\n");
handle_offerservice(addr, port, data_ptr, entries_len);
break;
default:
printf("unsupported entry type\r\n");
}
entries_len -= sizeof(someip_sd_entry_t);
data_ptr += sizeof(someip_sd_entry_t);
}
}
static nsapi_size_or_error_t mbed_lwip_socket_recvfrom(nsapi_stack_t *stack, nsapi_socket_t handle, nsapi_addr_t *addr, uint16_t *port, void *data, nsapi_size_t size)
{
struct lwip_socket *s = (struct lwip_socket *)handle;
struct netbuf *buf;
err_t err = netconn_recv(s->conn, &buf);
if (err != ERR_OK) {
return mbed_lwip_err_remap(err);
}
convert_lwip_addr_to_mbed(addr, netbuf_fromaddr(buf));
*port = netbuf_fromport(buf);
u16_t recv = netbuf_copy(buf, data, (u16_t)size);
netbuf_delete(buf);
return recv;
}
static int lwip_socket_recvfrom(nsapi_stack_t *stack, nsapi_socket_t handle, nsapi_addr_t *addr, uint16_t *port, void *data, unsigned size)
{
struct lwip_socket *s = (struct lwip_socket *)handle;
struct netbuf *buf;
err_t err = netconn_recv(s->conn, &buf);
if (err != ERR_OK) {
return lwip_err_remap(err);
}
addr->version = NSAPI_IPv4;
memcpy(addr->bytes, netbuf_fromaddr(buf), sizeof addr->bytes);
*port = netbuf_fromport(buf);
u16_t recv = netbuf_copy(buf, data, (u16_t)size);
netbuf_delete(buf);
return recv;
}
void
udpecho_thread(void *arg)
{
struct netconn *conn;
struct netbuf *buf;
struct ip_addr *addr;
unsigned short port;
conn = netconn_new(NETCONN_UDP);
netconn_bind(conn, NULL, 7);
while(1) {
buf = netconn_recv(conn);
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
netconn_connect(conn, addr, port);
netconn_send(conn, buf);
netbuf_copy(buf, buffer, sizeof(buffer));
netbuf_delete(buf);
}
}
/**
* UDP echo server thread.
*/
msg_t udp_echo_server(void *p)
{
err_t err, recv_err;
LWIP_UNUSED_ARG(p);
conn = netconn_new(NETCONN_UDP);
if (conn!= NULL)
{
err = netconn_bind(conn, IP_ADDR_ANY, UDP_THREAD_PORT);
if (err == ERR_OK)
{
while (1)
{
recv_err = netconn_recv(conn, &buf);
if (recv_err == ERR_OK)
{
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
netconn_connect(conn, addr, port);
buf->addr.addr = 0;
netconn_send(conn,buf);
netbuf_delete(buf);
}
}
}
else
{
netconn_delete(conn);
// printf("can not bind netconn");
}
}
else
{
// printf("can create new UDP netconn");
}
return RDY_OK;
}
nsapi_size_or_error_t LWIP::socket_recvfrom(nsapi_socket_t handle, SocketAddress *address, void *data, nsapi_size_t size)
{
struct mbed_lwip_socket *s = (struct mbed_lwip_socket *)handle;
struct netbuf *buf;
err_t err = netconn_recv(s->conn, &buf);
if (err != ERR_OK) {
return err_remap(err);
}
if (address) {
nsapi_addr_t addr;
convert_lwip_addr_to_mbed(&addr, netbuf_fromaddr(buf));
address->set_addr(addr);
address->set_port(netbuf_fromport(buf));
}
u16_t recv = netbuf_copy(buf, data, (u16_t)size);
netbuf_delete(buf);
return recv;
}
/*-----------------------------------------------------------------------------------*/
static void udpecho_thread(void *arg)
{
err_t err;
LWIP_UNUSED_ARG(arg);
conn = netconn_new(NETCONN_UDP);
if (conn!= NULL)
{
err = netconn_bind(conn, IP_ADDR_ANY, 7);
if (err == ERR_OK)
{
while (1)
{
buf = netconn_recv(conn);
if (buf!= NULL)
{
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
netconn_connect(conn, addr, port);
buf->addr = NULL;
netconn_send(conn,buf);
netbuf_delete(buf);
}
}
}
else
{
printf("can not bind netconn");
}
}
else
{
printf("can create new UDP netconn");
}
}
/*-----------------------------------------------------------------------------------*/
static void
udpecho_thread(void *arg)
{
static struct netconn *conn;
static struct netbuf *buf;
static ip_addr_t *addr;
static unsigned short port;
char buffer[4096];
err_t err;
LWIP_UNUSED_ARG(arg);
conn = netconn_new(NETCONN_UDP);
LWIP_ASSERT("con != NULL", conn != NULL);
netconn_bind(conn, NULL, 7);
while (1) {
err = netconn_recv(conn, &buf);
if (err == ERR_OK) {
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
/* no need netconn_connect here, since the netbuf contains the address */
if(netbuf_copy(buf, buffer, buf->p->tot_len) != buf->p->tot_len) {
LWIP_DEBUGF(LWIP_DBG_ON, ("netbuf_copy failed\n"));
} else {
buffer[buf->p->tot_len] = '\0';
err = netconn_send(conn, buf);
if(err != ERR_OK) {
LWIP_DEBUGF(LWIP_DBG_ON, ("netconn_send failed: %d\n", (int)err));
} else {
LWIP_DEBUGF(LWIP_DBG_ON, ("got %s\n", buffer));
}
}
netbuf_delete(buf);
}
}
}
/*-----------------------------------------------------------------------------------*/
void
udpecho_thread(void *arg)
{
static struct netconn *conn;
static struct netbuf *buf;
static struct ip_addr *addr;
static unsigned short port;
char buffer[4096];
conn = netconn_new(NETCONN_UDP);
netconn_bind(conn, NULL, 7);
while (1) {
buf = netconn_recv(conn);
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
netconn_connect(conn, addr, port);
netbuf_copy(buf, buffer, buf->p->tot_len);
buffer[buf->p->tot_len] = '\0';
netconn_send(conn, buf);
printf("got %s\n", buffer);
netbuf_delete(buf);
}
}
int
lwip_recvfrom(int s, void *mem, int len, unsigned int flags,
struct sockaddr *from, socklen_t *fromlen)
{
struct lwip_socket *sock;
struct netbuf *buf;
u16_t buflen, copylen;
struct ip_addr *addr;
u16_t port;
LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom(%d, %p, %d, 0x%x, ..)\n", s, mem, len, flags));
sock = get_socket(s);
if (!sock) {
set_errno(EBADF);
return -1;
}
/* Check if there is data left from the last recv operation. */
if (sock->lastdata) {
buf = sock->lastdata;
} else {
/* If this is non-blocking call, then check first */
if (((flags & MSG_DONTWAIT) || (sock->flags & O_NONBLOCK))
&& !sock->rcvevent)
{
LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom(%d): returning EWOULDBLOCK\n", s));
sock_set_errno(sock, EWOULDBLOCK);
return -1;
}
/* No data was left from the previous operation, so we try to get
some from the network. */
buf = netconn_recv(sock->conn);
if (!buf) {
/* We should really do some error checking here. */
LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom(%d): buf == NULL!\n", s));
sock_set_errno(sock, 0);
return 0;
}
}
buflen = netbuf_len(buf);
buflen -= sock->lastoffset;
if (len > buflen) {
copylen = buflen;
} else {
copylen = len;
}
/* copy the contents of the received buffer into
the supplied memory pointer mem */
netbuf_copy_partial(buf, mem, copylen, sock->lastoffset);
/* Check to see from where the data was. */
if (from && fromlen) {
struct sockaddr_in sin;
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
memset(&sin, 0, sizeof(sin));
sin.sin_len = sizeof(sin);
sin.sin_family = AF_INET;
sin.sin_port = htons(port);
sin.sin_addr.s_addr = addr->addr;
if (*fromlen > sizeof(sin))
*fromlen = sizeof(sin);
memcpy(from, &sin, *fromlen);
LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom(%d): addr=", s));
ip_addr_debug_print(SOCKETS_DEBUG, addr);
LWIP_DEBUGF(SOCKETS_DEBUG, (" port=%u len=%u\n", port, copylen));
} else {
#if SOCKETS_DEBUG
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom(%d): addr=", s));
ip_addr_debug_print(SOCKETS_DEBUG, addr);
LWIP_DEBUGF(SOCKETS_DEBUG, (" port=%u len=%u\n", port, copylen));
#endif
}
/* If this is a TCP socket, check if there is data left in the
buffer. If so, it should be saved in the sock structure for next
time around. */
if (netconn_type(sock->conn) == NETCONN_TCP && buflen - copylen > 0) {
sock->lastdata = buf;
sock->lastoffset += copylen;
} else {
sock->lastdata = NULL;
sock->lastoffset = 0;
netbuf_delete(buf);
}
sock_set_errno(sock, 0);
return copylen;
}
int recvfrom(int s, void *mem, int len, unsigned int flags,
struct sockaddr *from, socklen_t *fromlen)
{
struct lwip_socket *sock;
struct skbuff *buf;
u16_t buflen, copylen, off = 0;
struct ip_addr *addr;
u16_t port;
u8_t done = 0;
sock = get_socket(s);
if (!sock)
return -1;
do {
/* Check if there is data left from the last recv operation. */
if (sock->lastdata)
{
buf = sock->lastdata;
}
else
{
/* If this is non-blocking call, then check first */
if (((flags & MSG_DONTWAIT) || (sock->flags & O_NONBLOCK)) && !sock->rcvevent)
{
return -1;
}
/* No data was left from the previous operation, so we try to get
some from the network. */
sock->lastdata = buf = netconn_recv(sock->conn);
if (!buf)
{
/* We should really do some error checking here. */
return 0;
}
}
buflen = netbuf_len(buf);
buflen -= sock->lastoffset;
if (len > buflen)
{
copylen = buflen;
}
else
{
copylen = len;
}
/* copy the contents of the received buffer into
the supplied memory pointer mem */
netbuf_copy_partial(buf, (u8_t*)mem + off, copylen, sock->lastoffset);
off += copylen;
if (netconn_type(sock->conn) == NETCONN_TCP)
{
len -= copylen;
if ( (len <= 0) || (buf->p->flags & PBUF_FLAG_PUSH) || !sock->rcvevent)
{
done = 1;
}
}
else
{
done = 1;
}
/* If we don't peek the incoming message... */
if ((flags & MSG_PEEK)==0)
{
/* If this is a TCP socket, check if there is data left in the
buffer. If so, it should be saved in the sock structure for next
time around. */
if ((sock->conn->type == NETCONN_TCP) && (buflen - copylen > 0))
{
sock->lastdata = buf;
sock->lastoffset += copylen;
}
else
{
sock->lastdata = NULL;
sock->lastoffset = 0;
netbuf_delete(buf);
}
}
else
{
done = 1;
}
} while (!done);
/* Check to see from where the data was.*/
if (from && fromlen)
{
struct sockaddr_in sin;
if (netconn_type(sock->conn) == NETCONN_TCP)
{
addr = (struct ip_addr*)&(sin.sin_addr.s_addr);
netconn_getaddr(sock->conn, addr, &port, 0);
}
else
{
addr = netbuf_fromaddr(buf);
port = netbuf_fromport(buf);
}
memset(&sin, 0, sizeof(sin));
sin.sin_len = sizeof(sin);
sin.sin_family = AF_INET;
sin.sin_port = htons(port);
sin.sin_addr.s_addr = addr->addr;
if (*fromlen > sizeof(sin))
*fromlen = sizeof(sin);
SMEMCPY(from, &sin, *fromlen);
}
return off;
}
