File content as of revision 5:445d2fc04784:

/*********************************************************************
PicoTCP. Copyright (c) 2013 TASS Belgium NV. Some rights reserved.
See LICENSE and COPYING for usage.

Authors: Andrei Carp <andrei.carp@tass.be>, Daniele Lacamera <daniele.lacamera@tass.be>
*********************************************************************/
#include "pico_bsd_layer.h"

#include "mbed.h"
#include "rtos.h"
#include "cmsis_os.h"
#include "Queue.h"
#include "Mutex.h"
#include "PicoCondition.h"
/************************** PICO BSD SOCKET LAYER **************************/

#define DEFAULT_BACKLOG        (5u)
#define DEFAULT_TIMEOUT        (10000u)

// Handle events received from the callback
#define EVENT_PENDING(x,ev)          (x & (ev))
#define CLEAR_EVENTS(x)              (x ^= (x))
#define SET_EVENT(x,ev)              (x |= (ev))
#define RESET_EVENT(x,ev)            (x &= (~ev))
#define CLEAR_ERROR(x)               RESET_EVENT(x,PICO_SOCK_EV_ERR)

#define EVENT_EXPECTED(x)            (x->event & x->revent)

/* TODO: Sync blocking from here to blocking from Socket */

static struct socket dummy;
#define MAKE_SOCKET(x) ((struct socket *)x)

static int compareSock(void *ka, void *kb)
{
    return MAKE_SOCKET(ka)->socket - MAKE_SOCKET(kb)->socket;
}

PICO_TREE_DECLARE(sockTree,compareSock);

#define VALIDATE_NULL(param) \
    if(!param) \
    { \
        return -1; \
    }

#define VALIDATE_ONE(param,value) \
    if(param != value) { \
        pico_err = PICO_ERR_EINVAL; \
        return -1; \
    }

#define VALIDATE_TWO(param,value1,value2) \
    if(param != value1 && param != value2) { \
        pico_err = PICO_ERR_EINVAL; \
        return -1; \
    }

/* PRIVATE FUNCTIONS */

static inline struct socket * findAfterId(int sock)
{
    dummy.socket = sock;
    return (struct socket *)pico_tree_findKey(&sockTree,&dummy);
}

static inline struct socket * findAfterSocket(struct pico_socket * sock)
{
    struct pico_tree_node * index;

    pico_tree_foreach(index,&sockTree)
    {
        if( MAKE_SOCKET(index->keyValue)->sock == sock )
            return (struct socket *)index->keyValue;
    }

    return NULL;
}

static inline int waitForEvent(struct socket * sock, uint16_t event, uint8_t clear)
{
    int retValue = 0;
    
    if(sock->blocking)
    {
        if(EVENT_PENDING(sock->revent,event))
        {
            //printf("Already...\n");
            RESET_EVENT(sock->revent, event);
            RESET_EVENT(sock->event, event);
        }
        else
        {
            SET_EVENT(sock->event, event);
            //printf("Locked...\n");
            sock->mutex->lock();
            RESET_EVENT(sock->event,event);
    
            if(! EVENT_PENDING(sock->revent,event) || EVENT_PENDING(sock->revent, PICO_SOCK_EV_ERR) || 
            EVENT_PENDING(sock->revent,PICO_SOCK_EV_CLOSE) || EVENT_PENDING(sock->revent,PICO_SOCK_EV_FIN) )
                retValue = -1;
        }
        if(clear)
            RESET_EVENT(sock->revent, event);
    }
    
    return retValue;
}


static void dispatch_socket(uint16_t ev, struct pico_socket * sock)
{
    struct socket * _sock = findAfterSocket(sock);

    if(!_sock)
        return;

    //printf("Received : %d\n",ev);
    
    SET_EVENT(_sock->revent,ev);
    if(EVENT_EXPECTED(_sock))
    {
        // if one of the events we're waiting has arrived, unblock this
        //printf("Expected event, release the orcs!\n");
         _sock->mutex->unlock();
    }
    else
    {
        //printf("Unexpected event\n");
        // give semaphore back when -1 occurs or close requested
        if( (ev & PICO_SOCK_EV_ERR) || (ev & PICO_SOCK_EV_CLOSE) || (ev & PICO_SOCK_EV_FIN) )
        {
            //printf("Exception\n");
            
            _sock->state = SOCK_CLOSED;
            if(_sock->sock)
            {
                pico_socket_close(_sock->sock);
                _sock->sock = NULL;
            }
            _sock->mutex->unlock();
        }
    }
}

/* PUBLIC FUNCTIONS */

