File content as of revision 24:8bff2b51ea3b:

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

Authors: Daniele Lacamera
*********************************************************************/

#include "wrapper.h"
#include "rtos.h"
#include "cmsis_os.h"
#include "mbed.h"
#include "Socket.h"

//#define ptsock_dbg printf
#define ptsock_dbg(...)

int in_the_stack = 0;


static Thread * serverThread = NULL;
// global queue for open requests
Queue<struct picotcp_socketcall,10> picotcp_socketcall_queue;
Queue<struct stack_endpoint,10> picotcp_socketcall_response_queue;

static inline void sendOpenResponse(struct stack_endpoint * arg)
{
    picotcp_socketcall_response_queue.put(arg);
}

static inline uint16_t readSocketCommand(struct stack_endpoint * endp)
{
    osEvent event = endp->CallActivateQueue->get(0); // Tx for Client, Rx for Me, non blocking
    if(event.status == osEventMessage)
        return *((uint16_t *)event.value.p);
    return NO_COMMAND;
}

static inline void sendSocketResponse(struct stack_endpoint *endp)
{
    endp->CallResponseQueue->put(endp);
}

static int stack_compare(void *ka, void *kb)
{
  struct stack_endpoint *a = (struct stack_endpoint *)ka, *b = (struct stack_endpoint *)kb;
  if (a->s < b->s)
    return -1;
  if (a->s > b->s)
    return 1;
  return 0;
}

PICO_TREE_DECLARE(stack_ep_tree, stack_compare);

static struct stack_endpoint *GET_SOCK_BY_PICOSOCK(struct pico_socket *s)
{
  struct stack_endpoint test;
  test.s = s;  
  return (struct stack_endpoint *)pico_tree_findKey(&stack_ep_tree, &test);
}

static void stack_Add(struct stack_endpoint *ep)
{
  pico_tree_insert(&stack_ep_tree, ep);
}

static void stack_Del(struct stack_endpoint *ep)
{
  pico_tree_delete(&stack_ep_tree, ep);
}

static void status_pending_off(struct stack_endpoint *ep)
{
    //ep->pending = NO_COMMAND; //no event
}




static int send_wakeup(struct stack_endpoint *ep)
{
  int ret = 0;
  if (ep->pending == NO_COMMAND)
    return -1;
  ep->retval = 1;
  // send response
  sendSocketResponse(ep);
  return ret;
}



static int send_retval(struct stack_endpoint *ep, int retval)
{
  int ret = 0;
  if (ep->pending == NO_COMMAND)
    return -1;
  ep->retval = retval;
  // send queue
  sendSocketResponse(ep);
  status_pending_off(ep);
  return ret;
}

static int send_error(struct stack_endpoint *ep)
{
  ep->code  = pico_err;
  return send_retval(ep, -1);
}
static void set_timer(struct stack_endpoint *ep)
{
  if (ep->timeout == osWaitForever)
    return;

  if (ep->t_call == 0) {
    ep->t_call = PICO_TIME_MS();
    ep->t_expire = ep->t_call + ep->timeout;
  }
}

static void check_timer_expired(struct stack_endpoint *ep)
{
  if (ep->timeout == osWaitForever)
    return; /* never */

  if (ep->t_expire < PICO_TIME_MS()) {
    ep->retval = 0;
    sendSocketResponse(ep);
  }
}


static void retry_connect(struct stack_endpoint *ep)
{
  if ((ep->revents & PICO_SOCK_EV_CONN) && ep->connected) {
    ep->revents &= (~PICO_SOCK_EV_CONN);
    ep->revents |= PICO_SOCK_EV_WR;
    ptsock_dbg("Established. sock state: %x\n", ep->s->state);
    send_retval(ep, 0);
  }else {
    pico_socket_connect(ep->s, &ep->addr, ep->port);
    ep->events |= PICO_SOCK_EV_CONN;
  }
}

