sockets.c

基于STM32F107的UDP服务器程序

      buf = sock->lastdata;
    } else {
      /* If this is non-blocking call, then check first */
      if (((flags & MSG_DONTWAIT) || (sock->flags & O_NONBLOCK)) && 
          (sock->rcvevent <= 0)) {
        if (off > 0) {
          /* already received data, return that */
          sock_set_errno(sock, 0);
          return off;
        }
        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. */
      sock->lastdata = buf = netconn_recv(sock->conn);
      LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom: netconn_recv netbuf=%p\n", (void*)buf));

      if (!buf) {
        if (off > 0) {
          /* already received data, return that */
          sock_set_errno(sock, 0);
          return off;
        }
        /* We should really do some error checking here. */
        LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom(%d): buf == NULL!\n", s));
        sock_set_errno(sock, (((sock->conn->pcb.ip != NULL) && (sock->conn->err == ERR_OK))
          ? ETIMEDOUT : err_to_errno(sock->conn->err)));
        return 0;
      }
    }

    buflen = netbuf_len(buf);
    LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom: buflen=%"U16_F" len=%"SZT_F" off=%"U16_F" sock->lastoffset=%"U16_F"\n",
      buflen, len, off, sock->lastoffset));

    buflen -= sock->lastoffset;

    if (len > buflen) {
      copylen = buflen;
    } else {
      copylen = (u16_t)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) {
      LWIP_ASSERT("invalid copylen, len would underflow", len >= copylen);
      len -= copylen;
      if ( (len <= 0) || 
           (buf->p->flags & PBUF_FLAG_PUSH) || 
           (sock->rcvevent <= 0) || 
           ((flags & MSG_PEEK)!=0)) {
        done = 1;
      }
    } else {
      done = 1;
    }

    /* Check to see from where the data was.*/
    if (done) {
      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);
        }

        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=%"U16_F" len=%"U16_F"\n", port, off));
      } else {
  #if SOCKETS_DEBUG
        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);
        }

        LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom(%d): addr=", s));
        ip_addr_debug_print(SOCKETS_DEBUG, addr);
        LWIP_DEBUGF(SOCKETS_DEBUG, (" port=%"U16_F" len=%"U16_F"\n", port, off));
  #endif /*  SOCKETS_DEBUG */
      }
    }

    /* 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 ((netconn_type(sock->conn) == NETCONN_TCP) && (buflen - copylen > 0)) {
        sock->lastdata = buf;
        sock->lastoffset += copylen;
        LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom: lastdata now netbuf=%p\n", (void*)buf));
      } else {
        sock->lastdata = NULL;
        sock->lastoffset = 0;
        LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_recvfrom: deleting netbuf=%p\n", (void*)buf));
        netbuf_delete(buf);
      }
    }
  } while (!done);

  sock_set_errno(sock, 0);
  return off;
}

int
lwip_read(int s, void *mem, size_t len)
{
  return lwip_recvfrom(s, mem, len, 0, NULL, NULL);
}

int
lwip_recv(int s, void *mem, size_t len, int flags)
{
  return lwip_recvfrom(s, mem, len, flags, NULL, NULL);
}

int
lwip_send(int s, const void *data, size_t size, int flags)
{
  struct lwip_socket *sock;
  err_t err;

  LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_send(%d, data=%p, size=%"SZT_F", flags=0x%x)\n",
                              s, data, size, flags));

  sock = get_socket(s);
  if (!sock)
    return -1;

  if (sock->conn->type != NETCONN_TCP) {
#if (LWIP_UDP || LWIP_RAW)
    return lwip_sendto(s, data, size, flags, NULL, 0);
#else
    sock_set_errno(sock, err_to_errno(ERR_ARG));
    return -1;
#endif /* (LWIP_UDP || LWIP_RAW) */
  }

  err = netconn_write(sock->conn, data, size, NETCONN_COPY | ((flags & MSG_MORE)?NETCONN_MORE:0));

  LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_send(%d) err=%d size=%"SZT_F"\n", s, err, size));
  sock_set_errno(sock, err_to_errno(err));
  return (err == ERR_OK ? (int)size : -1);
}

