summaryrefslogtreecommitdiff
path: root/nuttx
diff options
context:
space:
mode:
authorpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2008-09-02 20:38:12 +0000
committerpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2008-09-02 20:38:12 +0000
commite7d4d115566c113f2cba32f16857ea5e90c03be0 (patch)
treeacc29e9bf0f255f5ade7780723e9101f4dcf5580 /nuttx
parenta376ebf2a0c8b3b2d50eeb6be645311eb27579dd (diff)
downloadpx4-nuttx-e7d4d115566c113f2cba32f16857ea5e90c03be0.tar.gz
px4-nuttx-e7d4d115566c113f2cba32f16857ea5e90c03be0.tar.bz2
px4-nuttx-e7d4d115566c113f2cba32f16857ea5e90c03be0.zip
sendto.c
git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@868 42af7a65-404d-4744-a932-0658087f49c3
Diffstat (limited to 'nuttx')
-rw-r--r--nuttx/net/sendto.c12
-rw-r--r--nuttx/net/uip/Make.defs6
-rw-r--r--nuttx/net/uip/uip-icmpinput.c94
-rw-r--r--nuttx/net/uip/uip-icmpping.c341
-rw-r--r--nuttx/net/uip/uip-icmppoll.c106
-rw-r--r--nuttx/net/uip/uip-icmpsend.c161
-rw-r--r--nuttx/net/uip/uip-input.c2
-rw-r--r--nuttx/net/uip/uip-internal.h48
-rw-r--r--nuttx/net/uip/uip-poll.c47
-rw-r--r--nuttx/net/uip/uip-udppoll.c4
10 files changed, 753 insertions, 68 deletions
diff --git a/nuttx/net/sendto.c b/nuttx/net/sendto.c
index 519510fe6..57c37668b 100644
--- a/nuttx/net/sendto.c
+++ b/nuttx/net/sendto.c
@@ -94,7 +94,7 @@ struct sendto_s
****************************************************************************/
#ifdef CONFIG_NET_UDP
-uint16 sendto_interrupt(struct uip_driver_s *dev, void *conn, void *pvprivate, uint16 flags)
+static uint16 sendto_interrupt(struct uip_driver_s *dev, void *conn, void *pvprivate, uint16 flags)
{
struct sendto_s *pstate = (struct sendto_s *)pvprivate;
@@ -111,12 +111,16 @@ uint16 sendto_interrupt(struct uip_driver_s *dev, void *conn, void *pvprivate, u
}
/* Check if the outgoing packet is available (it may have been claimed
- * by a sendto interrupt serving a different thread.
+ * by a sendto interrupt serving a different thread -OR- if the output
+ * buffer currently contains unprocessed incoming data. In these cases
+ * we will just have to wait for the next polling cycle.
*/
- else if (dev->d_sndlen > 0)
+ else if (dev->d_sndlen > 0 || (flags & UIP_NEWDATA) != 0)
{
- /* Another thread has beat us sending data, wait for the next poll */
+ /* Another thread has beat us sending data or the buffer is busy,
+ * wait for the next polling cycle
+ */
return flags;
}
diff --git a/nuttx/net/uip/Make.defs b/nuttx/net/uip/Make.defs
index 71fcdf67c..0a1031684 100644
--- a/nuttx/net/uip/Make.defs
+++ b/nuttx/net/uip/Make.defs
@@ -72,6 +72,12 @@ ifeq ($(CONFIG_NET_ICMP),y)
UIP_CSRCS += uip-icmpinput.c
+ifeq ($(CONFIG_NET_ICMP_PING),y)
+ifneq ($(CONFIG_DISABLE_CLOCK),y)
+UIP_CSRCS += uip-icmpping.c uip-icmppoll.c uip-icmpsend.c
+endif
+endif
+
endif
endif
diff --git a/nuttx/net/uip/uip-icmpinput.c b/nuttx/net/uip/uip-icmpinput.c
index 747301f2b..b6cad0118 100644
--- a/nuttx/net/uip/uip-icmpinput.c
+++ b/nuttx/net/uip/uip-icmpinput.c
@@ -2,7 +2,7 @@
* net/uip/uip-icmpinput.c
* Handling incoming ICMP/ICMP6 input
*
- * Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* Adapted for NuttX from logic in uIP which also has a BSD-like license:
@@ -55,6 +55,8 @@
#include "uip-internal.h"
+#ifdef CONFIG_NET_ICMP
+
/****************************************************************************
* Definitions
****************************************************************************/
@@ -69,6 +71,10 @@
* Private Variables
****************************************************************************/
+#ifdef CONFIG_NET_ICMP_PING
+struct uip_callback_s *g_echocallback = NULL;
+#endif
+
/****************************************************************************
* Private Functions
****************************************************************************/
@@ -109,47 +115,64 @@ void uip_icmpinput(struct uip_driver_s *dev)
* we return the packet.
*/
- if (ICMPBUF->type != ICMP_ECHO)
+ if (ICMPBUF->type == ICMP_ECHO_REQUEST)
{
- ndbg("Unknown ICMP cmd: %d\n", ICMPBUF->type);
- goto typeerr;
+ /* If we are configured to use ping IP address assignment, we use
+ * the destination IP address of this ping packet and assign it to
+ * ourself.
+ */
+
+#ifdef CONFIG_NET_PINGADDRCONF
+ if (dev->d_ipaddr == 0)
+ {
+ dev->d_ipaddr = ICMPBUF->destipaddr;
+ }
+#endif
+
+ ICMPBUF->type = ICMP_ECHO_REPLY;
+
+ if (ICMPBUF->icmpchksum >= HTONS(0xffff - (ICMP_ECHO_REPLY << 8)))
+ {
+ ICMPBUF->icmpchksum += HTONS(ICMP_ECHO_REPLY << 8) + 1;
+ }
+ else
+ {
+ ICMPBUF->icmpchksum += HTONS(ICMP_ECHO_REPLY << 8);
+ }
+
+ /* Swap IP addresses. */
+
+ uiphdr_ipaddr_copy(ICMPBUF->destipaddr, ICMPBUF->srcipaddr);
+ uiphdr_ipaddr_copy(ICMPBUF->srcipaddr, &dev->d_ipaddr);
+
+ nvdbg("Outgoing ICMP packet length: %d (%d)\n",
+ dev->d_len, (ICMPBUF->len[0] << 8) | ICMPBUF->len[1]);
+
+#ifdef CONFIG_NET_STATISTICS
+ uip_stat.icmp.sent++;
+ uip_stat.ip.sent++;
+#endif
}
- /* If we are configured to use ping IP address assignment, we use
- * the destination IP address of this ping packet and assign it to
- * ourself.
+ /* If an ICMP echo reply is received then there should also be
+ * a thread waiting to received the echo response.
*/
-#ifdef CONFIG_NET_PINGADDRCONF
- if (dev->d_ipaddr == 0)
+#ifdef CONFIG_NET_ICMP_PING
+ else if (ICMPBUF->type == ICMP_ECHO_REPLY && g_echocallback)
{
- dev->d_ipaddr = ICMPBUF->destipaddr;
+ (void)uip_callbackexecute(dev, ICMPBUF, UIP_ECHOREPLY, g_echocallback);
}
#endif
- ICMPBUF->type = ICMP_ECHO_REPLY;
+ /* Otherwise the ICMP input was not processed */
- if (ICMPBUF->icmpchksum >= HTONS(0xffff - (ICMP_ECHO << 8)))
- {
- ICMPBUF->icmpchksum += HTONS(ICMP_ECHO << 8) + 1;
- }
else
{
- ICMPBUF->icmpchksum += HTONS(ICMP_ECHO << 8);
+ ndbg("Unknown ICMP cmd: %d\n", ICMPBUF->type);
+ goto typeerr;
}
- /* Swap IP addresses. */
-
- uiphdr_ipaddr_copy(ICMPBUF->destipaddr, ICMPBUF->srcipaddr);
- uiphdr_ipaddr_copy(ICMPBUF->srcipaddr, &dev->d_ipaddr);
-
- nvdbg("Outgoing ICMP packet length: %d (%d)\n",
- dev->d_len, (ICMPBUF->len[0] << 8) | ICMPBUF->len[1]);
-
-#ifdef CONFIG_NET_STATISTICS
- uip_stat.icmp.sent++;
- uip_stat.ip.sent++;
-#endif
return;
typeerr:
@@ -198,7 +221,7 @@ typeerr:
goto drop;
}
}
- else if (ICMPBUF->type == ICMP6_ECHO)
+ else if (ICMPBUF->type == ICMP6_ECHO_REQUEST)
{
/* ICMP echo (i.e., ping) processing. This is simple, we only
* change the ICMP type from ECHO to ECHO_REPLY and update the
@@ -212,6 +235,18 @@ typeerr:
ICMPBUF->icmpchksum = 0;
ICMPBUF->icmpchksum = ~uip_icmp6chksum(dev);
}
+
+ /* If an ICMP echo reply is received then there should also be
+ * a thread waiting to received the echo response.
+ */
+
+#ifdef CONFIG_NET_ICMP_PING
+ else if (ICMPBUF->type == ICMP6_ECHO_REPLY && g_echocallback)
+ {
+ (void)uip_callbackexecute(dev, ICMPBUF, UIP_ECHOREPLY, g_echocallback);
+ }
+#endif
+
else
{
ndbg("Unknown ICMP6 cmd: %d\n", ICMPBUF->type);
@@ -241,4 +276,5 @@ drop:
#endif /* !CONFIG_NET_IPv6 */
}
+#endif /* CONFIG_NET_ICMP */
#endif /* CONFIG_NET */
diff --git a/nuttx/net/uip/uip-icmpping.c b/nuttx/net/uip/uip-icmpping.c
new file mode 100644
index 000000000..e62209dcd
--- /dev/null
+++ b/nuttx/net/uip/uip-icmpping.c
@@ -0,0 +1,341 @@
+/****************************************************************************
+ * net/uip/uip-icmpping.c
+ *
+ * Copyright (C) 2008 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#if defined(CONFIG_NET) && defined(CONFIG_NET_ICMP) && \
+ defined(CONFIG_NET_ICMP_PING) && !defined(CONFIG_DISABLE_CLOCK)
+
+#include <sys/types.h>
+#include <semaphore.h>
+#include <debug.h>
+
+#include <net/if.h>
+#include <nuttx/clock.h>
+#include <net/uip/uipopt.h>
+#include <net/uip/uip.h>
+#include <net/uip/uip-arch.h>
+
+#include "uip-internal.h"
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+#define ICMPBUF ((struct uip_icmpip_hdr *)&dev->d_buf[UIP_LLH_LEN])
+
+/* Allocate a new ICMP data callback */
+
+#define uip_icmpcallbackalloc() uip_callbackalloc(&g_echocallback)
+#define uip_icmpcallbackfree(cb) uip_callbackfree(cb, &g_echocallback)
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+struct icmp_ping_s
+{
+ FAR struct uip_callback_s *png_cb; /* Reference to callback instance */
+
+ sem_t png_sem; /* Use to manage the wait for the response */
+ uint32 png_time; /* Start time for determining timeouts */
+ uint32 png_ticks; /* System clock ticks to wait */
+ int png_result; /* 0: success; <0:negated errno on fail */
+ uip_ipaddr_t png_addr; /* The peer to be ping'ed */
+ uint16 png_id; /* Used to match requests with replies */
+ uint16 png_seqno; /* IN: seqno to send; OUT: seqno recieved */
+ boolean png_sent; /* TRUE... the PING request has been sent */
+};
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Function: ping_timeout
+ *
+ * Description:
+ * Check for send timeout.
+ *
+ * Parameters:
+ * pstate - Ping state structure
+ *
+ * Returned Value:
+ * TRUE:timeout FALSE:no timeout
+ *
+ * Assumptions:
+ * Running at the interrupt level
+ *
+ ****************************************************************************/
+
+static inline int ping_timeout(struct icmp_ping_s *pstate)
+{
+ uint32 elapsed = g_system_timer - pstate->png_time;
+ if (elapsed >= pstate->png_ticks)
+ {
+ return TRUE;
+ }
+ return FALSE;
+}
+
+/****************************************************************************
+ * Function: ping_interrupt
+ *
+ * Description:
+ * This function is called from the interrupt level to perform the actual
+ * ECHO request and/or ECHO reply actions when polled by the uIP layer.
+ *
+ * Parameters:
+ * dev The structure of the network driver that caused the interrupt
+ * conn The received packet, cast to void *
+ * pvprivate An instance of struct icmp_ping_s cast to void*
+ * flags Set of events describing why the callback was invoked
+ *
+ * Returned Value:
+ * Modified value of the input flags
+ *
+ * Assumptions:
+ * Running at the interrupt level
+ *
+ ****************************************************************************/
+
+static uint16 ping_interrupt(struct uip_driver_s *dev, void *conn,
+ void *pvprivate, uint16 flags)
+{
+ struct icmp_ping_s *pstate = (struct icmp_ping_s *)pvprivate;
+ int failcode = -ETIMEDOUT;
+
+ nvdbg("flags: %04x\n", flags);
+ if (pstate)
+ {
+ /* Check if this device is on the same network as the destination device. */
+
+ if (!uip_ipaddr_maskcmp(pstate->png_addr, dev->d_ipaddr, dev->d_netmask))
+ {
+ /* Destination address was not on the local network served by this
+ * device. If a timeout occurs, then the most likely reason is
+ * that the destination address is not reachable.
+ */
+
+ failcode = -ENETUNREACH;
+ }
+ else
+ {
+ /* Check if this is a ICMP ECHO reply. If so, return the sequence
+ * number to the the caller. NOTE: We may not even have sent the
+ * requested ECHO request; this could have been the delayed ECHO
+ * response from a previous ping.
+ */
+
+ if ((flags & UIP_ECHOREPLY) != 0 && conn != NULL)
+ {
+ struct uip_icmpip_hdr *icmp = (struct uip_icmpip_hdr *)conn;
+ if (ntohs(icmp->id) == pstate->png_id)
+ {
+ pstate->png_result = OK;
+ pstate->png_seqno = ntohs(icmp->seqno);
+ goto end_wait;
+ }
+ }
+
+ /* Check:
+ * If the outgoing packet is available (it may have been claimed
+ * by a sendto interrupt serving a different thread
+ * -OR-
+ * If the output buffer currently contains unprocessed incoming
+ * data.
+ * -OR-
+ * If we have alread sent the ECHO request
+ *
+ * In the first two cases, we will just have to wait for the next
+ * polling cycle.
+ */
+
+ if (dev->d_sndlen <= 0 && /* Packet available */
+ (flags & UIP_NEWDATA) == 0 && /* No incoming data */
+ !pstate->png_sent) /* Request not sent */
+ {
+ /* We can send the ECHO request now.
+ *
+ * Format the ICMP ECHO request packet
+ */
+
+ ICMPBUF->type = ICMP_ECHO_REQUEST;
+ ICMPBUF->icode = 0;
+#ifndef CONFIG_NET_IPv6
+ ICMPBUF->id = htons(pstate->png_id);
+ ICMPBUF->seqno = htons(pstate->png_seqno);
+#else
+# error "IPv6 ECHO Request not implemented"
+#endif
+
+ /* Send the ICMP echo request. Note that d_sndlen is set to
+ * the size of the ICMP payload and does not include the size
+ * of the ICMP header.
+ */
+
+ dev->d_sndlen= 4;
+ uip_icmpsend(dev, &pstate->png_addr);
+ pstate->png_sent = TRUE;
+ return flags;
+ }
+ }
+
+ /* Check if the selected timeout has elapsed */
+
+ if (ping_timeout(pstate))
+ {
+ /* Yes.. report the timeout */
+
+ nvdbg("Ping timeout\n");
+ pstate->png_result = failcode;
+ goto end_wait;
+ }
+
+ /* Continue waiting */
+ }
+ return flags;
+
+end_wait:
+ /* Do not allow any further callbacks */
+
+ pstate->png_cb->flags = 0;
+ pstate->png_cb->private = NULL;
+ pstate->png_cb->event = NULL;
+
+ /* Wake up the waiting thread */
+
+ sem_post(&pstate->png_sem);
+ return flags;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: uip_ping
+ *
+ * Description:
+ * Send a ECHO request and wait for the ECHO response
+ *
+ * Parameters:
+ * addr - The IP address of the peer to send the ICMP ECHO request to
+ * in network order.
+ * id - The ID to use in the ICMP ECHO request. This number should be
+ * unique; only ECHO responses with this matching ID will be
+ * processed (host order)
+ * seqno - The sequence number used in the ICMP ECHO request. NOT used
+ * to match responses (host order)
+ * dsecs - Wait up to this many deci-seconds for the ECHO response to be
+ * returned (host order).
+ *
+ * Return:
+ * seqno of received ICMP ECHO with matching ID (may be different
+ * from the seqno argument (may be a delayed response from an earlier
+ * ping with the same ID). Or a negated errno on any failure.
+ *
+ * Assumptions:
+ * Called from the user level with interrupts enabled.
+ *
+ ****************************************************************************/
+
+int uip_ping(uip_ipaddr_t addr, uint16 id, uint16 seqno, int dsecs)
+{
+ struct icmp_ping_s state;
+ irqstate_t save;
+
+ /* Initialize the state structure */
+
+ sem_init(&state.png_sem, 0, 0);
+ state.png_ticks = DSEC2TICK(dsecs); /* System ticks to wait */
+ state.png_result = -ENOMEM; /* Assume allocation failure */
+ state.png_addr = addr; /* Address of the peer to be ping'ed */
+ state.png_id = id; /* The ID to use in the ECHO request */
+ state.png_seqno = seqno; /* The seqno to use int the ECHO request */
+ state.png_sent = FALSE; /* ECHO request not yet sent */
+
+ save = irqsave();
+ state.png_time = g_system_timer;
+
+ /* Set up the callback */
+
+ state.png_cb = uip_icmpcallbackalloc();
+ if (state.png_cb)
+ {
+ state.png_cb->flags = UIP_POLL|UIP_ECHOREPLY;
+ state.png_cb->private = (void*)&state;
+ state.png_cb->event = ping_interrupt;
+
+ /* Wait for either the full round trip transfer to complete or
+ * for timeout to occur. (1) sem_wait will also terminate if a
+ * signal is received, (2) interrupts are disabled! They will
+ * be re-enabled while the task sleeps and automatically
+ * re-enabled when the task restarts.
+ */
+
+ state.png_result = -EINTR; /* Assume sem-waited interrupt by signal */
+ sem_wait(&state.png_sem);
+
+ uip_icmpcallbackfree(state.png_cb);
+ }
+ irqrestore(save);
+
+ /* Return the negated error number in the event of a failure, or the
+ * sequence number of the ECHO reply on success.
+ */
+
+ if (!state.png_result)
+ {
+ return (int)state.png_seqno;
+ }
+ else
+ {
+ return state.png_result;
+ }
+}
+
+#endif /* CONFIG_NET_ICMP && CONFIG_NET_ICMP_PING ... */
diff --git a/nuttx/net/uip/uip-icmppoll.c b/nuttx/net/uip/uip-icmppoll.c
new file mode 100644
index 000000000..662409eb1
--- /dev/null
+++ b/nuttx/net/uip/uip-icmppoll.c
@@ -0,0 +1,106 @@
+/****************************************************************************
+ * net/uip/uip-icmppoll.c
+ *
+ * Copyright (C) 2008 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#if defined(CONFIG_NET) && defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING)
+
+#include <sys/types.h>
+#include <debug.h>
+
+#include <net/uip/uipopt.h>
+#include <net/uip/uip.h>
+#include <net/uip/uip-arch.h>
+
+#include "uip-internal.h"
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+#define ICMPBUF ((struct uip_icmpip_hdr *)&dev->d_buf[UIP_LLH_LEN])
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: uip_icmppoll
+ *
+ * Description:
+ * Poll a UDP "connection" structure for availability of TX data
+ *
+ * Parameters:
+ * dev - The device driver structure to use in the send operation
+ *
+ * Return:
+ * None
+ *
+ * Assumptions:
+ * Called from the interrupt level or with interrupts disabled.
+ *
+ ****************************************************************************/
+
+void uip_icmppoll(struct uip_driver_s *dev)
+{
+ /* Setup for the application callback */
+
+ dev->d_appdata = &dev->d_buf[UIP_LLH_LEN + UIP_IPICMPH_LEN];
+ dev->d_snddata = &dev->d_buf[UIP_LLH_LEN + UIP_IPICMPH_LEN];
+
+ dev->d_len = 0;
+ dev->d_sndlen = 0;
+
+ /* Perform the application callback */
+
+ (void)uip_callbackexecute(dev, NULL, UIP_POLL, g_echocallback);
+}
+
+#endif /* CONFIG_NET && CONFIG_NET_ICMP && CONFIG_NET_ICMP_PING */
diff --git a/nuttx/net/uip/uip-icmpsend.c b/nuttx/net/uip/uip-icmpsend.c
new file mode 100644
index 000000000..90c2a6c6e
--- /dev/null
+++ b/nuttx/net/uip/uip-icmpsend.c
@@ -0,0 +1,161 @@
+/****************************************************************************
+ * net/uip/uip-icmsend.c
+ *
+ * Copyright (C) 2008 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#if defined(CONFIG_NET) && defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING)
+
+#include <sys/types.h>
+#include <debug.h>
+
+#include <net/uip/uipopt.h>
+#include <net/uip/uip.h>
+#include <net/uip/uip-arch.h>
+
+#include "uip-internal.h"
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+#define ICMPBUF ((struct uip_icmpip_hdr *)&dev->d_buf[UIP_LLH_LEN])
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: uip_icmpsend
+ *
+ * Description:
+ * Setup to send an ICMP packet
+ *
+ * Parameters:
+ * dev - The device driver structure to use in the send operation
+ *
+ * Return:
+ * None
+ *
+ * Assumptions:
+ * Called from the interrupt level or with interrupts disabled.
+ *
+ ****************************************************************************/
+
+void uip_icmpsend(struct uip_driver_s *dev, uip_ipaddr_t *destaddr)
+{
+ if (dev->d_sndlen > 0)
+ {
+ /* The total lenth to send is the size of the application data plus
+ * the IP and ICMP headers (and, eventually, the ethernet header)
+ */
+
+ dev->d_len = dev->d_sndlen + UIP_IPICMPH_LEN;
+
+ /* Initialize the IP header. Note that for IPv6, the IP length field
+ * does not include the IPv6 IP header length.
+ */
+
+#ifdef CONFIG_NET_IPv6
+
+ ICMPBUF->vtc = 0x60;
+ ICMPBUF->tcf = 0x00;
+ ICMPBUF->flow = 0x00;
+ ICMPBUF->len[0] = (dev->d_sndlen >> 8);
+ ICMPBUF->len[1] = (dev->d_sndlen & 0xff);
+ ICMPBUF->nexthdr = UIP_PROTO_ICMP;
+ ICMPBUF->hoplimit = UIP_TTL;
+
+ uip_ipaddr_copy(ICMPBUF->srcipaddr, &dev->d_ipaddr);
+ uip_ipaddr_copy(ICMPBUF->destipaddr, destaddr);
+
+#else /* CONFIG_NET_IPv6 */
+
+ ICMPBUF->vhl = 0x45;
+ ICMPBUF->tos = 0;
+ ICMPBUF->len[0] = (dev->d_len >> 8);
+ ICMPBUF->len[1] = (dev->d_len & 0xff);
+ ++g_ipid;
+ ICMPBUF->ipid[0] = g_ipid >> 8;
+ ICMPBUF->ipid[1] = g_ipid & 0xff;
+ ICMPBUF->ipoffset[0] = 0;
+ ICMPBUF->ipoffset[1] = 0;
+ ICMPBUF->ttl = UIP_TTL;
+ ICMPBUF->proto = UIP_PROTO_ICMP;
+
+ /* Calculate IP checksum. */
+
+ ICMPBUF->ipchksum = 0;
+ ICMPBUF->ipchksum = ~(uip_ipchksum(dev));
+
+ uiphdr_ipaddr_copy(ICMPBUF->srcipaddr, &dev->d_ipaddr);
+ uiphdr_ipaddr_copy(ICMPBUF->destipaddr, destaddr);
+
+#endif /* CONFIG_NET_IPv6 */
+
+ /* Calculate the ICMP checksum. */
+
+ ICMPBUF->icmpchksum = 0;
+ ICMPBUF->icmpchksum = ~(uip_icmpchksum(dev));
+ if (ICMPBUF->icmpchksum == 0)
+ {
+ ICMPBUF->icmpchksum = 0xffff;
+ }
+
+ nvdbg("Outgoing ICMP packet length: %d (%d)\n",
+ dev->d_len, (ICMPBUF->len[0] << 8) | ICMPBUF->len[1]);
+
+#ifdef CONFIG_NET_STATISTICS
+ uip_stat.icmp.sent++;
+ uip_stat.ip.sent++;
+#endif
+ }
+}
+
+#endif /* CONFIG_NET && CONFIG_NET_ICMP && CONFIG_NET_ICMP_PING */
diff --git a/nuttx/net/uip/uip-input.c b/nuttx/net/uip/uip-input.c
index acb18990b..52f29f20c 100644
--- a/nuttx/net/uip/uip-input.c
+++ b/nuttx/net/uip/uip-input.c
@@ -407,6 +407,7 @@ void uip_input(struct uip_driver_s *dev)
else
#endif
+#ifdef CONFIG_NET_ICMP
if (uip_ipaddr_cmp(dev->d_ipaddr, g_allzeroaddr))
{
/* If we are configured to use ping IP address configuration and
@@ -431,6 +432,7 @@ void uip_input(struct uip_driver_s *dev)
/* Check if the pack is destined for out IP address */
else
+#endif
{
/* Check if the packet is destined for our IP address. */
#ifndef CONFIG_NET_IPv6
diff --git a/nuttx/net/uip/uip-internal.h b/nuttx/net/uip/uip-internal.h
index e408a66b3..362cecc57 100644
--- a/nuttx/net/uip/uip-internal.h
+++ b/nuttx/net/uip/uip-internal.h
@@ -55,37 +55,6 @@
* Public Macro Definitions
****************************************************************************/
-/* TCP definitions */
-
-#define TCP_FIN 0x01
-#define TCP_SYN 0x02
-#define TCP_RST 0x04
-#define TCP_PSH 0x08
-#define TCP_ACK 0x10
-#define TCP_URG 0x20
-#define TCP_CTL 0x3f
-
-#define TCP_OPT_END 0 /* End of TCP options list */
-#define TCP_OPT_NOOP 1 /* "No-operation" TCP option */
-#define TCP_OPT_MSS 2 /* Maximum segment size TCP option */
-
-#define TCP_OPT_MSS_LEN 4 /* Length of TCP MSS option. */
-
-/* ICMP/ICMP6 definitions */
-
-#define ICMP_ECHO_REPLY 0
-#define ICMP_ECHO 8
-
-#define ICMP6_ECHO_REPLY 129
-#define ICMP6_ECHO 128
-#define ICMP6_NEIGHBOR_SOLICITATION 135
-#define ICMP6_NEIGHBOR_ADVERTISEMENT 136
-
-#define ICMP6_FLAG_S (1 << 6)
-
-#define ICMP6_OPTION_SOURCE_LINK_ADDRESS 1
-#define ICMP6_OPTION_TARGET_LINK_ADDRESS 2
-
/****************************************************************************
* Public Type Definitions
****************************************************************************/
@@ -107,6 +76,12 @@ extern uint16 g_ipid;
extern uint8 uip_reasstmr;
#endif
+/* List of applications waiting for ICMP ECHO REPLY */
+
+#if defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING)
+extern struct uip_callback_s *g_echocallback;
+#endif
+
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
@@ -213,6 +188,17 @@ EXTERN void uip_udpcallback(struct uip_driver_s *dev,
/* Defined in uip-icmpinput.c ***********************************************/
EXTERN void uip_icmpinput(struct uip_driver_s *dev);
+
+#ifdef CONFIG_NET_ICMP_PING
+/* Defined in uip-icmpoll.c *************************************************/
+
+EXTERN void uip_icmppoll(struct uip_driver_s *dev);
+
+/* Defined in uip-icmsend.c *************************************************/
+
+EXTERN void uip_icmpsend(struct uip_driver_s *dev, uip_ipaddr_t *destaddr);
+
+#endif /* CONFIG_NET_ICMP_PING */
#endif /* CONFIG_NET_ICMP */
#undef EXTERN
diff --git a/nuttx/net/uip/uip-poll.c b/nuttx/net/uip/uip-poll.c
index 1e8483ea6..d05e9f58f 100644
--- a/nuttx/net/uip/uip-poll.c
+++ b/nuttx/net/uip/uip-poll.c
@@ -70,6 +70,31 @@
*
****************************************************************************/
+#if defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING)
+static inline int uip_pollicmp(struct uip_driver_s *dev, uip_poll_callback_t callback)
+{
+ /* Perform the UDP TX poll */
+
+ uip_icmppoll(dev);
+
+ /* Call back into the driver */
+
+ return callback(dev);
+}
+#endif /* CONFIG_NET_UDP */
+
+/****************************************************************************
+ * Function: uip_polludpconnections
+ *
+ * Description:
+ * Poll all UDP connections for available packets to send.
+ *
+ * Assumptions:
+ * This function is called from the CAN device driver and may be called from
+ * the timer interrupt/watchdog handle level.
+ *
+ ****************************************************************************/
+
#ifdef CONFIG_NET_UDP
static int uip_polludpconnections(struct uip_driver_s *dev,
uip_poll_callback_t callback)
@@ -92,8 +117,6 @@ static int uip_polludpconnections(struct uip_driver_s *dev,
return bstop;
}
-#else
-# define uip_polludpconnections(dev,callback) (0)
#endif /* CONFIG_NET_UDP */
/****************************************************************************
@@ -210,9 +233,19 @@ int uip_poll(struct uip_driver_s *dev, uip_poll_callback_t callback)
bstop = uip_polltcpconnections(dev, callback);
if (!bstop)
{
+#ifdef CONFIG_NET_UDP
/* Traverse all of the allocated UDP connections and perform the poll action */
bstop = uip_polludpconnections(dev, callback);
+ if (!bstop)
+#endif
+ {
+#if defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING)
+ /* Traverse all of the tasks waiting to send an ICMP ECHO request */
+
+ bstop = uip_pollicmp(dev, callback);
+#endif
+ }
}
return bstop;
@@ -262,7 +295,17 @@ int uip_timer(struct uip_driver_s *dev, uip_poll_callback_t callback, int hsec)
{
/* Traverse all of the allocated UDP connections and perform the poll action */
+#ifdef CONFIG_NET_UDP
bstop = uip_polludpconnections(dev, callback);
+ if (!bstop)
+#endif
+ {
+#if defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING)
+ /* Traverse all of the tasks waiting to send an ICMP ECHO request */
+
+ bstop = uip_pollicmp(dev, callback);
+#endif
+ }
}
return bstop;
diff --git a/nuttx/net/uip/uip-udppoll.c b/nuttx/net/uip/uip-udppoll.c
index e664fcd73..649190b29 100644
--- a/nuttx/net/uip/uip-udppoll.c
+++ b/nuttx/net/uip/uip-udppoll.c
@@ -2,7 +2,7 @@
* net/uip/uip-udppoll.c
* Poll for the availability of UDP TX data
*
- * Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* Adapted for NuttX from logic in uIP which also has a BSD-like license:
@@ -119,7 +119,7 @@ void uip_udppoll(struct uip_driver_s *dev, struct uip_udp_conn *conn)
}
}
- /* Make sure that d_len is zerp meaning that there is nothing to be sent */
+ /* Make sure that d_len is zero meaning that there is nothing to be sent */
dev->d_len = 0;
}