static void retry_accept(struct stack_endpoint *ep)
{
  struct pico_ip4 peer ={};
  uint16_t port = 0;
  
  struct stack_endpoint *aep;
  struct pico_socket *sa;
  set_timer(ep); 
  if (ep->revents & PICO_SOCK_EV_CONN) {
    ptsock_dbg("Incoming connection...\n");
    sa = pico_socket_accept(ep->s, &peer.addr, &port);
    if (sa) {
      ptsock_dbg("Accepted!\n");
      aep = (struct stack_endpoint *)pico_zalloc(sizeof(struct stack_endpoint));
      if (!aep) {
        send_error(ep);
        return;
      }
      
      aep->sock_fd = (uint16_t)pico_rand();
      aep->s = sa;
      aep->connected = 1;
      aep->peer = peer;
      aep->peer_port = port;
      aep->addr = ep->addr;//copy parent's address
      aep->port = ep->port;
      aep->pending = NO_COMMAND;
      aep->revents |= PICO_SOCK_EV_WR;
      ep->revents &= (~PICO_SOCK_EV_CONN);
      aep->CallResponseQueue = new Queue<struct stack_endpoint,2>();
      aep->CallActivateQueue = new Queue<struct stack_endpoint,2>();
      aep->state = SOCK_CONNECTED;
      stack_Add(aep);
      ep->retval = 0;
      ep->CallResponseQueue->put(aep);
    } else {
      send_error(ep);
    }
    return;
  }
  ep->events |= PICO_SOCK_EV_CONN;
  check_timer_expired(ep);
}

static void retry_close(struct stack_endpoint *ep)
{
  pico_socket_close(ep->s);
  send_retval(ep, 0);
  stack_Del(ep);
}

static void retry_write(struct stack_endpoint *ep)
{

  if (ep->pending == NO_COMMAND)
    return;

  if (!ep->buffer) {
    send_error(ep);
    return;
  }

  if (ep->pending == TOK_SENDTO) {    
    ep->size = pico_socket_sendto(ep->s, ep->buffer, ep->size, &ep->addr, ep->port);
  } else {
    ep->size = pico_socket_write(ep->s, ep->buffer, ep->size);
  }
  ep->revents &= (~PICO_SOCK_EV_WR);
  if (ep->size < 0)
    send_error(ep);
  else if (ep->size > 0)
    send_retval(ep, ep->size);
  else {
    ep->events |= PICO_SOCK_EV_WR;
    return;
  }
}

static void retry_read(struct stack_endpoint *ep)
{
  int len;
  if (ep->pending == NO_COMMAND)
    return;

  ptsock_dbg("in retry-read, len: %d \n", len);
  
  
  
  if (!ep->buffer) {
    send_error(ep);
    return;
  } else {
    if (ep->pending == TOK_RECVFROM)
      len = pico_socket_recvfrom(ep->s, ep->buffer, ep->size, &ep->addr, &ep->port);
    else {
      len = pico_socket_read(ep->s, ep->buffer, ep->size);
    }
  }
  
  if (ep->size > len)
    ep->revents &= (~PICO_SOCK_EV_RD);
  
  if (len == 0) {
    ptsock_dbg("Read returned 0.");
    ep->events |= PICO_SOCK_EV_RD;
    return;
  } 
  send_retval(ep, len);
}

void retry_poll(struct stack_endpoint *ep)
{
  if (ep->pending == NO_COMMAND)
    return;

  set_timer(ep);

  if ((ep->events & ep->revents) != 0) {
    send_retval(ep, 1);
    return;
  }

  check_timer_expired(ep);
}

static void retry_token(struct stack_endpoint *ep)
{
  if (ep->pending == NO_COMMAND)
    return;
  ptsock_dbg("in retry-token, cmd: %04x\n", ep->pending);
  switch(ep->pending) {
    case TOK_POLL:
      retry_poll(ep);
      break;
    case TOK_CONNECT:
      retry_connect(ep);
      break;
    case TOK_ACCEPT:
      retry_accept(ep);
      break;

    case TOK_READ:
    case TOK_RECV:
    case TOK_RECVFROM:
      retry_read(ep);
      break;

    case TOK_WRITE:
    case TOK_SEND:
    case TOK_SENDTO:
      retry_write(ep);
      break;

    case TOK_CLOSE:
      retry_close(ep);
      break;
    default:
      break;
    }
}