int
lwip_sendto(int s, const void *data, size_t size, int flags,
       const struct sockaddr *to, socklen_t tolen)
{
  struct lwip_socket *sock;
  struct ip_addr remote_addr;
  err_t err;
  u16_t short_size;
#if !LWIP_TCPIP_CORE_LOCKING
  struct netbuf buf;
  u16_t remote_port;
#endif

  sock = get_socket(s);
  if (!sock)
    return -1;

  if (sock->conn->type == NETCONN_TCP) {
#if LWIP_TCP
    return lwip_send(s, data, size, flags);
#else
    sock_set_errno(sock, err_to_errno(ERR_ARG));
    return -1;
#endif /* LWIP_TCP */
  }

  LWIP_ASSERT("lwip_sendto: size must fit in u16_t", size <= 0xffff);
  short_size = (u16_t)size;
  LWIP_ERROR("lwip_sendto: invalid address", (((to == NULL) && (tolen == 0)) ||
             ((tolen == sizeof(struct sockaddr_in)) &&
             ((((const struct sockaddr_in *)to)->sin_family) == AF_INET))),
             sock_set_errno(sock, err_to_errno(ERR_ARG)); return -1;);

#if LWIP_TCPIP_CORE_LOCKING
  /* Should only be consider like a sample or a simple way to experiment this option (no check of "to" field...) */
  { struct pbuf* p;
  
    p = pbuf_alloc(PBUF_TRANSPORT, 0, PBUF_REF);
    if (p == NULL) {
      err = ERR_MEM;
    } else {
      p->payload = (void*)data;
      p->len = p->tot_len = short_size;
      
      remote_addr.addr = ((const struct sockaddr_in *)to)->sin_addr.s_addr;
      
      LOCK_TCPIP_CORE();
      if (sock->conn->type==NETCONN_RAW) {
        err = sock->conn->err = raw_sendto(sock->conn->pcb.raw, p, &remote_addr);
      } else {
        err = sock->conn->err = udp_sendto(sock->conn->pcb.udp, p, &remote_addr, ntohs(((const struct sockaddr_in *)to)->sin_port));
      }
      UNLOCK_TCPIP_CORE();
      
      pbuf_free(p);
    }
  }
#else
  /* initialize a buffer */
  buf.p = buf.ptr = NULL;
  if (to) {
    remote_addr.addr = ((const struct sockaddr_in *)to)->sin_addr.s_addr;
    remote_port      = ntohs(((const struct sockaddr_in *)to)->sin_port);
    buf.addr         = &remote_addr;
    buf.port         = remote_port;
  } else {
    remote_addr.addr = 0;
    remote_port      = 0;
    buf.addr         = NULL;
    buf.port         = 0;
  }

  LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_sendto(%d, data=%p, short_size=%d"U16_F", flags=0x%x to=",
              s, data, short_size, flags));
  ip_addr_debug_print(SOCKETS_DEBUG, &remote_addr);
  LWIP_DEBUGF(SOCKETS_DEBUG, (" port=%"U16_F"\n", remote_port));

  /* make the buffer point to the data that should be sent */
#if LWIP_NETIF_TX_SINGLE_PBUF
  /* Allocate a new netbuf and copy the data into it. */
  if (netbuf_alloc(&buf, short_size) == NULL) {
    err = ERR_MEM;
  } else {
    err = netbuf_take(&buf, data, short_size);
  }
#else /* LWIP_NETIF_TX_SINGLE_PBUF */
  err = netbuf_ref(&buf, data, short_size);
#endif /* LWIP_NETIF_TX_SINGLE_PBUF */
  if (err == ERR_OK) {
    /* send the data */
    err = netconn_send(sock->conn, &buf);
  }

  /* deallocated the buffer */
  netbuf_free(&buf);
#endif /* LWIP_TCPIP_CORE_LOCKING */
  sock_set_errno(sock, err_to_errno(err));
  return (err == ERR_OK ? short_size : -1);
}

int
lwip_socket(int domain, int type, int protocol)
{
  struct netconn *conn;
  int i;

  LWIP_UNUSED_ARG(domain);

  /* create a netconn */
  switch (type) {
  case SOCK_RAW:
    conn = netconn_new_with_proto_and_callback(NETCONN_RAW, (u8_t)protocol, event_callback);
    LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_socket(%s, SOCK_RAW, %d) = ",
                                 domain == PF_INET ? "PF_INET" : "UNKNOWN", protocol));
    break;
  case SOCK_DGRAM:
    conn = netconn_new_with_callback( (protocol == IPPROTO_UDPLITE) ?
                 NETCONN_UDPLITE : NETCONN_UDP, event_callback);
    LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_socket(%s, SOCK_DGRAM, %d) = ",
                                 domain == PF_INET ? "PF_INET" : "UNKNOWN", protocol));
    break;
  case SOCK_STREAM:
    conn = netconn_new_with_callback(NETCONN_TCP, event_callback);
    LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_socket(%s, SOCK_STREAM, %d) = ",
                                 domain == PF_INET ? "PF_INET" : "UNKNOWN", protocol));
    break;
  default:
    LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_socket(%d, %d/UNKNOWN, %d) = -1\n",
                                 domain, type, protocol));
    set_errno(EINVAL);
    return -1;
  }

  if (!conn) {
    LWIP_DEBUGF(SOCKETS_DEBUG, ("-1 / ENOBUFS (could not create netconn)\n"));
    set_errno(ENOBUFS);
    return -1;
  }

  i = alloc_socket(conn);

  if (i == -1) {
    netconn_delete(conn);
    set_errno(ENFILE);
    return -1;
  }
  conn->socket = i;
  LWIP_DEBUGF(SOCKETS_DEBUG, ("%d\n", i));
  set_errno(0);
  return i;
}

int
lwip_write(int s, const void *data, size_t size)
{
  return lwip_send(s, data, size, 0);
}