int picotcp_socket(uint16_t net, uint16_t proto, uint16_t flags)
{
    struct socket * sock = NULL;

    net = (net == AF_INET ? PICO_PROTO_IPV4 : (net == AF_INET6 ? PICO_PROTO_IPV6 : -1));
    proto = (proto == SOCK_DGRAM ? PICO_PROTO_UDP : (proto == SOCK_STREAM ? PICO_PROTO_TCP : -1));
    
    VALIDATE_TWO(net,PICO_PROTO_IPV4,PICO_PROTO_IPV6);
    VALIDATE_TWO(proto,PICO_PROTO_TCP,PICO_PROTO_UDP);
    //printf("Parameters are correct...\n");
    sock = (struct socket*)pico_zalloc(sizeof(struct socket));
    VALIDATE_NULL(sock);

    sock->proto = proto;
    sock->sock = pico_socket_open(net,proto,&dispatch_socket);

    if(!sock->sock)
    {
        pico_free(sock);
        return -1;
    }

    sock->socket = (int)sock;
    sock->mutex = new PicoCondition();

    if(!sock->mutex)
    {
        pico_socket_close(sock->sock);
        pico_free(sock);
        return -1;
    }

    pico_tree_insert(&sockTree,sock);
    sock->state = SOCK_OPEN;

    return sock->socket;
}

int picotcp_bind(int sock, struct sockaddr *local_addr, socklen_t len)
{
    struct socket * _sock = findAfterId(sock);
    int ret;
    struct sockaddr_in *s_in = (struct sockaddr_in *) local_addr;

    VALIDATE_NULL(_sock);
    VALIDATE_NULL(local_addr);
    VALIDATE_NULL(len);

    ret = pico_socket_bind(_sock->sock,(void *)&s_in->sin_addr,(uint16_t *)&s_in->sin_port);

    if(ret == 0)
        _sock->state = SOCK_BOUND;

    return ret;
}

int picotcp_state(int sock)
{
    struct socket * _sock = findAfterId(sock);
    VALIDATE_NULL(_sock);

    return _sock->state;
}

int picotcp_listen(int sock, int queue)
{
    struct socket * _sock = findAfterId(sock);
    int ret;

    if(queue <= 0)
        queue = DEFAULT_BACKLOG;

    VALIDATE_NULL(_sock);
    VALIDATE_ONE(_sock->state,SOCK_BOUND);

    ret = pico_socket_listen(_sock->sock,queue);

    if(!ret)
        _sock->state = SOCK_LISTEN;

    return ret;
}

int picotcp_connect(int sock, struct sockaddr *srv_addr, socklen_t len)
{
    struct socket * _sock = findAfterId(sock);
    int ret;
    struct sockaddr_in *s_in = (struct sockaddr_in *) srv_addr;

    VALIDATE_NULL(_sock);
    VALIDATE_NULL(len);

    ret = pico_socket_connect(_sock->sock,(void*)&s_in->sin_addr,s_in->sin_port);

    if(ret < 0)
    {
        printf("connect error\n");
        return -1;
    }

    if(_sock->proto == PICO_PROTO_TCP)
    {
        if(waitForEvent(_sock,PICO_SOCK_EV_CONN,1) < 0)
            return -1;
    }

    _sock->state = SOCK_CONNECTED;

    return ret;
}

int picotcp_accept(int sock, struct sockaddr *orig, socklen_t *len)
{
    struct socket * _sock = findAfterId(sock);
    struct socket * client = NULL;
    struct pico_socket * sck = NULL;
    struct sockaddr_in *s_in = (struct sockaddr_in *) orig;


    VALIDATE_NULL(_sock);
    VALIDATE_ONE(_sock->proto, PICO_PROTO_TCP);
    VALIDATE_ONE(_sock->state, SOCK_LISTEN);
    VALIDATE_NULL(len);

    if(waitForEvent(_sock,PICO_SOCK_EV_CONN,1))
    {
        printf("Wait for event failed\n");
        return -1;
    }

    // create new socket and set it directly to connected
    sck = pico_socket_accept(_sock->sock,(void *)&s_in->sin_addr,(uint16_t *)&s_in->sin_port);
    *len = sizeof(struct sockaddr_in);

    if(!sck)
        return -1;

    client = (struct socket *)pico_zalloc(sizeof(struct socket));

    if(!client)
        return -1;

    client->sock = sck;
    client->proto = _sock->proto;
    client->state = SOCK_CONNECTED;
    client->socket = (int)sck;
    client->mutex = new PicoCondition();

    pico_tree_insert(&sockTree,client);

    return client->socket;
}