static void wakeup(uint16_t ev, struct pico_socket *s)
{
  int t;
  struct stack_endpoint *ep = GET_SOCK_BY_PICOSOCK(s);
  
  if (!ep) {
    ptsock_dbg("WAKEUP: no socket :(\n");
    return;
  }
  t = ep->pending;
  ptsock_dbg("Wake up event: %04x\n", ev);
  ep->revents |= ev;
  
  
  
  if (ev & PICO_SOCK_EV_CLOSE){
    printf("** CLOSE\n");
    ep->connected = 0;
    send_error(ep);
  }
  if (ev & PICO_SOCK_EV_FIN) {
      printf("** FIN\n");
  }
  
  if (ev & PICO_SOCK_EV_ERR) {
    ep->revents = PICO_SOCK_EV_ERR;
    send_error(ep);
    return;
  }
  
  if ((ev & PICO_SOCK_EV_CONN) || (ev & PICO_SOCK_EV_RD)) {
    if ((ev & PICO_SOCK_EV_CONN) && ((ep->pending == TOK_CONNECT) || (ep->pending == TOK_ACCEPT))) {
      printf("** Connected\n");
      ep->connected = 1;
    }
  }
  ep->revents = ev;  
  if ( (ep->pending != 0) && ((ep->events & ep->revents)!=0) )
    retry_token(ep);
}

static int stack_parse_requests(void)
{
  struct stack_endpoint *ep = NULL;
  struct picotcp_socketcall *skc;
  osEvent event; // used to see the status of queue get
  struct pico_tree_node * index = NULL;
  uint16_t proto,net;
  int ret = 0;
  int yes = 1;
  // first check if the global queue has something in it
  // and open a new socket if so
  do
  {
    stack_endpoint *stack_ep = NULL;
    
    event = picotcp_socketcall_queue.get(0); // non blocking get
    if(event.status == osEventMessage) {
        ret = 1;
        // Get the arguments
        skc = (struct picotcp_socketcall *)event.value.p;
    
        // open new socket and send the response
        stack_ep = (struct stack_endpoint *)pico_zalloc(sizeof(struct stack_endpoint));
        if(!stack_ep)
        {
            ptsock_dbg("Out of memory\n");
            return -1;
        }
        stack_ep->CallActivateQueue = new Queue<struct stack_endpoint,2>();
        stack_ep->CallResponseQueue = new Queue<struct stack_endpoint,2>();

        net = ((skc->family == AF_INET6)?PICO_PROTO_IPV6:PICO_PROTO_IPV4);
        proto = ((skc->proto == SOCK_DGRAM)?PICO_PROTO_UDP:PICO_PROTO_TCP);
        stack_ep->s = pico_socket_open( net, proto, &wakeup );
        
        if (!stack_ep->s)
            stack_ep->retval = -1;
        //pico_socket_setoption(stack_ep->s, PICO_SOCKET_OPT_TCPNODELAY, &yes);

        //stack_ep->pending = NO_COMMAND;  
        stack_Add(stack_ep);
        // send the response back to the client
        sendOpenResponse(stack_ep);
        ptsock_dbg("New socket added: %p\n", stack_ep->s);
    }
  }while(event.status == osEventMessage); 

  // go through all the sockets and check if there is a request from someone
  // call to the get function has timeout 0, must be non blocking
  // if a request has come from someone parse it
  pico_tree_foreach(index,&stack_ep_tree)
  {
    ep = (struct stack_endpoint *)index->keyValue;
    // check if data has arrived
    event = ep->CallActivateQueue->get(0); // again non blocking
    if(event.status == osEventMessage)
    {
        ret = 1;
        //printf("Socket contacted. CMD: %X\n", ep->pending);
        // check command sent
        switch(ep->pending)
        {
            case TOK_BIND:
              if(pico_socket_bind(ep->s, &ep->addr, &ep->port) == 0) {
                ptsock_dbg("stack: bind success (addr: %08x, port %d)!\n", ep->addr.addr, ep->port);
                send_retval(ep, 0);
              } else {
                ptsock_dbg("stack: bind error (pico_s: %p, addr: %08x, port %d err: %d)!\n", ep->s, ep->addr.addr, ep->port, pico_err);
                send_error(ep);
              }
              break;
            case TOK_LISTEN:
              if(pico_socket_listen(ep->s, ep->size) == 0)
                send_retval(ep, 0);
              else
                send_error(ep);
              break;
            default:
              retry_token(ep);  
        }
        break;
     }
  }
  return ret;
}

void pico_wrapper_loop(const void * arg)
{
  (void)arg;
  int ret = 0;
  while(1) {
    ret = stack_parse_requests();
    pico_stack_tick();
    Thread::wait(1);
  }
}

void picotcp_start(void)
{
  Ticker *picotcpTicker = new Ticker();
  if (serverThread == NULL) {
    printf (" *** PicoTCP initialized *** \n");
    serverThread = new Thread(pico_wrapper_loop);
    serverThread->set_priority(osPriorityIdle);
    //picotcpTicker->attach(&pico_stack_tick, 0.001);
  }
}