/**
 * Go through the readset and writeset lists and see which socket of the sockets
 * set in the sets has events. On return, readset, writeset and exceptset have
 * the sockets enabled that had events.
 *
 * exceptset is not used for now!!!
 *
 * @param maxfdp1 the highest socket index in the sets
 * @param readset in: set of sockets to check for read events;
 *                out: set of sockets that had read events
 * @param writeset in: set of sockets to check for write events;
 *                 out: set of sockets that had write events
 * @param exceptset not yet implemented
 * @return number of sockets that had events (read+write)
 */
static int
lwip_selscan(int maxfdp1, fd_set *readset, fd_set *writeset, fd_set *exceptset)
{
  int i, nready = 0;
  fd_set lreadset, lwriteset, lexceptset;
  struct lwip_socket *p_sock;
  
  FD_ZERO(&lreadset);
  FD_ZERO(&lwriteset);
  FD_ZERO(&lexceptset);
  
  /* Go through each socket in each list to count number of sockets which
  currently match */
  for(i = 0; i < maxfdp1; i++) {
    if (FD_ISSET(i, readset)) {
      /* See if netconn of this socket is ready for read */
      p_sock = get_socket(i);
      if (p_sock && (p_sock->lastdata || (p_sock->rcvevent > 0))) {
        FD_SET(i, &lreadset);
        LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_selscan: fd=%d ready for reading\n", i));
        nready++;
      }
    }
    if (FD_ISSET(i, writeset)) {
      /* See if netconn of this socket is ready for write */
      p_sock = get_socket(i);
      if (p_sock && p_sock->sendevent) {
        FD_SET(i, &lwriteset);
        LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_selscan: fd=%d ready for writing\n", i));
        nready++;
      }
    }
  }
  *readset = lreadset;
  *writeset = lwriteset;
  FD_ZERO(exceptset);
  
  return nready;
}


/**
 * Processing exceptset is not yet implemented.
 */
int
lwip_select(int maxfdp1, fd_set *readset, fd_set *writeset, fd_set *exceptset,
               struct timeval *timeout)
{
  int i;
  int nready;
  fd_set lreadset, lwriteset, lexceptset;
  u32_t msectimeout;
  struct lwip_select_cb select_cb;
  struct lwip_select_cb *p_selcb;

  LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_select(%d, %p, %p, %p, tvsec=%ld tvusec=%ld)\n",
                  maxfdp1, (void *)readset, (void *) writeset, (void *) exceptset,
                  timeout ? (long)timeout->tv_sec : (long)-1,
                  timeout ? (long)timeout->tv_usec : (long)-1));

  select_cb.next = 0;
  select_cb.readset = readset;
  select_cb.writeset = writeset;
  select_cb.exceptset = exceptset;
  select_cb.sem_signalled = 0;

  /* Protect ourselves searching through the list */
  sys_sem_wait(selectsem);

  if (readset)
    lreadset = *readset;
  else
    FD_ZERO(&lreadset);
  if (writeset)
    lwriteset = *writeset;
  else
    FD_ZERO(&lwriteset);
  if (exceptset)
    lexceptset = *exceptset;
  else
    FD_ZERO(&lexceptset);

  /* Go through each socket in each list to count number of sockets which
     currently match */
  nready = lwip_selscan(maxfdp1, &lreadset, &lwriteset, &lexceptset);

  /* If we don't have any current events, then suspend if we are supposed to */
  if (!nready) {
    if (timeout && timeout->tv_sec == 0 && timeout->tv_usec == 0) {
      sys_sem_signal(selectsem);
      if (readset)
        FD_ZERO(readset);
      if (writeset)
        FD_ZERO(writeset);
      if (exceptset)
        FD_ZERO(exceptset);
  
      LWIP_DEBUGF(SOCKETS_DEBUG, ("lwip_select: no timeout, returning 0\n"));
      set_errno(0);
  
      return 0;
    }
    
    /* add our semaphore to list */
    /* We don't actually need any dynamic memory. Our entry on the
     * list is only valid while we are in this function, so it's ok
     * to use local variables */
    
    select_cb.sem = sys_sem_new(0);
    /* Note that we are still protected */
    /* Put this select_cb on top of list */
    select_cb.next = select_cb_list;
    select_cb_list = &select_cb;
    
    /* Now we can safely unprotect */
    sys_sem_signal(selectsem);
    
    /* Now just wait to be woken */
    if (timeout == 0)
      /* Wait forever */
      msectimeout = 0;
    else {
      msectimeout =  ((timeout->tv_sec * 1000) + ((timeout->tv_usec + 500)/1000));
      if(msectimeout == 0)
        msectimeout = 1;
    }
    
    i = sys_sem_wait_timeout(select_cb.sem, msectimeout);
    
    /* Take us off the list */
    sys_sem_wait(selectsem);
    if (select_cb_list == &select_cb)
      select_cb_list = select_cb.next;
    else
      for (p_selcb = select_cb_list; p_selcb; p_selcb = p_selcb->next) {
        if (p_selcb->next == &select_cb) {
          p_selcb->next = select_cb.next;
          break;
        }
      }
    
    sys_sem_signal(selectsem);
    
    sys_sem_free(select_cb.sem);
    if (i == 0)  {