int picotcp_close(int sock)
{
    struct socket * _sock = findAfterId(sock);
    int ret = 0;

    VALIDATE_NULL(_sock);
    pico_socket_close(_sock->sock);
    if(_sock->state == SOCK_CONNECTED && _sock->proto == PICO_PROTO_TCP)
    {
        ret = waitForEvent(_sock,PICO_SOCK_EV_CLOSE | PICO_SOCK_EV_FIN,1);
    }


    if(_sock->mutex)
    {
        delete(_sock->mutex);
    }

    pico_tree_delete(&sockTree,_sock);
    pico_free(_sock);

    return ret;
}

int picotcp_send(int sock,void * buff, int len, int flags)
{
    struct socket * _sock = findAfterId(sock);
    
    VALIDATE_NULL(_sock);

    if(_sock->state != SOCK_BOUND && _sock->state != SOCK_CONNECTED)
    {
            printf("Bad state\n");
            return -1;
    }
    if(_sock->proto == PICO_PROTO_TCP)
    {
        if(waitForEvent(_sock,PICO_SOCK_EV_WR,1))
            return -1;
    }
    //printf("Sending...\n");
    return pico_socket_send(_sock->sock,buff,len);
}

int picotcp_recv(int sock,void * buff, int len, int flags)
{
    struct socket * _sock = findAfterId(sock);

    VALIDATE_NULL(_sock);

    if(_sock->state != SOCK_BOUND && _sock->state != SOCK_CONNECTED)
        return -1;

    if(waitForEvent(_sock,PICO_SOCK_EV_RD,1))
        return -1;

    return pico_socket_recv(_sock->sock,buff,len);
}

int picotcp_sendto(int sock,void * buff, int len, struct sockaddr *orig, socklen_t socklen)
{
    struct socket * _sock = findAfterId(sock);
    struct sockaddr_in *s_in = (struct sockaddr_in *) orig;

    VALIDATE_NULL(_sock);
    VALIDATE_ONE(_sock->proto,PICO_PROTO_UDP);
    VALIDATE_NULL(len);
    VALIDATE_NULL(socklen);
    printf("Sendto : parameters correct !\n");
  
    return pico_socket_sendto(_sock->sock,buff,len,(void *)&s_in->sin_addr,s_in->sin_port);
}

int picotcp_recvfrom(int sock,void * buff, int len, struct sockaddr *orig, socklen_t *socklen)
{
    struct socket * _sock = findAfterId(sock);
    struct sockaddr_in *s_in = (struct sockaddr_in *) orig;

    VALIDATE_NULL(_sock);
    VALIDATE_ONE(_sock->proto,PICO_PROTO_UDP);
    VALIDATE_NULL(socklen);
        
    if(waitForEvent(_sock,PICO_SOCK_EV_RD,1))
        return -1;
    return pico_socket_recvfrom(_sock->sock,buff,len,(void *)&s_in->sin_addr,(uint16_t *)&s_in->sin_port);
}

int picotcp_read(int sock,void *buf, int len)
{
    struct socket * _sock = findAfterId(sock);

    VALIDATE_NULL(_sock);
    VALIDATE_ONE(_sock->proto,PICO_PROTO_TCP);
    VALIDATE_ONE(_sock->state, SOCK_CONNECTED);

    if(waitForEvent(_sock,PICO_SOCK_EV_RD,1))
        return -1;

    return pico_socket_read(_sock->sock,buf,len);
}

int picotcp_write(int sock,void *buf, int len)
{
    struct socket * _sock = findAfterId(sock);

    VALIDATE_NULL(_sock);
    VALIDATE_ONE(_sock->proto,PICO_PROTO_TCP);
    VALIDATE_ONE(_sock->state, SOCK_CONNECTED);
    if(waitForEvent(_sock,PICO_SOCK_EV_WR,1))
        return -1;
    return pico_socket_write(_sock->sock,buf,len);
}

int picotcp_setsockopt(int sock, int option, void *value)
{
    struct socket * _sock =  findAfterId(sock);

    VALIDATE_NULL(_sock);

    return pico_socket_setoption(_sock->sock,option,value);
}

int picotcp_getsockopt(int sock, int option, void *value)
{
    struct socket * _sock =  findAfterId(sock);

    VALIDATE_NULL(_sock);

    return pico_socket_getoption(_sock->sock,option,value);
}

int picotcp_select(int sock, struct timeval *timeout, int read, int write)
{
    uint32_t ms = 1000 * timeout->tv_sec + timeout->tv_usec / 1000;
    uint32_t now = PICO_TIME_MS();
    int ret = 0;
    
    struct socket * _sock =  findAfterId(sock);
    VALIDATE_NULL(_sock);
    
    // tell the dispatcher what events are you waiting for
    if(read)
        SET_EVENT(_sock->event,PICO_SOCK_EV_RD|PICO_SOCK_EV_CONN);
    if(write)
        SET_EVENT(_sock->event,PICO_SOCK_EV_WR);
        
    while (PICO_TIME_MS() < now + ms) {
        if (_sock->mutex->lock(1) == false) {
            Thread::wait(1);
            continue;
        }
             
        if (read && EVENT_PENDING(_sock->revent,PICO_SOCK_EV_RD|PICO_SOCK_EV_CONN))
        {
            RESET_EVENT(_sock->revent,PICO_SOCK_EV_RD|PICO_SOCK_EV_CONN);
            ret++;
        }
        if (write && EVENT_PENDING(_sock->revent, PICO_SOCK_EV_WR))
        {
            RESET_EVENT(_sock->revent,PICO_SOCK_EV_WR);
            ret++;
        }
        if (ret > 0)
            break;
    }
    
    if(read)
        RESET_EVENT(_sock->event,PICO_SOCK_EV_RD|PICO_SOCK_EV_CONN);
    if(write)
        RESET_EVENT(_sock->event,PICO_SOCK_EV_WR);
    
    return ret;
}

int picotcp_setblocking(int sock, int blocking)
{
    struct socket * _sock =  findAfterId(sock);
    VALIDATE_NULL(_sock);
    
    _sock->blocking = blocking;
        
    return 0;
}


int picotcp_join_multicast(int sock,const char* address,const char* local)
{
    struct socket * _sock =  findAfterId(sock);
    struct pico_ip4 inaddr_dst, inaddr_link;
    struct pico_ip_mreq mreq = {{0},{0}};
    VALIDATE_NULL(_sock);
    pico_string_to_ipv4(address, &inaddr_dst.addr);
    pico_string_to_ipv4(local, &inaddr_link.addr); // local ip address ?
    mreq.mcast_group_addr = inaddr_dst;
    mreq.mcast_link_addr = inaddr_link;
    return pico_socket_setoption(_sock->sock, PICO_IP_ADD_MEMBERSHIP, &mreq);
}

/************************** PICO BSD DNS LAYER **************************/

struct dnsQuery
{
    PicoCondition * mutex;
    char * result;
};

static void dispatch_dns(char *ip, void *arg)
{
    ((struct dnsQuery *)arg)->result = ip;
    printf("Disp : %x\n",((struct dnsQuery *)arg)->mutex);
    ((struct dnsQuery *)arg)->mutex->unlock();
    printf("Result : %s\n",ip);
}

struct hostent * picotcp_gethostbyname(const char *url)
{
    struct dnsQuery * dns;
    struct hostent * hresult = NULL;
    
    dns = (struct dnsQuery *)pico_zalloc(sizeof(struct dnsQuery));

    if(!dns)
        return NULL;

    dns->mutex = new PicoCondition();

    pico_dns_client_getaddr(url,dispatch_dns,(void *)dns);
    dns->mutex->lock();
    
    if(dns->result)
    {
        hresult = (struct hostent *)pico_zalloc(sizeof(struct hostent));
        hresult->h_addrtype = AF_INET; // ipv4
        // make a copy of the name
        hresult->h_name = new char[strlen(url)+1];
        memcpy(hresult->h_name,url,strlen(url)+1);
        hresult->h_aliases = NULL;
        // address list
        hresult->h_addr_list = new char* [2];
        hresult->h_addr_list[0] = dns->result;
        hresult->h_addr_list[1] = NULL;
    }
    
    delete(dns->mutex);
    pico_free(dns);
    //  
    return hresult;
}

char * picotcp_gethostbyaddr(const char *ip)
{
    struct dnsQuery * dns;
    char * result;
    dns = (struct dnsQuery *)pico_zalloc(sizeof(struct dnsQuery));
    int tries = 5u;
    
    if(!dns)
        return NULL;

    dns->mutex = new PicoCondition();

    pico_dns_client_getname(ip,dispatch_dns,(void *)dns);

    while(tries>0)
    {
        if(dns->mutex->lock() == false)
            tries--;
        else
            break;
    }

    result = dns->result;
    delete(dns->mutex);
    pico_free(dns);

    return result;
}


static void picotcp_loop(void const *args)
{
    while(1) {
        pico_stack_tick();
        Thread::wait(1);
    }
}

static Thread * pico_thread;

void picotcp_init(void)
{
    //Thread pico_thread(picotcp_loop);
    pico_thread= new Thread(picotcp_loop);
    // doesn;t this call the ~Thread
}